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
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
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:
[-18.22, 2.923000000000002, 6.462, -101.846, -1.187, 20.44, -7.771, -3.666]
[[-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:
[-16.724999999999994, -1.0330000000000013, 9.626, -96.396, -4.088, 15.516, -8.651, -3.959]
[[-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!