Fix fourbarlinkage (#1837)

Author: BartlomiejK2

transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp

@@ -47,8 +47,8 @@ namespace transmission_interface
  * </td>
  * <td>
  * \f{eqnarray*}{
- * \tau_{j_1} & = & n_{j_1} n_{a_1} \tau_{a_1} \\
- * \tau_{j_2} & = & n_{j_2} (n_{a_2} \tau_{a_2} - n_{j_1} n_{a_1} \tau_{a_1})
+ * \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}2}
  * \f}
  * </td>
  * <td>
@@ -69,8 +69,8 @@ namespace transmission_interface
  * </td>
  * <td>
  * \f{eqnarray*}{
- * \tau_{a_1} & = & \tau_{j_1} / (n_{j_1} n_{a_1}) \\
- * \tau_{a_2} & = & \frac{ \tau_{j_1} + \tau_{j_2} / n_{j_2} }{ n_{a_2} }
+ * \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}}
  * \f}
  * </td>
  * <td>
@@ -295,9 +295,8 @@ inline void FourBarLinkageTransmission::actuator_to_joint()
   {
     assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);
 
-    joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0]);
-    joint_eff[1].set_value(
-      jr[1] * (act_eff[1].get_value() * ar[1] - jr[0] * act_eff[0].get_value() * ar[0]));
+    joint_eff[0].set_value(jr[0] * act_eff[0].get_value() * ar[0] + act_eff[1].get_value() * ar[1]);
+    joint_eff[1].set_value(jr[1] * act_eff[1].get_value() * ar[1]);
   }
 }
 
@@ -337,8 +336,8 @@ inline void FourBarLinkageTransmission::joint_to_actuator()
   {
     assert(act_eff[0] && act_eff[1] && joint_eff[0] && joint_eff[1]);
 
-    act_eff[0].set_value(joint_eff[0].get_value() / (ar[0] * jr[0]));
-    act_eff[1].set_value((joint_eff[0].get_value() + joint_eff[1].get_value() / jr[1]) / ar[1]);
+    act_eff[0].set_value((joint_eff[0].get_value() - joint_eff[1].get_value() / jr[1]) / (jr[0] * ar[0]));
+    act_eff[1].set_value(joint_eff[1].get_value() / (ar[1] * jr[1]));
   }
 }
 

transmission_interface/test/four_bar_linkage_transmission_test.cpp

@@ -297,8 +297,8 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly)
     auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]);
     trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
     trans.actuator_to_joint();
-    EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS));
-    EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS));
+    EXPECT_THAT(200.0, DoubleNear(j_val[0], EPS));
+    EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS));
   }
 
   // Velocity interface
@@ -349,7 +349,7 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly)
     auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]);
     trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
     trans.actuator_to_joint();
-    EXPECT_THAT(0.0, DoubleNear(j_val[0], EPS));
+    EXPECT_THAT(100.0, DoubleNear(j_val[0], EPS));
     EXPECT_THAT(200.0, DoubleNear(j_val[1], EPS));
   }
 
@@ -402,8 +402,8 @@ TEST_F(WhiteBoxTest, MoveBothJoints)
     auto joint2_handle = JointHandle("joint2", HW_IF_EFFORT, j_vec[1]);
     trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle});
     trans.actuator_to_joint();
-    EXPECT_THAT(-60.0, DoubleNear(j_val[0], EPS));
-    EXPECT_THAT(-160.0, DoubleNear(j_val[1], EPS));
+    EXPECT_THAT(-160.0, DoubleNear(j_val[0], EPS));
+    EXPECT_THAT(-400.0, DoubleNear(j_val[1], EPS));
   }
 
   // Velocity interface