[balancing] The orientations of the feet are extracted as quaternion but they are used as axis-angle .

[traversaro]

In the balancing controller, the orientation of the feet are obtained from the interface as quaternions (with real-imaginary serialization) [1] but they are used as axis-angle [2] [3].

At the beginning I was expecting at least the serialization to be consistent (the real part of the quaternion is the cosine of the rotation angle divided by two, while the imaginary is a 3d vector obtained multiplying the axis by the sine of the rotation angle diveded by two) but apparently (if I am not wrong) this is not the case. I guess then that things works because those rotation are really close to the identity most of the time (they are just used to be sure that there is no integration drift) and hence for a 0 angle rotation the axis can be arbitrary, and you still get more and less the identity. Nevertheless this must be corrected, preferably by consolidating the function for the conversion between the different representations of SO(3) (see https://github.com/robotology/mex-wholebodymodel/issues/42) .

[1] : https://github.com/robotology/mex-wholebodymodel/blob/00e222b6ef681095fc7c29c94c6e493ddd24bc5b/matlab-src/mex-wholebodymodel_balancing/forwardDynamics.m#L114 [2] : https://github.com/robotology/mex-wholebodymodel/blob/00e222b6ef681095fc7c29c94c6e493ddd24bc5b/matlab-src/mex-wholebodymodel_balancing/controllerFCN.m#L36 [3] : https://github.com/robotology/mex-wholebodymodel/commit/b9d71b9c0eb9e84b2919f6e26c5e941cd3f87c8e#commitcomment-13939900

[traversaro]

cc @gabrielenava @DanielePucci

[DanielePucci]

Yes @traversaro, you are right. Things work because you are locally close to the identity. @gabrielenava, con you correct this bug ASAP?

[gabrielenava]

The Rf function comes directly from simulink code and, as you said, is used only for computing the constraints for QP. I noticed that the rotation matrix was calculated in a different way but I just added the formulation normally used in balancing code as a comment in the function. I can uncomment it and test if everything is ok, than eliminate the wrong formula

[traversaro]

It would be great. Please take your time to properly correct also the documentation.

[gabrielenava]

I uncommented the correct formula and tested it and it works without problem, so I'm going to switch on master, do the changes and push. For documentation you mean only in Rf.m , were is said that the formulation is axis-angle but now is actually real-imaginary, right?

[traversaro]

Yes. Please feel free to open a pull request to get used to the classical github workflow : https://guides.github.com/introduction/flow/ .

[traversaro]

I guess we can close the issue now, right?

[DanielePucci]

yes