[Transmissions] Add absolute_position and torque interfaces (#2310)

Author: jordan-palacios

transmission_interface/include/transmission_interface/differential_transmission.hpp

@@ -108,6 +108,9 @@ namespace transmission_interface
  *
  * \ingroup transmission_types
  */
+
+constexpr auto HW_IF_ABSOLUTE_POSITION = "absolute_position";
+
 class DifferentialTransmission : public Transmission
 {
 public:
@@ -164,10 +167,14 @@ class DifferentialTransmission : public Transmission
   std::vector<JointHandle> joint_position_;
   std::vector<JointHandle> joint_velocity_;
   std::vector<JointHandle> joint_effort_;
+  std::vector<JointHandle> joint_torque_;
+  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_absolute_position_;
 };
 
 inline DifferentialTransmission::DifferentialTransmission(
@@ -228,8 +235,13 @@ void DifferentialTransmission::configure(
   joint_velocity_ =
     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_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)
+  if (
+    joint_position_.size() != 2 && joint_velocity_.size() != 2 && joint_effort_.size() != 2 &&
+    joint_torque_.size() != 2 && joint_absolute_position_.size() != 2)
   {
     throw Exception("Not enough valid or required joint handles were presented.");
   }
@@ -240,10 +252,15 @@ void DifferentialTransmission::configure(
     get_ordered_handles(actuator_handles, actuator_names, hardware_interface::HW_IF_VELOCITY);
   actuator_effort_ =
     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_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_effort_.size() != 2 && actuator_torque_.size() != 2 &&
+    actuator_absolute_position_.size() != 2)
   {
     throw Exception(
       fmt::format(
@@ -254,7 +271,9 @@ void DifferentialTransmission::configure(
   if (
     joint_position_.size() != actuator_position_.size() &&
     joint_velocity_.size() != actuator_velocity_.size() &&
-    joint_effort_.size() != actuator_effort_.size())
+    joint_effort_.size() != actuator_effort_.size() &&
+    joint_torque_.size() != actuator_torque_.size() &&
+    joint_absolute_position_.size() != actuator_absolute_position_.size())
   {
     throw Exception(
       fmt::format(FMT_COMPILE("Pair-wise mismatch on interfaces. \n{}"), get_handles_info()));
@@ -303,6 +322,32 @@ inline void DifferentialTransmission::actuator_to_joint()
     joint_eff[1].set_value(
       jr[1] * (act_eff[0].get_value() * ar[0] - act_eff[1].get_value() * ar[1]));
   }
+
+  auto & act_tor = actuator_torque_;
+  auto & joint_tor = joint_torque_;
+  if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
+  {
+    assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);
+
+    joint_tor[0].set_value(
+      jr[0] * (act_tor[0].get_value() * ar[0] + act_tor[1].get_value() * ar[1]));
+    joint_tor[1].set_value(
+      jr[1] * (act_tor[0].get_value() * ar[0] - act_tor[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())
+  {
+    assert(act_abs_pos[0] && act_abs_pos[1] && joint_abs_pos[0] && joint_abs_pos[1]);
+
+    joint_abs_pos[0].set_value(
+      (act_abs_pos[0].get_value() / ar[0] + act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[0]) +
+      joint_offset_[0]);
+    joint_abs_pos[1].set_value(
+      (act_abs_pos[0].get_value() / ar[0] - act_abs_pos[1].get_value() / ar[1]) / (2.0 * jr[1]) +
+      joint_offset_[1]);
+  }
 }
 
 inline void DifferentialTransmission::joint_to_actuator()
@@ -347,6 +392,18 @@ inline void DifferentialTransmission::joint_to_actuator()
     act_eff[1].set_value(
       (joint_eff[0].get_value() / jr[0] - joint_eff[1].get_value() / jr[1]) / (2.0 * ar[1]));
   }
+
+  auto & act_tor = actuator_torque_;
+  auto & joint_tor = joint_torque_;
+  if (act_tor.size() == num_actuators() && joint_tor.size() == num_joints())
+  {
+    assert(act_tor[0] && act_tor[1] && joint_tor[0] && joint_tor[1]);
+
+    act_tor[0].set_value(
+      (joint_tor[0].get_value() / jr[0] + joint_tor[1].get_value() / jr[1]) / (2.0 * ar[0]));
+    act_tor[1].set_value(
+      (joint_tor[0].get_value() / jr[0] - joint_tor[1].get_value() / jr[1]) / (2.0 * ar[1]));
+  }
 }
 
 std::string DifferentialTransmission::get_handles_info() const
@@ -356,10 +413,15 @@ std::string DifferentialTransmission::get_handles_info() const
       "Got the following handles:\n"
       "Joint position: {}, Actuator position: {}\n"
       "Joint velocity: {}, Actuator velocity: {}\n"
-      "Joint effort: {}, Actuator effort: {}"),
+      "Joint effort: {}, Actuator effort: {}\n"
+      "Joint torque: {}, Actuator torque: {}\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_effort_)), to_string(get_names(actuator_effort_)),
+    to_string(get_names(joint_torque_)), to_string(get_names(actuator_torque_)),
+    to_string(get_names(joint_absolute_position_)),
+    to_string(get_names(actuator_absolute_position_)));
 }
 
 }  // namespace transmission_interface

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