Add torque and absolute position to simple transmission

Author: jordan-palacios

transmission_interface/include/transmission_interface/simple_transmission.hpp

@@ -80,6 +80,9 @@ namespace transmission_interface
  *
  * \ingroup transmission_types
  */
+
+constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position";
+
 class SimpleTransmission : public Transmission
 {
 public:
@@ -131,10 +134,14 @@ class SimpleTransmission : public Transmission
   JointHandle joint_position_ = {"", "", nullptr};
   JointHandle joint_velocity_ = {"", "", nullptr};
   JointHandle joint_effort_ = {"", "", nullptr};
+  JointHandle joint_torque_ = {"", "", nullptr};
+  JointHandle joint_absolute_position_ = {"", "", nullptr};
 
   ActuatorHandle actuator_position_ = {"", "", nullptr};
   ActuatorHandle actuator_velocity_ = {"", "", nullptr};
   ActuatorHandle actuator_effort_ = {"", "", nullptr};
+  ActuatorHandle actuator_torque_ = {"", "", nullptr};
+  ActuatorHandle actuator_absolute_position_ = {"", "", nullptr};
 };
 
 inline SimpleTransmission::SimpleTransmission(
@@ -198,17 +205,25 @@ inline void SimpleTransmission::configure(
   joint_position_ = get_by_interface(joint_handles, hardware_interface::HW_IF_POSITION);
   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_absolute_position_ = get_by_interface(joint_handles, HW_IF_ABSOLUTE_POSITION);
 
-  if (!joint_position_ && !joint_velocity_ && !joint_effort_)
+  if (
+    !joint_position_ && !joint_velocity_ && !joint_effort_ && !joint_torque_ &&
+    !joint_absolute_position_)
   {
     throw Exception("None of the provided joint handles are valid or from the required interfaces");
   }
 
   actuator_position_ = get_by_interface(actuator_handles, hardware_interface::HW_IF_POSITION);
   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_absolute_position_ = get_by_interface(actuator_handles, HW_IF_ABSOLUTE_POSITION);
 
-  if (!actuator_position_ && !actuator_velocity_ && !actuator_effort_)
+  if (
+    !actuator_position_ && !actuator_velocity_ && !actuator_effort_ && !actuator_torque_ &&
+    !actuator_absolute_position_)
   {
     throw Exception("None of the provided joint handles are valid or from the required interfaces");
   }
@@ -230,6 +245,17 @@ inline void SimpleTransmission::actuator_to_joint()
   {
     joint_position_.set_value(actuator_position_.get_value() / reduction_ + jnt_offset_);
   }
+
+  if (joint_torque_ && actuator_torque_)
+  {
+    joint_torque_.set_value(actuator_torque_.get_value() * reduction_);
+  }
+
+  if (joint_absolute_position_ && actuator_absolute_position_)
+  {
+    joint_absolute_position_.set_value(
+      actuator_absolute_position_.get_value() / reduction_ + jnt_offset_);
+  }
 }
 
 inline void SimpleTransmission::joint_to_actuator()
@@ -248,6 +274,11 @@ inline void SimpleTransmission::joint_to_actuator()
   {
     actuator_position_.set_value((joint_position_.get_value() - jnt_offset_) * reduction_);
   }
+
+  if (joint_torque_ && actuator_torque_)
+  {
+    actuator_torque_.set_value(joint_torque_.get_value() / reduction_);
+  }
 }
 
 }  // namespace transmission_interface

transmission_interface/test/simple_transmission_test.cpp

@@ -20,9 +20,11 @@
 
 using hardware_interface::HW_IF_EFFORT;
 using hardware_interface::HW_IF_POSITION;
+using hardware_interface::HW_IF_TORQUE;
 using hardware_interface::HW_IF_VELOCITY;
 using transmission_interface::ActuatorHandle;
 using transmission_interface::Exception;
+using transmission_interface::HW_IF_ABSOLUTE_POSITION;
 using transmission_interface::JointHandle;
 using transmission_interface::SimpleTransmission;
 
@@ -118,18 +120,15 @@ class BlackBoxTest : public TransmissionSetup, public ::testing::TestWithParam<S
   void testIdentityMap(
     SimpleTransmission & trans, const std::string & interface_name, const double ref_val)
   {
-    // Effort interface
-    {
-      auto actuator_handle = ActuatorHandle("act1", interface_name, &a_val);
-      auto joint_handle = JointHandle("joint1", interface_name, &j_val);
-      trans.configure({joint_handle}, {actuator_handle});
-
-      a_val = ref_val;
-
-      trans.actuator_to_joint();
-      trans.joint_to_actuator();
-      EXPECT_THAT(ref_val, DoubleNear(a_val, EPS));
-    }
+    auto actuator_handle = ActuatorHandle("act1", interface_name, &a_val);
+    auto joint_handle = JointHandle("joint1", interface_name, &j_val);
+    trans.configure({joint_handle}, {actuator_handle});
+
+    a_val = ref_val;
+
+    trans.actuator_to_joint();
+    trans.joint_to_actuator();
+    EXPECT_THAT(ref_val, DoubleNear(a_val, EPS));
   }
 };
 
@@ -158,6 +157,18 @@ TEST_P(BlackBoxTest, IdentityMap)
   testIdentityMap(trans, HW_IF_EFFORT, 0.0);
   reset_values();
   testIdentityMap(trans, HW_IF_EFFORT, -1.0);
+
+  testIdentityMap(trans, HW_IF_TORQUE, 1.0);
+  reset_values();
+  testIdentityMap(trans, HW_IF_TORQUE, 0.0);
+  reset_values();
+  testIdentityMap(trans, HW_IF_TORQUE, -1.0);
+
+  testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, 1.0);
+  reset_values();
+  testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, 0.0);
+  reset_values();
+  testIdentityMap(trans, HW_IF_ABSOLUTE_POSITION, -1.0);
 }
 
 INSTANTIATE_TEST_SUITE_P(
@@ -211,6 +222,26 @@ TEST_F(WhiteBoxTest, MoveJoint)
     EXPECT_THAT(1.1, DoubleNear(j_val, EPS));
   }
 
+  // Torque interface
+  {
+    auto actuator_handle = ActuatorHandle("act1", HW_IF_TORQUE, &a_val);
+    auto joint_handle = JointHandle("joint1", HW_IF_TORQUE, &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);
+    auto joint_handle = JointHandle("joint1", HW_IF_ABSOLUTE_POSITION, &j_val);
+    trans.configure({joint_handle}, {actuator_handle});
+
+    trans.actuator_to_joint();
+    EXPECT_THAT(1.1, DoubleNear(j_val, EPS));
+  }
+
   // Mismatched interface is ignored
   {
     double unique_value = 13.37;