diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 5c37d7f934..903280545f 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -17,3 +17,6 @@ hardware_interface ros2controlcli ************** +transmission_interface +********************** +* The ``simple_transmission`` and ``differential_transmission`` now also support the ``force`` interface (`#2588 `_). diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 1947c2a99e..c36ea9019e 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -168,12 +168,14 @@ class DifferentialTransmission : public Transmission std::vector joint_velocity_; std::vector joint_effort_; std::vector joint_torque_; + std::vector joint_force_; std::vector joint_absolute_position_; std::vector actuator_position_; std::vector actuator_velocity_; std::vector actuator_effort_; std::vector actuator_torque_; + std::vector actuator_force_; std::vector actuator_absolute_position_; }; @@ -236,12 +238,13 @@ void DifferentialTransmission::configure( get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_VELOCITY); joint_effort_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_EFFORT); joint_torque_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_TORQUE); + joint_force_ = get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_FORCE); joint_absolute_position_ = get_ordered_handles(joint_handles, joint_names, HW_IF_ABSOLUTE_POSITION); if ( joint_position_.size() != 2 && joint_velocity_.size() != 2 && joint_effort_.size() != 2 && - joint_torque_.size() != 2 && joint_absolute_position_.size() != 2) + joint_torque_.size() != 2 && joint_force_.size() != 2 && joint_absolute_position_.size() != 2) { throw Exception("Not enough valid or required joint handles were presented."); } @@ -254,12 +257,14 @@ void DifferentialTransmission::configure( get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_EFFORT); actuator_torque_ = get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_TORQUE); + actuator_force_ = + get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_FORCE); actuator_absolute_position_ = get_ordered_handles(actuator_handles, actuator_names, HW_IF_ABSOLUTE_POSITION); if ( actuator_position_.size() != 2 && actuator_velocity_.size() != 2 && - actuator_effort_.size() != 2 && actuator_torque_.size() != 2 && + actuator_effort_.size() != 2 && actuator_torque_.size() != 2 && actuator_force_.size() != 2 && actuator_absolute_position_.size() != 2) { throw Exception( @@ -273,6 +278,7 @@ void DifferentialTransmission::configure( joint_velocity_.size() != actuator_velocity_.size() && joint_effort_.size() != actuator_effort_.size() && joint_torque_.size() != actuator_torque_.size() && + joint_force_.size() != actuator_force_.size() && joint_absolute_position_.size() != actuator_absolute_position_.size()) { throw Exception( @@ -335,6 +341,18 @@ inline void DifferentialTransmission::actuator_to_joint() jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[1].get_value() * ar[1])); } + auto & act_for = actuator_force_; + auto & joint_for = joint_force_; + if (act_for.size() == num_actuators() && joint_for.size() == num_joints()) + { + assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]); + + joint_for[0].set_value( + jr[0] * (act_for[0].get_value() * ar[0] + act_for[1].get_value() * ar[1])); + joint_for[1].set_value( + jr[1] * (act_for[0].get_value() * ar[0] - act_for[1].get_value() * ar[1])); + } + auto & act_abs_pos = actuator_absolute_position_; auto & joint_abs_pos = joint_absolute_position_; if (act_abs_pos.size() == num_actuators() && joint_abs_pos.size() == num_joints()) @@ -404,6 +422,18 @@ inline void DifferentialTransmission::joint_to_actuator() act_tor[1].set_value( (joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1])); } + + auto & act_for = actuator_force_; + auto & joint_for = joint_force_; + if (act_for.size() == num_actuators() && joint_for.size() == num_joints()) + { + assert(act_for[0] && act_for[1] && joint_for[0] && joint_for[1]); + + act_for[0].set_value( + (joint_for[0].get_value() / jr[0] + joint_for[1].get_value() / jr[1]) / (2.0 * ar[0])); + act_for[1].set_value( + (joint_for[0].get_value() / jr[0] - joint_for[1].get_value() / jr[1]) / (2.0 * ar[1])); + } } std::string DifferentialTransmission::get_handles_info() const @@ -415,11 +445,13 @@ std::string DifferentialTransmission::get_handles_info() const "Joint velocity: {}, Actuator velocity: {}\n" "Joint effort: {}, Actuator effort: {}\n" "Joint torque: {}, Actuator torque: {}\n" + "Joint force: {}, Actuator force: {}\n" "Joint absolute position: {}, Actuator absolute position: {}"), to_string(get_names(joint_position_)), to_string(get_names(actuator_position_)), to_string(get_names(joint_velocity_)), to_string(get_names(actuator_velocity_)), to_string(get_names(joint_effort_)), to_string(get_names(actuator_effort_)), to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)), + to_string(get_names(joint_force_)), to_string(get_names(actuator_force_)), to_string(get_names(joint_absolute_position_)), to_string(get_names(actuator_absolute_position_))); } diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 3ba87e348b..1d53557b91 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -135,12 +135,14 @@ class SimpleTransmission : public Transmission JointHandle joint_velocity_ = {"", "", nullptr}; JointHandle joint_effort_ = {"", "", nullptr}; JointHandle joint_torque_ = {"", "", nullptr}; + JointHandle joint_force_ = {"", "", nullptr}; JointHandle joint_absolute_position_ = {"", "", nullptr}; ActuatorHandle actuator_position_ = {"", "", nullptr}; ActuatorHandle actuator_velocity_ = {"", "", nullptr}; ActuatorHandle actuator_effort_ = {"", "", nullptr}; ActuatorHandle actuator_torque_ = {"", "", nullptr}; + ActuatorHandle actuator_force_ = {"", "", nullptr}; ActuatorHandle actuator_absolute_position_ = {"", "", nullptr}; }; @@ -206,10 +208,11 @@ inline void SimpleTransmission::configure( joint_velocity_ = get_by_interface(joint_handles, hardware_interface::HW_IF_VELOCITY); joint_effort_ = get_by_interface(joint_handles, hardware_interface::HW_IF_EFFORT); joint_torque_ = get_by_interface(joint_handles, hardware_interface::HW_IF_TORQUE); + joint_force_ = get_by_interface(joint_handles, hardware_interface::HW_IF_FORCE); joint_absolute_position_ = get_by_interface(joint_handles, HW_IF_ABSOLUTE_POSITION); if ( - !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ && + !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ && !joint_force_ && !joint_absolute_position_) { throw Exception("None of the provided joint handles are valid or from the required interfaces"); @@ -219,13 +222,15 @@ inline void SimpleTransmission::configure( actuator_velocity_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_VELOCITY); actuator_effort_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_EFFORT); actuator_torque_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_TORQUE); + actuator_force_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_FORCE); actuator_absolute_position_ = get_by_interface(actuator_handles, HW_IF_ABSOLUTE_POSITION); if ( !actuator_position_ && !actuator_velocity_ && !actuator_effort_ && !actuator_torque_ && - !actuator_absolute_position_) + !actuator_force_ && !actuator_absolute_position_) { - throw Exception("None of the provided joint handles are valid or from the required interfaces"); + throw Exception( + "None of the provided actuator handles are valid or from the required interfaces"); } } @@ -251,6 +256,11 @@ inline void SimpleTransmission::actuator_to_joint() joint_torque_.set_value(actuator_torque_.get_value() * reduction_); } + if (joint_force_ && actuator_force_) + { + joint_force_.set_value(actuator_force_.get_value() * reduction_); + } + if (joint_absolute_position_ && actuator_absolute_position_) { joint_absolute_position_.set_value( @@ -279,6 +289,11 @@ inline void SimpleTransmission::joint_to_actuator() { actuator_torque_.set_value(joint_torque_.get_value() / reduction_); } + + if (joint_force_ && actuator_force_) + { + actuator_force_.set_value(joint_force_.get_value() / reduction_); + } } } // namespace transmission_interface diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 792779c8c0..96a6b8e002 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -21,6 +21,7 @@ #include "transmission_interface/differential_transmission.hpp" using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_FORCE; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_TORQUE; using hardware_interface::HW_IF_VELOCITY; @@ -128,6 +129,7 @@ TEST(ConfigureTest, FailsWithBadHandles) testConfigureWithBadHandles(HW_IF_VELOCITY); testConfigureWithBadHandles(HW_IF_EFFORT); testConfigureWithBadHandles(HW_IF_TORQUE); + testConfigureWithBadHandles(HW_IF_FORCE); testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION); } @@ -225,6 +227,7 @@ TEST_F(BlackBoxTest, IdentityMap) testIdentityMap(transmission, input_value, HW_IF_VELOCITY); testIdentityMap(transmission, input_value, HW_IF_EFFORT); testIdentityMap(transmission, input_value, HW_IF_TORQUE); + testIdentityMap(transmission, input_value, HW_IF_FORCE); } } } @@ -293,6 +296,18 @@ TEST_F(WhiteBoxTest, DontMoveJoints) EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); } + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, 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(0.0, DoubleNear(j_val[1], EPS)); + } + // Absolute position interface { auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); @@ -365,6 +380,18 @@ TEST_F(WhiteBoxTest, MoveFirstJointOnly) EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); } + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(400.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(0.0, DoubleNear(j_val[1], EPS)); + } + // Absolute position interface { auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); @@ -437,6 +464,18 @@ TEST_F(WhiteBoxTest, MoveSecondJointOnly) EXPECT_THAT(400.0, DoubleNear(j_val[1], EPS)); } + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, 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(400.0, DoubleNear(j_val[1], EPS)); + } + // Absolute position interface { auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); @@ -514,6 +553,18 @@ TEST_F(WhiteBoxTest, MoveBothJoints) EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); } + // Force interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_FORCE, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_FORCE, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_FORCE, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_FORCE, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(140.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(520.0, DoubleNear(j_val[1], EPS)); + } + // Absolute position interface { auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); diff --git a/transmission_interface/test/simple_transmission_test.cpp b/transmission_interface/test/simple_transmission_test.cpp index 63922badc6..cd348e6497 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -19,6 +19,7 @@ #include "transmission_interface/simple_transmission.hpp" using hardware_interface::HW_IF_EFFORT; +using hardware_interface::HW_IF_FORCE; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_TORQUE; using hardware_interface::HW_IF_VELOCITY; @@ -164,6 +165,12 @@ TEST_P(BlackBoxTest, IdentityMap) reset_values(); testIdentityMap(trans, HW_IF_TORQUE, -1.0); + testIdentityMap(trans, HW_IF_FORCE, 1.0); + reset_values(); + testIdentityMap(trans, HW_IF_FORCE, 0.0); + reset_values(); + testIdentityMap(trans, HW_IF_FORCE, -1.0); + testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, 1.0); reset_values(); testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, 0.0); @@ -232,6 +239,16 @@ TEST_F(WhiteBoxTest, MoveJoint) EXPECT_THAT(10.0, DoubleNear(j_val, EPS)); } + // Force interface + { + auto actuator_handle = ActuatorHandle("act1", HW_IF_FORCE, &a_val); + auto joint_handle = JointHandle("joint1", HW_IF_FORCE, &j_val); + trans.configure({joint_handle}, {actuator_handle}); + + trans.actuator_to_joint(); + EXPECT_THAT(10.0, DoubleNear(j_val, EPS)); + } + // Absolute position interface { auto actuator_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, &a_val);