search

epfl-lasa / control-libraries

A collection of library modules to facilitate the creation of full control loop algorithms, including state representation, motion planning, kinematics, dynamics and control.
https://epfl-lasa.github.io/control-libraries
GNU General Public License v3.0
27 stars 2 forks source link

Not using actual rotation vector for ImpedanceController # #332

Closed hubernikus closed 1 year ago

hubernikus commented 1 year ago

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

domire8 commented 1 year ago

You are correct that the impedance controller is using an 'orientation displacement' instead of the axis angle error. This has been described in several papers, for example here (equation 10) and it has proven to work quite well with the control libraries controllers in the past.