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

Users Support
1

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

2

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.

1 Like
3

@matthieu thanks for your reply.

I have two question if you don’t mind:

  1. Consider this scenario please:
    I hold the arm, move it to a specific position and read the angles -> angles_A.
    I then send a command to the arm that takes it to the same position in 3D space, then I read the motor angles -> angles_B.
    Are you saying that angles_A will not equal angles_B because I was supporting the arm’s weight when reading angles_A?
  1. Considering what you’re saying about compensating the gravity by changing the target, either for each joint goal position or the inverse kinematics z position.
    We did think of that and attempted to implement, but the offset it usually not fixed for different positions in 3D space. Do you have a method to ‘predict’ what the offset will be at different positions which we can use to change the target before sending the command to reachy?

Thanks a lot for your help :smiley:

4

Yes indeed

We don’t right now but it’s something we want to provide in the future.

1 Like