Feet in the half-sitting position are not flat on the ground

[gabrielebndn]

The half-sitting position, defined in https://github.com/stack-of-tasks/sot-talos/blob/master/src/dynamic_graph/sot/pyrene/robot.py, is currently given as

halfSitting = (0.0, 0.0,  1.018213,  0.00  ,  0.0, 0.0,                         #Free flyer
                   0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708, #Left Leg
                   0.0,  0.0, -0.411354,  0.859395, -0.448041, -0.001708, #Right Leg
                   0.0 ,  0.006761,                                                 #Chest
                   0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1, -0.005,        #Left Arm
                  -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,      #Right Arm
                   0.,  0.                                                          #Head
)

With this configuration, the feet are not flat on the ground, but slightly tilted around the x-axis. These are the results I get (I add the CoM for reference):

leg_left_6_joint: 13
  R =
        1         0         0
        0  0.999999  0.001708
        0 -0.001708  0.999999
  p = -0.00884695       0.085    0.105941

leg_right_6_joint: 27
  R =
        1         0         0
        0  0.999999  0.001708
        0 -0.001708  0.999999
  p = -0.00884695      -0.085    0.105941

com: [-0.003163736637462875, 0.0012373842912037295, 0.875624058590194]

This does not seem a good idea.

The last joint of each leg corresponds to the roll angle of the respective ankle. It is enough to set these two joints to zero instead of their current value of -0.001708, and the problem is solved. In other words, by correcting the half-sitting configuration to

halfSitting = (0.0, 0.0,  1.018213,  0.00  ,  0.0, 0.0,                         #Free flyer
                   0.0,  0.0, -0.411354,  0.859395, -0.448041, 0.0, #Left Leg
                   0.0,  0.0, -0.411354,  0.859395, -0.448041, 0.0, #Right Leg
                   0.0 ,  0.006761,                                                 #Chest
                   0.25847 ,  0.173046, -0.0002, -0.525366, 0.0, -0.0,  0.1, -0.005,        #Left Arm
                  -0.25847 , -0.173046, 0.0002  , -0.525366, 0.0,  0.0,  0.1,-0.005,      #Right Arm
                   0.,  0.                                                          #Head
)

the results are

leg_left_6_joint: 13
  R =
1 0 0
0 1 0
0 0 1
  p = -0.00884695       0.085    0.105941

leg_right_6_joint: 27
  R =
1 0 0
0 1 0
0 0 1
  p = -0.00884695      -0.085    0.105941

com: [-0.0031637366374628797, 0.0012410807883628755, 0.8756240439687248]

The error is tiny and invisible to the naked eye (about one hundredth of a degree) but it was screwing up the expected results in some unit tests I was making, where I required really strict correspondence between the algorithmic results and the analytically-computed ones (specifically, the transformation of the vertical force to the local ankle frame was causing a tiny force along the y-axis to appear, a bit less than 1 N, which is irrelevant from the control viewpoint but was still much more than I expected).

I do not know how these values were chosen, but I really think the feet in the default position should be flat on the ground. That is what one would expect.

Should we change the half-sitting configuration to reflect this need?

[jcarpent]

@gabrielebndn This is certainly due to the numerical resolution of an inverse kinematics problem I guess.

[gabrielebndn]

@jcarpent I doubt it. That's what I thought at first, but now I am not convinced. Look at the matrix:

1         0         0
0  0.999999  0.001708
0 -0.001708  0.999999

and the joint angle is 0.001708... We can really see sin(x) =~ x there... 1e-3 is not so small as an error, and the kinematic chain from the base to the foot is only 6 joints! And when the angle is zero, they are perfectly flat! It seems this angle was introduced kind of on purpose, and I really think there is a problem with the posture itself. Also it is weird that both feet have the roll in the same direction, if the robot posture is supposed to be symmetrical with respect to the sagittal plane I would expect the two angles to be opposite to each other...