[Transmissions] Add force interface (backport #2588) (#2678)

Author: mergify[bot]

doc/release_notes.rst

@@ -214,3 +214,7 @@ ros2controlcli
     ros2 control <verb> <arguments> --ros-args -r __ns:=<namespace>
 
 * The CLI verbs ``list_hardware_components`` and ``list_hardware_interfaces`` will now show the data type used by the internal Command and State interfaces (`#2204 <https://github.com/ros-controls/ros2_control/pull/2204>`_).
+
+transmission_interface
+**********************
+* The ``simple_transmission`` and ``differential_transmission`` now also support the ``force`` interface (`#2588 <https://github.com/ros-controls/ros2_control/pull/2588>`_).

transmission_interface/include/transmission_interface/differential_transmission.hpp

@@ -168,12 +168,14 @@ class DifferentialTransmission : public Transmission
   std::vector<JointHandle> joint_velocity_;
   std::vector<JointHandle> joint_effort_;
   std::vector<JointHandle> joint_torque_;
+  std::vector<JointHandle> joint_force_;
   std::vector<JointHandle> joint_absolute_position_;
 
   std::vector<ActuatorHandle> actuator_position_;
   std::vector<ActuatorHandle> actuator_velocity_;
   std::vector<ActuatorHandle> actuator_effort_;
   std::vector<ActuatorHandle> actuator_torque_;
+  std::vector<ActuatorHandle> actuator_force_;
   std::vector<ActuatorHandle> 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_)));
 }

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

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]);

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);