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, to this day I have not found any 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.