PD control - trajectory tracking

Technical Questions URScript
1

Hi all,
I am doing trajectory planning from which I obtain x,x_dot vectors that show the path in which I want the end effector to follow. The trajectory x,x_dot vector is calculated every 0.002 sec (time step)
In order to make the robot follow the path I want to use a variation of PD controller, so assuming its 6dof robot;

force=P(delta_pos)+D(delta_velocity)
moment = P(delta_orientantion)+D(delta_ang_velocity )

wrench = [force, moment]

Then I want to use force_mode to feed the wrench to the robot so it can follow the path.
As you can see on the plot I get Zr which I the position of the robot to reach the Zo which is the desired goal, but it doesn’t follow the trajectory Zm.

Is it because of the different time steps that force_mode uses? Is there some other reason why the following is not good? Does the robot has its own PD controller inside?

image
P.S.
I know about servoj but I am implementing a more complex algorithm so usage of force_mode is required.

2

Does anyone have any experience with the PD controller and force_mode implementation?

3

Hi @RobotNoob,

It seems like we are missing some details here. You have a target position and an expected way to the target position. But you input the robot a wrench.
What is your expected result from an input wrench in terms of speed and acceleration?
Are you evaluating your implementation on a simulator, a CB robot or an e-Series?

The script functions force_mode_set_damping(damping) and force_mode_set_gain_scaling(scaling) can be used to tweak the behavior of force_mode.

Ebbe

1 Like
4

Yes, I have a trajectory xm.
In order to reach the desired position x0 along trajectory xm, what I do is I generate a wrench based on PD control law;

delta_pos = xr-xm
delta_velocity = xr_dot - xm_dot 

force=P(delta_pos)+D(delta_velocity)
moment = P(delta_orientantion)+D(delta_ang_velocity )

wrench = [force, moment]

While generating the xm trajectory my initial velocity and final velocity is 0, therefore the trajecotry will look like a minimum-jerk trajectory (s-shaped).

I am implementing it on UR5e robot.

5

If you are testing it on the simulator first. You will then be sure that you are not disturbed by any noise or faulty signals from the hardware.
Implementing and tweaking control loop can be complex task. If you are struggling I will suggest you to start with a pure P implementation.

From you code snippet it is hard to read the gain. If you are having a gain of 1 and a pure P contribution. That will result in a force of 1N/m for the translation error. My best guess is that it will react slowly and be doubtful if it will work on a real robot.