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:

$${\mathit{J}}_j=\left[\begin{array}{ccc}-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{array}\right]$$

Actuator Jacobian (written using joint positions for simplicity):

$${\mathit{J}}_a=\left[\begin{array}{ccc}-\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{array}\right]$$

Using equation for calculating static torques from external forces:

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

We get:

$${\mathit{\tau }}_j={\mathit{J}}_j^T{{\mathit{J}}_a^T}^{-1}{\mathit{\tau }}_a$$ $${\mathit{J}}_j^T{{\mathit{J}}_a^T}^{-1}=\left[\begin{array}{ccc}{n}_{a_1}{n}_{j_1}& & n_{a_2}\\ 0& & n_{a_2}{n}_{j_2}\end{array}\right]$$

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 89.06%. Comparing base (af0d776) to head (254a4a8).

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1837      +/-   ##
==========================================
- Coverage   89.08%   89.06%   -0.03%     
==========================================
  Files         139      139              
  Lines       16118    16118              
  Branches     1389     1389              
==========================================
- Hits        14359    14355       -4     
- Misses       1228     1231       +3     
- Partials      531      532       +1     

Flag	Coverage Δ
unittests	89.06% <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	77.65% <100.00%> (-0.24%)	⬇️
...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.