diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index 1947c2a99e..edad952c85 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -168,12 +168,16 @@ class DifferentialTransmission : public Transmission std::vector joint_velocity_; std::vector joint_effort_; std::vector joint_torque_; + std::vector joint_force_; + std::vector joint_curret_; 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_current_; std::vector actuator_absolute_position_; }; @@ -236,12 +240,16 @@ 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_curret_ = + get_ordered_handles(joint_handles, joint_names, hardware_interface::HW_IF_CURRENT); 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_curret_.size() != 2 && + joint_absolute_position_.size() != 2) { throw Exception("Not enough valid or required joint handles were presented."); } @@ -254,13 +262,17 @@ 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_current_ = + get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_CURRENT); 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_absolute_position_.size() != 2) + actuator_effort_.size() != 2 && actuator_torque_.size() != 2 && actuator_force_.size() != 2 && + actuator_current_.size() != 2 && actuator_absolute_position_.size() != 2) { throw Exception( fmt::format( @@ -273,6 +285,8 @@ 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_curret_.size() != actuator_current_.size() && joint_absolute_position_.size() != actuator_absolute_position_.size()) { throw Exception( @@ -335,6 +349,28 @@ 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_cur = actuator_current_; + auto & joint_cur = joint_curret_; + if (act_cur.size() == num_actuators() && joint_cur.size() == num_joints()) + { + assert(act_cur[0] && act_cur[1] && joint_cur[0] && joint_cur[1]); + + joint_cur[0].set_value(act_cur[0].get_value() + act_cur[1].get_value()); + joint_cur[1].set_value(act_cur[0].get_value() - act_cur[1].get_value()); + } + 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 +440,28 @@ 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])); + } + + auto & act_cur = actuator_current_; + auto & joint_cur = joint_curret_; + if (act_cur.size() == num_actuators() && joint_cur.size() == num_joints()) + { + assert(act_cur[0] && act_cur[1] && joint_cur[0] && joint_cur[1]); + + act_cur[0].set_value((joint_cur[0].get_value() + joint_cur[1].get_value()) / 2.0); + act_cur[1].set_value((joint_cur[0].get_value() - joint_cur[1].get_value()) / 2.0); + } } std::string DifferentialTransmission::get_handles_info() const @@ -415,11 +473,15 @@ 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 current: {}, Actuator current: {}\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_curret_)), to_string(get_names(actuator_current_)), 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..214b6c9958 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -135,12 +135,16 @@ class SimpleTransmission : public Transmission JointHandle joint_velocity_ = {"", "", nullptr}; JointHandle joint_effort_ = {"", "", nullptr}; JointHandle joint_torque_ = {"", "", nullptr}; + JointHandle joint_force_ = {"", "", nullptr}; + JointHandle joint_current_ = {"", "", 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_current_ = {"", "", nullptr}; ActuatorHandle actuator_absolute_position_ = {"", "", nullptr}; }; @@ -206,11 +210,13 @@ 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_current_ = get_by_interface(joint_handles, hardware_interface::HW_IF_CURRENT); joint_absolute_position_ = get_by_interface(joint_handles, HW_IF_ABSOLUTE_POSITION); if ( - !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ && - !joint_absolute_position_) + !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ && !joint_force_ && + !joint_current_ && !joint_absolute_position_) { throw Exception("None of the provided joint handles are valid or from the required interfaces"); } @@ -219,13 +225,16 @@ 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_current_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_CURRENT); 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_current_ && !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 +260,16 @@ 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_current_ && actuator_current_) + { + joint_current_.set_value(actuator_current_.get_value()); + } + if (joint_absolute_position_ && actuator_absolute_position_) { joint_absolute_position_.set_value( @@ -279,6 +298,16 @@ 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_); + } + + if (joint_current_ && actuator_current_) + { + actuator_current_.set_value(joint_current_.get_value()); + } } } // namespace transmission_interface diff --git a/transmission_interface/test/differential_transmission_test.cpp b/transmission_interface/test/differential_transmission_test.cpp index 792779c8c0..00d3613543 100644 --- a/transmission_interface/test/differential_transmission_test.cpp +++ b/transmission_interface/test/differential_transmission_test.cpp @@ -20,7 +20,9 @@ #include "random_generator_utils.hpp" #include "transmission_interface/differential_transmission.hpp" +using hardware_interface::HW_IF_CURRENT; 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 +130,8 @@ TEST(ConfigureTest, FailsWithBadHandles) testConfigureWithBadHandles(HW_IF_VELOCITY); testConfigureWithBadHandles(HW_IF_EFFORT); testConfigureWithBadHandles(HW_IF_TORQUE); + testConfigureWithBadHandles(HW_IF_FORCE); + testConfigureWithBadHandles(HW_IF_CURRENT); testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION); } @@ -225,6 +229,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 +298,30 @@ 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)); + } + + // Current interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, 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 +394,30 @@ 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)); + } + + // Current interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(20.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 +490,30 @@ 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)); + } + + // Current interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, 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(20.0, DoubleNear(j_val[1], EPS)); + } + // Absolute position interface { auto a1_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, a_vec[0]); @@ -514,6 +591,30 @@ 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)); + } + + // Current interface + { + auto a1_handle = ActuatorHandle("act1", HW_IF_CURRENT, a_vec[0]); + auto a2_handle = ActuatorHandle("act2", HW_IF_CURRENT, a_vec[1]); + auto joint1_handle = JointHandle("joint1", HW_IF_CURRENT, j_vec[0]); + auto joint2_handle = JointHandle("joint2", HW_IF_CURRENT, j_vec[1]); + trans.configure({joint1_handle, joint2_handle}, {a1_handle, a2_handle}); + trans.actuator_to_joint(); + EXPECT_THAT(8.0, DoubleNear(j_val[0], EPS)); + EXPECT_THAT(-2.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..bac0009a92 100644 --- a/transmission_interface/test/simple_transmission_test.cpp +++ b/transmission_interface/test/simple_transmission_test.cpp @@ -18,7 +18,9 @@ #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "transmission_interface/simple_transmission.hpp" +using hardware_interface::HW_IF_CURRENT; 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 +166,18 @@ 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_CURRENT, 1.0); + reset_values(); + testIdentityMap(trans, HW_IF_CURRENT, 0.0); + reset_values(); + testIdentityMap(trans, HW_IF_CURRENT, -1.0); + testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, 1.0); reset_values(); testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, 0.0); @@ -232,6 +246,26 @@ 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)); + } + + // Current interface + { + auto actuator_handle = ActuatorHandle("act1", HW_IF_CURRENT, &a_val); + auto joint_handle = JointHandle("joint1", HW_IF_CURRENT, &j_val); + trans.configure({joint_handle}, {actuator_handle}); + + trans.actuator_to_joint(); + EXPECT_THAT(1.0, DoubleNear(j_val, EPS)); + } + // Absolute position interface { auto actuator_handle = ActuatorHandle("act1", HW_IF_ABSOLUTE_POSITION, &a_val);