Pose difference estimation in cartesian impedance control

Question

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

This whole method is summarized here:

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!

Answer 1

1. Weird motion

I'm pretty sure it happen because you make wrong implementation on orientation control.

2. Orientation of end-effector

There are 4 main representation of end-effector orientation which is axis angle, rpy (expanded into 6 types), euler angle (expanded into 6 types) and unit quarternion. Normal jacobian derived freshly from forward kinematic matrix is for axis angle representation, a basic 3x3 matrix representation of orientation located on top left of forward kinematic matrix.

$$FK(\theta)=\begin{bmatrix}R_{3\times3} & D_{3\times1}\\ 0_{1\times3} & 1\end{bmatrix}$$

For axis angle representation error computation, with

Robot orientation (each letter represent column) : $$R_e = \begin{bmatrix}n_e & s_e & a_e\end{bmatrix} $$

Desired orientation : $$R_d = \begin{bmatrix}n_d & s_d & a_d\end{bmatrix} $$

Error would be :

$$e = \frac{1}{2}(n_e \times n_d +s_e\times s_d+o_e\times o_d)$$

If you wanna use other representation such as quarternion the error computation would be different and you would need analytical jacobian Ja.

$$J_a = T\times J$$

where T is transformation matrix. For more detail you can look at "Robotics motion, planning and control" by B. Siciliano sect 3.7.3 for orientation error and sect 3.6 for analytical jacobian. Or code implementation by Peter Corke on his github here

3. Quarternion error

Your method error computation already correct,

$$e=Q_d * Q_e^{-1}$$