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.