epfl-lasa/control-libraries

Not using actual rotation vector for ImpedanceController

liuyangdh opened this issue · 0 comments

https://github.com/epfl-lasa/control-libraries/blob/main/source/controllers/src/impedance/Impedance.cpp

The CartesianState-Impedance controller method compute_command uses the method BaseQuaternon.vec() to calulcate the angular-error. However, this is just the vector part of the a quaternion Q but has does not represent a the rotation angle, see:
https://eigen.tuxfamily.org/dox/classEigen_1_1QuaternionBase.html#a91f93bde88f52796cfcd92c3594f39e5

The actual rotation angle can be calculated as:
rotvec = Q.vec() / sin(cos(Q.w)) * (2 * cos(Q.w))

Hence, it yields a different result as for example the scipy library, when using "Rotation.as_rotvec()"
https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.html