[Admittance] applies control frame transform to mass matrix

[MarcoMagriDev]

In admittance control mass matrix is not transformed to the control frame. This will reflect in two main issues:

• Inconsistency between stiffness/damping_ratio (set from the user in control_frame) and mass (set from the user in base frame) may create confusion
• the damping value is computed from the damping_ratio under assumption that mass and stiffness are in the same reference frame but this at the end is not the case (mass in base and stiffness in control). This (I think) may cause an under/over-damped behavior if the masses of the two involved axis (x and z in the example) has very different values.

How to reproduce

For simplicity, suppose to be in a configuration with the control_frame having the z axis coincident with the x axis of robot base like the following:

The admittance controller is configured with the following admittance parameters:

admittance:
  mass:
    - 1.0 # x
    - 1.0 # y
    - 1.0 # z
    - 0.05 # rx
    - 0.05 # ry
    - 0.05 # rz
  
  stiffness:
    - 50.0 # x
    - 50.0 # y
    - 50.0 # z
    - 1.0 # rx
    - 1.0 # ry
    - 1.0 # rz
  
  damping_ratio: 
    - # Not relevant

Now, faking force readings we are able to make the robot move due to compliance:

1. constant value of force readings as [0.0, 0.0, 5.0, 0.0, 0.0, 0.0] -> the robot move along the z axis
2. constant value of force readings as [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] -> the robot returns to its original position

Changing the value of the parameter admittance.mass to [1.0, 1.0, 100.0, 0.05, 0.05, 0.05] and repeating the previous experiment should end up in a shorter motion performed by the TCP but this is not the case. Instead, by setting admittance.mass to [100.0, 1.0, 1.0, 0.05, 0.05, 0.05] the motion amplitude is reduced.

The effect of the missing transformation can be seen clearly in this case due to the selected configuration and since we are exasperating asymmetry in desired masses.

Proposed solution

Apply the same operations done for stiffness and damping also to the mass_inv

---

Note: what about defining a new method or lambda function to avoid to repeat the same code for each matrix? Let me know so that I can open a new PR for that

[christophfroehlich]

Maybe @pac48 can comment on this?

[MarcoMagriDev]

Any update on this?

[pac48]

@MarcoMagriDev Can the math be simplified? For example

R*M_inv*R^T*(F + R*D*R^T*Xd + R*K*R^T*X)

We can distribute R^T and get the following:

R*M_inv(R^T*F + D*R^T*Xd + K*R^T*X)

[MarcoMagriDev]

@pac48 yes i think we can.

Here is the complete reasoning behind:

• A generic admittance control law can be expressed as $\ddot{X} = M^{-1}(F - D \dot{X} - K X)$
• Since we would like to define matrices $M$, $D$, $K$ in end effector frame all the other quantities should be in expressed in the same frame obtaining: $\ddot{X_{e}} = M^{-1}(F_{e} - D \dot{X_e} - K X_e)$ (where the subscript e indicates that the vector is expressed in end effector frame)
• By exploiting the relation $X_{e} = R^T X_{b}$ and $F_{e} = R^T F_{b}$ (where the subscript b indicates that the vector is expressed in base frame) we get: $ \ddot{X_b} = R M^{-1}(R^T F_b - D R^T \dot{X_b} - K R^T X_b)$ that is exactly your "math simplified equation"

Should I update the PR with this modification?

[pac48]

@MarcoMagriDev Yes, I think it will reduce unnecessary computation and make the math more clear. Currently, each vector is rotated into the control frame, multiplied by the matrix, then rotated back to the base frame. It makes sense to do the math in the control frame and only rotate back to the base frame as the last step.