Fix fourbarlinkage

[BartlomiejK2]

Summary:

Fixed effort mapping for FourBarLinkageTransmission, related to #1746.

New equations for effort mapping meet the principles of conservation of energy.

Actuator to joint:

$$\tau_{j_1} = n_{j_1} n_{a_1} \tau_{a_1} + n_{a_2} \tau_{a_2}$$ $$\tau_{j_2} = n_{j_2} n_{a_2} \tau_{a_2}$$

Joint to actuator:

$$\tau_{a_1} = \frac{\tau_{j_1} - \tau_{j_2}/n_{j_2}} {n_{j_1} n_{a_1}}$$ $$\tau_{a_2} = \frac{\tau_{j_2}} {n_{j_2} n_{a_2}}$$

[bmagyar]

Could you provide a few further references for documentation please? Wikipedia works too

[BartlomiejK2]

Unfortunately, I can not find any documentation other than original FourBarLinkageTransmission doxygen documentation, in which this class is the same as one in ros2-control transmission.

To calculate torques, I derived velocity equations for Remote second joint actuation model, and then used energy conservtion law to calculate torques. I could not derive them by itself. If needed, I can post derivation here.

Kinematic equations in original class are correct, only torques needs to be changed.

[github-actions]

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.

[BartlomiejK2]

Hi @bmagyar and @christophfroehlich , to this day I have not found any official derivation for FourBarLinkageTransmission, only one paper that talks about special case when all reductions are equal 1 (here's link).
Below is my derivation for torques using original FourBarLinkageTransmission documentation, where equations for position and velocity are derived correctly:

$$\dot{\theta}_{j_1}= \frac{\dot{\theta}_{a_1}}{n_{a_1}n_{j_1}}$$ $$\dot{\theta}_{j_2} = \frac{\frac{\dot{\theta}_{a_2}}{n_{a_1}} + \frac{\dot{\theta}_{a_1}}{n_{a_1}n_{j_1}}}{n_{j_2}}$$

Using two link system (from here) I derive jacobians for joints and actuators:

Joint Jacobian:

$$\boldsymbol{J}_j = \begin{bmatrix} -l_1 \sin{\theta_1} - l_2 \sin(\theta_1 + \theta_2) && -l_2 \sin(\theta_1 + \theta_2) \\\ l_1 \cos(\theta_1) + l_2 \cos(\theta_1 + \theta_2) && l_2 \cos(\theta_1 + \theta_2) \end{bmatrix}$$

Actuator Jacobian (written using joint positions for simplicity):

$$\boldsymbol{J}_a = \begin{bmatrix} -\frac{l_1 \sin{\theta_1} }{n_{a_1}n_{j_1}}- \frac{l_2 \sin(\theta_1 + \theta_2) (n_{j_2} - 1)}{n_{a_1}n_{j_1}n_{j_2}} && \frac{l_2 \sin{\theta_2} }{n_{a_2}n_{j_2}} \\\ \frac{l_1 \cos{\theta_1} }{n_{a_1}n_{j_1}} + \frac{l_2 \cos(\theta_1 + \theta_2) (n_{j_2} - 1)}{n_{a_1}n_{j_1}n_{j_2}} && \frac{l_2 \cos{\theta_2} }{n_{a_2}n_{j_2}} \end{bmatrix}$$

Using equation for calculating static torques from external forces:

$$\boldsymbol{\tau}_j = - \boldsymbol{J}_j^T \boldsymbol{F}_{ext}$$ $$\boldsymbol{\tau}_a = - \boldsymbol{J}_a^T \boldsymbol{F}_{ext}$$

We get:

$$\boldsymbol{\tau}_j = \boldsymbol{J}_j^T {\boldsymbol{J}_a^T}^{-1} \boldsymbol{\tau}_a$$ $$\boldsymbol{J}_j^T {\boldsymbol{J}_a^T}^{-1} = \begin{bmatrix} n_{a_1}n_{j_1} && n_{a_2} \\\ 0 && n_{a_2}n_{j_2} \end{bmatrix}$$

Which is exactly what I've derived in the first comment of this pull request.

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 88.93%. Comparing base (122b9ed) to head (9a1e7be).
Report is 1 commits behind head on master.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1837      +/-   ##
==========================================
- Coverage   88.95%   88.93%   -0.03%     
==========================================
  Files         143      143              
  Lines       16485    16484       -1     
  Branches     1425     1425              
==========================================
- Hits        14665    14660       -5     
- Misses       1270     1272       +2     
- Partials      550      552       +2     

Flag	Coverage Δ
unittests	88.93% <100.00%> (-0.03%)	⬇️

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

Files with missing lines	Coverage Δ
...ission_interface/four_bar_linkage_transmission.hpp	78.78% <100.00%> (-0.22%)	⬇️
...erface/test/four_bar_linkage_transmission_test.cpp	99.39% <ø> (ø)

... and 1 file with indirect coverage changes

🚀 New features to boost your workflow:

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

[bmagyar]

Let's roll this out, thank you for the details on the derivation!