Slow/jittery servo performance

Support
,
1

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!

2

As an update, i timed the functions and the entire IK call including the body and all six legs typically takes less than 3ms. All the calls to write to the servos is less than 1ms. So i don’t think the code is too slow. Maybe the math isn’t fine/accurate enough? I doubt that… It does seem to run smoother if i set the sleep time to .01. Still not as good as this math has shown before on this hardware, though.

Any thoughts would be much appreciated. I’ll try to get a video tomorrow.

3

I collected the goal_position and the present_position of one of the 18 servos by simply printing the value to console, and then copy and pasting into Excel. I am running my calculation loop approximately every 20ms (50Hz). Here is a plot:

image.png1241×732 70.9 KB

As you can see, the commanded goal_positions are smooth so I think there is no error in my math. You can also see that the present_position has many flat areas where the values are the same. It seems that a present_position “sticks” for several of my loops attempting to command it. This makes it seem like the servos are being updated slower than i am sending commands.

I then tried to run my loop very fast in order to try and measure the actual update frequency of the present_position. For example, if i poll for present_positions every .0005 and often saw the present_positions hold the same value four times, that would indicate that the present_positions are updating every .005*4 = .02 or 50Hz. Which would be good. When i tried this, i do infact most often get repeats around 3-5 times. However often enough (all the flat spots in the plot) there are repeats of over ten, sometimes 14, 15, etc. For some reason at that moment in time, the present_position isn’t being updated.

Any ideas? Thanks a lot.

4

Hi all,

I just tried using the low level dxl API and it seems to be quite a bit smoother than using the robot controller. Any ideas? Right now i’m using one set_goal_position command per servo. How would i execute a sync_write using the low level API? Thanks again!

import numpy as np
import math
import time
import itertools
from bodyIK import bodyIK

import pypot.dynamixel

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)

m1 = 1
m2 = 3
m3 = 5
m4 = 13
m5 = 15
m6 = 17
m7 = 7
m8 = 9
m9 = 11
m10 = 8
m11 = 10
m12 = 12
m13 = 14
m14 = 16
m15 = 18
m16 = 2
m17 = 4
m18 = 6

dxl_io = pypot.dynamixel.DxlIO("/dev/ttyACM0")
print('Connected!')

found_ids = dxl_io.scan(range(20))
print('Found ids:', found_ids)

ids = found_ids[:2]
dxl_io.enable_torque(ids)

amp = 10.0
freq = 0.5


t0 = time.time()
while True:
	t = time.time() - t0
	
	if t > 10:
		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)
	
	m1_goalpos = -angles.item((0,0)) + 45.00
	m2_goalpos = -angles.item((1,0))-13.58
	m3_goalpos = -angles.item((2,0))-35.12
	m4_goalpos = -angles.item((0,1))
	m5_goalpos = -angles.item((1,1))-13.58
	m6_goalpos = -angles.item((2,1))-35.12
	m7_goalpos = -angles.item((0,2))-45.00
	m8_goalpos = -angles.item((1,2))-13.58
	m9_goalpos = -angles.item((2,2))-35.12
	m10_goalpos = angles.item((0,3))+45.00
	m11_goalpos = angles.item((1,3))+13.58
	m12_goalpos = angles.item((2,3))+35.12
	m13_goalpos = angles.item((0,4)) 
	m14_goalpos = angles.item((1,4))+13.58
	m15_goalpos = angles.item((2,4))+35.12
	m16_goalpos = angles.item((0,5))-45.00
	m17_goalpos = angles.item((1,5))+13.58
	m18_goalpos = angles.item((2,5))+35.12

	dxl_io.set_goal_position({m1: m1_goalpos})
	dxl_io.set_goal_position({m2: m2_goalpos})
	dxl_io.set_goal_position({m3: m3_goalpos})
	dxl_io.set_goal_position({m4: m4_goalpos})
	dxl_io.set_goal_position({m5: m5_goalpos})
	dxl_io.set_goal_position({m6: m6_goalpos})
	dxl_io.set_goal_position({m7: m7_goalpos})
	dxl_io.set_goal_position({m8: m8_goalpos})
	dxl_io.set_goal_position({m9: m9_goalpos})
	dxl_io.set_goal_position({m10: m10_goalpos})
	dxl_io.set_goal_position({m11: m11_goalpos})
	dxl_io.set_goal_position({m12: m12_goalpos})
	dxl_io.set_goal_position({m13: m13_goalpos})
	dxl_io.set_goal_position({m14: m14_goalpos})
	dxl_io.set_goal_position({m15: m15_goalpos})
    dxl_io.set_goal_position({m16: m16_goalpos})
	dxl_io.set_goal_position({m17: m17_goalpos})
	dxl_io.set_goal_position({m18: m18_goalpos})
	
	time.sleep(0.005)# - worktime) #roughly 50Hz loop freq

