[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.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[christophfroehlich]

@MarcoMagriDev Could you address the last request please?

[codecov[bot]]

Codecov Report

:white_check_mark: All modified and coverable lines are covered by tests. :white_check_mark: Project coverage is 84.90%. Comparing base (ce81c9e) to head (f535ba8). :warning: Report is 2 commits behind head on master.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1139      +/-   ##
==========================================
- Coverage   84.91%   84.90%   -0.01%     
==========================================
  Files         148      148              
  Lines       14367    14359       -8     
  Branches     1230     1230              
==========================================
- Hits        12200    12192       -8     
  Misses       1740     1740              
  Partials      427      427              

Flag	Coverage Δ
unittests	84.90% <100.00%> (-0.01%)	:arrow_down:

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...ude/admittance_controller/admittance_rule_impl.hpp	93.26% <100.00%> (-0.25%)	:arrow_down:

:rocket: New features to boost your workflow:

• :snowflake: Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[MarcoMagriDev]

@christophfroehlich I just pushed a new commit that should address the request from @pac48. Not entirely sure if it aligns perfectly with what was expected — happy to discuss and adjust it further if needed!

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[github-actions[bot]]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[Juliaj]

@MarcoMagriDev, please add tests covering this change or update existing tests. This helps with review and future maintenance.