Improving differential_transmission configure checks (#2812)

Author: jordan-palacios

transmission_interface/include/transmission_interface/differential_transmission.hpp

@@ -344,42 +344,74 @@ void DifferentialTransmission::configure(
         get_handles_info()));
   }
 
-  if (joint_position_.size() != actuator_position_.size())
+  if (
+    !joint_position_.empty() && !actuator_position_.empty() &&
+    joint_position_.size() != actuator_position_.size())
   {
     throw Exception(
       fmt::format(
         FMT_COMPILE("Pair-wise mismatch on position interfaces. \n{}"), get_handles_info()));
   }
-  if (joint_velocity_.size() != actuator_velocity_.size())
+  if (
+    !joint_velocity_.empty() && !actuator_velocity_.empty() &&
+    joint_velocity_.size() != actuator_velocity_.size())
   {
     throw Exception(
       fmt::format(
         FMT_COMPILE("Pair-wise mismatch on velocity interfaces. \n{}"), get_handles_info()));
   }
-  if (joint_effort_.size() != actuator_effort_.size())
+  if (
+    !joint_effort_.empty() && !actuator_effort_.empty() &&
+    joint_effort_.size() != actuator_effort_.size())
   {
     throw Exception(
       fmt::format(
         FMT_COMPILE("Pair-wise mismatch on effort interfaces. \n{}"), get_handles_info()));
   }
-  if (joint_torque_.size() != actuator_torque_.size())
+  if (
+    !joint_torque_.empty() && !actuator_torque_.empty() &&
+    joint_torque_.size() != actuator_torque_.size())
   {
     throw Exception(
       fmt::format(
         FMT_COMPILE("Pair-wise mismatch on torque interfaces. \n{}"), get_handles_info()));
   }
-  if (joint_force_.size() != actuator_force_.size())
+  if (
+    !joint_force_.empty() && !actuator_force_.empty() &&
+    joint_force_.size() != actuator_force_.size())
   {
     throw Exception(
       fmt::format(FMT_COMPILE("Pair-wise mismatch on force interfaces. \n{}"), get_handles_info()));
   }
-  if (joint_absolute_position_.size() != actuator_absolute_position_.size())
+  if (
+    !joint_absolute_position_.empty() && !actuator_absolute_position_.empty() &&
+    joint_absolute_position_.size() != actuator_absolute_position_.size())
   {
     throw Exception(
       fmt::format(
         FMT_COMPILE("Pair-wise mismatch on absolute position interfaces. \n{}"),
         get_handles_info()));
   }
+
+  // Check at least one pair-wise interface is available
+  if (!((!joint_position_.empty() && !actuator_position_.empty() &&
+         joint_position_.size() == actuator_position_.size()) ||
+        (!joint_velocity_.empty() && !actuator_velocity_.empty() &&
+         joint_velocity_.size() == actuator_velocity_.size()) ||
+        (!joint_effort_.empty() && !actuator_effort_.empty() &&
+         joint_effort_.size() == actuator_effort_.size()) ||
+        (!joint_torque_.empty() && !actuator_torque_.empty() &&
+         joint_torque_.size() == actuator_torque_.size()) ||
+        (!joint_force_.empty() && !actuator_force_.empty() &&
+         joint_force_.size() == actuator_force_.size()) ||
+        (!joint_absolute_position_.empty() && !actuator_absolute_position_.empty() &&
+         joint_absolute_position_.size() == actuator_absolute_position_.size())))
+  {
+    throw Exception(
+      fmt::format(
+        FMT_COMPILE("No pair-wise interface available between joints and actuators. \n{}"),
+        get_handles_info()));
+  }
 }
 
 inline void DifferentialTransmission::actuator_to_joint()

transmission_interface/test/differential_transmission_test.cpp

@@ -133,6 +133,37 @@ TEST(ConfigureTest, FailsWithBadHandles)
   testConfigureWithBadHandles(HW_IF_ABSOLUTE_POSITION);
 }
 
+TEST(ConfigureTest, SuccessWithOneGoodHandle)
+{
+  DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0});
+  double dummy;
+
+  auto a1_p_handle = ActuatorHandle("act1", HW_IF_POSITION, &dummy);
+  auto a2_p_handle = ActuatorHandle("act2", HW_IF_POSITION, &dummy);
+  auto a1_f_handle = ActuatorHandle("act1", HW_IF_FORCE, &dummy);
+  auto a2_f_handle = ActuatorHandle("act2", HW_IF_FORCE, &dummy);
+  auto j1_p_handle = JointHandle("joint1", HW_IF_POSITION, &dummy);
+  auto j2_p_handle = JointHandle("joint2", HW_IF_POSITION, &dummy);
+
+  // No exception should be thrown even though there is no force interface in the joints
+  ASSERT_NO_THROW(trans.configure(
+    {j1_p_handle, j2_p_handle}, {a1_p_handle, a2_p_handle, a1_f_handle, a2_f_handle}));
+}
+
+TEST(ConfigureTest, FailWhenGoodHandles)
+{
+  DifferentialTransmission trans({1.0, 1.0}, {1.0, 1.0});
+  double dummy;
+
+  auto a1_f_handle = ActuatorHandle("act1", HW_IF_FORCE, &dummy);
+  auto a2_f_handle = ActuatorHandle("act2", HW_IF_FORCE, &dummy);
+  auto j1_p_handle = JointHandle("joint1", HW_IF_POSITION, &dummy);
+  auto j2_p_handle = JointHandle("joint2", HW_IF_POSITION, &dummy);
+
+  // No pair-wise interfaces available
+  EXPECT_THROW(trans.configure({j1_p_handle, j2_p_handle}, {a1_f_handle, a2_f_handle}), Exception);
+}
+
 class TransmissionSetup : public ::testing::Test
 {
 protected: