I am trying to implement a cartesian impedance controller in simulation (gazebo + KDL). The method which I am using is the following:
- Compute the EE pose from KDL's forward kinematics for the position
- Compute the EE twist from KDL's forward kinematics for the velocity
- Compute the Jacobian (which I am not sure how to figure out if it's the correct one - see later the comment about the rotation error) from the base frame to the EE frame
- Compute the difference betweent the EE's pose and the desired pose
- Multiply the difference with the Cartesian stiffness matrix
- Muttiply the velocity of the EE with the Cartesian damping matrix
- Sum the stiffness, damping
- Multiply the sum with the transpose of the Jacobian to go from Cartesian to joint space
- Add gravity compensation terms (computed by KDL)
- Apply the computed torque on the joints
Note, for my use case (contactless, stable target position) the impedance controller reduces to PD + gravity compensation control.
The whole method seems to work, but I am running into the following issues:
- The target position takes a lot of time to reach and in the meantime there are some weird oscillations (see video here https://drive.google.com/file/d/1tC20Jf24BbdLCqRbOItIvdZIkqAq1Gfn/view?usp=sharing)
- I am very unsure of how to find the difference between the rotational part of the EE pose and target pose. For now I am using quaternions but I am not sure that the way that I am computing the difference is correct. To compute the rotation error I multiply the inverse of the EE pose quaternion with the target pose quaternion. Does that sound right? How can I make sure that the Jacobian uses the same representation for the rotational part? In general, any advice on how to properly compute the error between two poses?
Thanks a lot for the help!