Arm's goal position is not the same as the actual end position

Hello again!

We are trying out your API in a very simple script. It involves simply initialising the left arm, making the motors compliant for a few seconds, reading the arm’s current position and saving it to a variable.

We then send the same values back to the arm to have it go back to the same position which was read using your built-in goto function:
reachy.goto(goal_positions=saved_position, duration=2, wait=True).

We printed out the motor angles that was read initially (from current_position = [m.present_position for m in reachy.left_arm.motors]), as well as the 4x4 matrix which is the outcome of the forward kinematics (using your built-in reachy.left_arm.inverse_kinematics() function).

We also printed the same things after reachy was sent to the same position. There was an obvious discrepancy in the values, especially in the vertical plane (Z).

Here is what read initially:

Motor angles:

[-18.22, 2.923000000000002, 6.462, -101.846, -1.187, 20.44, -7.771, -3.666]

Position:

[[-1.65500901e-01  2.97234621e-04 -9.86209594e-01  4.07621722e-01]
 [-5.28736804e-02  9.98559068e-01  9.17396069e-03  2.22581336e-01]
 [ 9.84791260e-01  5.36628296e-02 -1.65246709e-01 -1.22168054e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Here is what was read after we sent the same motor angles to the arm using your goto() function:

Motor angles:

[-16.724999999999994, -1.0330000000000013, 9.626, -96.396, -4.088, 15.516, -8.651, -3.959]

Position:

[[-0.12268847 -0.03371322 -0.99187245  0.41378292]
 [-0.02341032  0.99924308 -0.03106804  0.22521195]
 [ 0.99216909  0.01940836 -0.12338485 -0.15855226]

We tried this multiple times in different 3D positions and found that there is always this error which is mainly in the vertical Z axis. The difference between the read position and the position the arm goes to alone is very noticeable, and frequently goes up to 6cm.

Are you aware of this issue, or do you have any obvious ideas on how to fix it?

Thanks a lot!

Ziad Abass

It’s not really an issue of Reachy but it’s part of the roboticist life, dealing with real world physics.

When you take reachy, your hand is supporting its weight. When you turn compliant false and remove your hand, the gravity applies on the arm and therefore each joint moves slightly until the internal PID compensates.

It’s normal the robot doesn’t behave exactly as demonstrated because conditions have changed.

It’s possible to change the PID for something more stiff but you will get a more jerky robot as well and it will not avoid a slight change in position anyway.

What we do is compensating the gravity by changing the target, either for each joint goal position or the inverse kinematics z position. You should do it as well to generate your key points. Keep in mind the offset between present position and goal position is the real world physics.

We are exploring a ML algorithm so reachy can learn how to compensate by himself the gravity but it’s not close to a release yet.