From b6bdc7b456f1b3b043e47ac1456c4e2a4ad052b1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 2 Nov 2024 17:16:13 +0100 Subject: [PATCH 1/3] Fixed effort mapping for FourBarLinkageTransmission --- .../four_bar_linkage_transmission.hpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 107133128c..7fb6642f2c 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -45,8 +45,8 @@ namespace transmission_interface * * * \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} * \f} * * @@ -67,8 +67,8 @@ namespace transmission_interface * * * \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} & = & \tau_{j_2} / ({n_{j_2} n_{a_2}) * \f} * * @@ -287,9 +287,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]); } } @@ -329,8 +328,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])); } } From b39be8c4b71776a8dfc05ef010da71fed3de6a3b Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 2 Nov 2024 17:36:27 +0100 Subject: [PATCH 2/3] Fixed effort mapping equations in comment --- .../four_bar_linkage_transmission.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 7fb6642f2c..14c66b9c44 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -45,8 +45,8 @@ namespace transmission_interface * * * \f{eqnarray*}{ - * \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} + * \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} * * @@ -68,7 +68,7 @@ namespace transmission_interface * * \f{eqnarray*}{ * \tau_{a_1} & = & \frac{\tau_{j_1} - \tau_{j_2}/n_{j_2}} {n_{j_1} n_{a_1}} \\ - * \tau_{a_2} & = & \tau_{j_2} / ({n_{j_2} n_{a_2}) + * \tau_{a_2} & = & \frac{\tau_{j_2}} {n_{j_2} n_{a_2}} * \f} * * From 3e7180a00f6a10edc67e56986013d32fc7d5cfd5 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 18 Apr 2025 18:22:18 +0200 Subject: [PATCH 3/3] Fixed tests for FourBarLinkageTransmission --- .../test/four_bar_linkage_transmission_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/transmission_interface/test/four_bar_linkage_transmission_test.cpp b/transmission_interface/test/four_bar_linkage_transmission_test.cpp index 64a91a7780..a70b61010b 100644 --- a/transmission_interface/test/four_bar_linkage_transmission_test.cpp +++ b/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