Hello,

I’m working on porting my old hexapod project to python and found your pypot platform. It looks great! I’m using a Raspberry Pi 3 (Raspbian Stretch), USB2AX, and 18 Robotis AX-12A servos. The robot hardware is from the Trossen Robotics hexapod kit.

I’ve had this robot working pretty well before using C/C++ on a Raspberry Pi 2. It handled body/leg IK and foot trajectories for creating gates no problem and the robot/servo motion was very smooth. In this project i simply set the servo goal positions directly at 50Hz using the Dynamixel SDK and the syncwrite function.

I just installed PyPot on the RPi3 and hacked together some code. The code keeps the feet planted and simply tilts the body back and forth using IK algorithms for a stewart platform and then individual 3DOF leg IK. The problem is that the servo motion is very jittery. I’d like to confirm that I’m using PyPot and the built in sync loop correctly. Here is my main code. Btw, i’m very new to Python (this is part of the reason for this project) so my code is likely terrible.

```
import numpy as np
import math
import time
from contextlib import closing
from bodyIK import bodyIK
import pypot.robot
# leg1 X+, forward leg 6
# \ /
# \ ----------- /
# | |
# Y+, left | |
# leg 2 -----| |----- leg 5
# | |
# | |
# / ----------- \
# / \
# leg 3 leg 4
#
# Z+ is up, via right hand rule
l1=52 #mm, coxa
l2=66 #mm, femur
l3=132 #mm, tibia
lBodyToFRCoxa_X = 120 #distance from center of body to front/rear coxa servo along X
lBodyToFRCoxa_Y = 60 #distance from center of body to front/rear coxa servo along Y
lBodyToMidCoxa_Y = 100 #distance from center of body to side coxa servo along Y
lBodyToFRCoxa = math.sqrt(lBodyToFRCoxa_X**2 + lBodyToFRCoxa_Y**2)
# setup body vectors, center of body to coxa joints
s1 = np.array(( lBodyToFRCoxa_X, lBodyToFRCoxa_Y, 0, 1)) # leg 1
s2 = np.array(( 0, lBodyToMidCoxa_Y, 0, 1)) # leg 2
s3 = np.array((-lBodyToFRCoxa_X, lBodyToFRCoxa_Y, 0, 1)) # leg 3
s4 = np.array((-lBodyToFRCoxa_X, -lBodyToFRCoxa_Y, 0, 1)) # leg 4
s5 = np.array(( 0,-lBodyToMidCoxa_Y, 0, 1)) # leg 5
s6 = np.array(( lBodyToFRCoxa_X, -lBodyToFRCoxa_Y, 0, 1)) # leg 6
s = np.column_stack((s1,s2,s3,s4,s5,s6))
#print("body vectors, s: \n",s)
# Foot vectors
u1 = np.array(( lBodyToFRCoxa_X+50, lBodyToFRCoxa_Y+l1+l2, 0, 1)) # leg 1
u2 = np.array(( 0, lBodyToMidCoxa_Y+l1+l2, 0, 1)) # leg 2
u3 = np.array((-lBodyToFRCoxa_X-50, lBodyToFRCoxa_Y+l1+l2, 0, 1)) # leg 3
u4 = np.array((-lBodyToFRCoxa_X-50, -lBodyToFRCoxa_Y-l1-l2, 0, 1)) # leg 4
u5 = np.array(( 0,-lBodyToMidCoxa_Y-l1-l2, 0, 1)) # leg 5
u6 = np.array(( lBodyToFRCoxa_X+50, -lBodyToFRCoxa_Y-l1-l2, 0, 1)) # leg 6
u = np.column_stack((u1,u2,u3,u4,u5,u6))
#print("foot vectors, u: \n",u)
# create robot instance from config file
beth = pypot.robot.from_json('bethconfig.json')
print('Configuration loaded, Beth started')
for m in beth.motors:
m.compliant = False
#m.goal_position = 0
amp = 10
freq = 0.25
t0 = time.time()
while True:
t = time.time() - t0
if t > 20:
break
xtilt = amp*np.sin(2*np.pi*freq*t)
bodytilt = np.array([xtilt, 0, 0]) # euler x,y,z degrees
angles = bodyIK(s,u,bodytilt) #returns six column vectors of leg angles
beth.m1.goal_position = angles.item((0,0))
beth.m2.goal_position = angles.item((1,0))
beth.m3.goal_position = angles.item((2,0))
beth.m4.goal_position = angles.item((0,1))
beth.m5.goal_position = angles.item((1,1))
beth.m6.goal_position = angles.item((2,1))
beth.m7.goal_position = angles.item((0,2))
beth.m8.goal_position = angles.item((1,2))
beth.m9.goal_position = angles.item((2,2))
beth.m10.goal_position = angles.item((0,3))
beth.m11.goal_position = angles.item((1,3))
beth.m12.goal_position = angles.item((2,3))
beth.m13.goal_position = angles.item((0,4))
beth.m14.goal_position = angles.item((1,4))
beth.m15.goal_position = angles.item((2,4))
beth.m16.goal_position = angles.item((0,5))
beth.m17.goal_position = angles.item((1,5))
beth.m18.goal_position = angles.item((2,5))
time.sleep(0.02)
#end of while loop
for m in beth.motors:
m.compliant = True
beth.close()
```

This main calls on every loop (20ms) a bodyFK function which does some matrix math on the body and then calls an IK function six times, one for each of the six legs, that does some trig.

Am i correctly setting the servo angles? Am I properly timing my while loop and using the sync loop?

Is it reasonable to expect a RPi3 running Python to do this math in time?

Any advice would be much appreciated. Thanks!