Not using actual rotation vector for ImpedanceController
liuyangdh opened this issue · 0 comments
liuyangdh commented
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