#end of while loop


dxl_io.close()

I tried sending lists as the parameter but it doesn’t seem to work:

	positions = [m1_goalpos,m2_goalpos,m3_goalpos,m4_goalpos,m5_goalpos,m6_goalpos,m7_goalpos,m8_goalpos,m9_goalpos,m10_goalpos,m11_goalpos,m12_goalpos,m13_goalpos,m14_goalpos,m15_goalpos,m16_goalpos,m17_goalpos,m18_goalpos]
	dxl_io.set_goal_position(ids,positions)

Thanks again

5

Hi TXBDan,

Were you able to overcome this problem with jittery movement? I have exactly the same issue using low-level API.

I believe you already found the solution to sync_write matter, but just in case:

targets = dict()
targets[servo1_id] = m1_goalpos
targets[servo2_id] = m2_goalpos

dxl_io.set_goal_position(targets)

6

Interesting. I have not found any solution, but i also haven’t been able to figure out syncwrite. Thanks for providing that info! Did you notice any performance improvement using syncwrite vs not?

I’ll try it out this weekend and let you know how it goes.

Thanks

7

It seems that I managed to overcome the problem and get smooth movements.

I am using ROS and my dynamixel node gets target joint angles at about 100Hz rate. Each time the new target angles are received, the procedure is called to update the goal positions of the motors.

Initially I just went through all the motors and set their goal_positions individually. That caused jerky moves.

Yesterday I changed that to a single instruction: robot.goto_position({motor1_id:position, motor2_id:position, …}, duration=0.3)

With durations less than about 0.2 movements become jittery, but with 0.2-0.3 everything gets very smooth. It appears that the problem is related to the motor speeds, but I do not understand why these specific duration values work in my particular case.

8

I implemented the low level syncwrite like you show above. Duh on me for not realizing how it worked. I’m new to Python and dictionaries didn’t come to mind.

It doesn’t seem to help much, so i guess i wasn’t taxing the bus too much with individual write commands. It does feel nicer though. My jitter isn’t too bad so i’ll prob just go with it for now.

The goto_position() looks like it interpolates positions over the given time period. I might try it, but i’d rather do the trajectory myself I think.

Thanks!

1 Like
9

You can simply do something like that, but sync_write is automatically set to True.

dxl_io.set_goal_position({"m1": m1_goalpos, "m2": m2_goalpos}, sync_write=True)
10

Can you post if you find a solution for the jittery motion using the robot controller? I have a similar problem and can not use the low level IO because the feedback loops take to long.

Thanks!

11

I may have found a solution for Raspberry Pi 3+. When a USB UART controller is used, it is slow down by Linux Kernel with NAK holdoff.

adding dwc_otg.nak_holdoff=1 to /boot/cmdline.txt)

I am going to test this this afternoon.

1 Like
12

I did the modification and there is still a jittery :frowning: I really do not understand why. It is like sometimes there is another message on the bus.
Why no issue with the odroid U3 less powerful ?

@Nicolas, @Xevel any solution or experiment protocol to debug ?

13

Hi,

Not sure about a solution, but one of the first things to do is always to sniff on the Dynamixel bus directly to know if the problem is on the servo side or on the host side.
Do you have a logic analyser to record what is going on on the bus (timing is the most interesting thing here)?

14

I did find one. I will test and compare this tuesday :slight_smile:

1 Like
15

We did the test yesterday the conclusion is that the differents frames are the good frames, each motor receive the frames but there are intervals where nothing is sent. The issue is then at software level, not at bus level.

  • when I command a motor sending instruction without pypot loop at 50Hz, it works perfectly
  • when I do the same with pypot loop, there is jittery

We approach :smile: !!