Following a trajectory

Technology
1

Hi! I’ve just started working with Dynamixel motors using pypot, and my aim is to get the motors to follow a trajectory. It is critical that the velocity and position trajectories are followed as closely as possible. Here’s the code I’m using:

import pypot.dynamixel.io
import pypot.dynamixel.error
import pypot.dynamixel.motor
import pypot.dynamixel.controller
import pypot.robot
import matplotlib.pyplot as plt

robot = pypot.robot.from_json('robot_config.json')

#setting motors to 0
for m in robot.motors:
    m.moving_speed=0
    m.goal_position=0
time.sleep(3)
amp=30
freq=0.5
t = numpy.arange(0, 10, 0.01)
#follow this trajectory
speeds = amp * numpy.cos(2 * numpy.pi * freq * t)
desired_positions = amp*numpy.sin(2*numpy.pi * freq * t) / (2*numpy.pi*freq)

positions = []
velocities = []
for s,p in map(None,speeds,desired_positions):
    robot.motors[0].moving_speed = s
    robot.motors[0].goal_position=p
    positions.append(robot.motors[0].present_position)
    velocities.append(robot.motors[0].present_speed)
    time.sleep(0.01)
    
plt.plot(t,velocities)
plt.show()

When I plot the velocity, here’s what I get:

Is this a sensor error? Or am I doing something fundamentally wrong?

PS: I can’t use goal_speed because I need control over speed at every instant of time. I have also tried using a while loop to move with a speed till a point is reached (bypassing time.sleep() altogether), but those results are no good either.

2

Can you plot the position ? Try also to derive the position “a posteriori” to see if the results are the same.

3

Here’s a position plot. Green is desired, blue shows sensor values.

(I haven’t added any offset in the Json configuration file)

4

OK, it is nominal. The delay you see between what you want and what you have is something you cannot reduce.
Concerning the derivative, it is computed doing the differential of the position. The consequence is that any noise on position is amplified by derivation as you can see.
For me, you cannot have better.
What motor is it? If you have one with PID control, you can tune it to have better performances.

5

It’s an MX-64. I’ll try tuning the PID. Will let you know if I get better results.
Thanks for the help.

6

Another important detail is that the moving_speed is the maximum speed the motor can reach. It’s not a direct control of the speed.

As @Thot said, to get a more precise control you will need to hand tune the pid.

7

But if I tune my PID properly, it will maintain that speed (or remain very close to it) after the settling time, correct?

8

The PID is applied on position control.
You cannot command the speed (except limiting it).

Therefore, the problem is that you want to follow a ramp input. I advise you to test your PID tuning on a triangle input instead of a sinus. The integral term will help you to follow this ramp but you should have 2 integrators to follow exactly the ramp.