diff --git a/transmission_interface/include/transmission_interface/differential_transmission.hpp b/transmission_interface/include/transmission_interface/differential_transmission.hpp index c36ea9019e..5dc61dc795 100644 --- a/transmission_interface/include/transmission_interface/differential_transmission.hpp +++ b/transmission_interface/include/transmission_interface/differential_transmission.hpp @@ -242,11 +242,48 @@ void DifferentialTransmission::configure( 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_force_.size() != 2 && joint_absolute_position_.size() != 2) + if (!joint_position_.empty() && joint_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint position handles were present. \n{}"), + get_handles_info())); + } + if (!joint_velocity_.empty() && joint_velocity_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint velocity handles were present. \n{}"), + get_handles_info())); + } + if (!joint_effort_.empty() && joint_effort_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint effort handles were present. \n{}"), + get_handles_info())); + } + if (!joint_torque_.empty() && joint_torque_.size() != 2) { - throw Exception("Not enough valid or required joint handles were presented."); + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint torque handles were present. \n{}"), + get_handles_info())); + } + if (!joint_force_.empty() && joint_force_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required joint force handles were present. \n{}"), + get_handles_info())); + } + if (!joint_absolute_position_.empty() && joint_absolute_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE( + "Not enough valid or required joint absolute position handles were present. \n{}"), + get_handles_info())); } actuator_position_ = @@ -262,27 +299,86 @@ void DifferentialTransmission::configure( 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_force_.size() != 2 && - actuator_absolute_position_.size() != 2) + if (!actuator_position_.empty() && actuator_position_.size() != 2) { throw Exception( fmt::format( - FMT_COMPILE("Not enough valid or required actuator handles were presented. \n{}"), + FMT_COMPILE("Not enough valid or required actuator position handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_velocity_.empty() && actuator_velocity_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator velocity handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_effort_.empty() && actuator_effort_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator effort handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_torque_.empty() && actuator_torque_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator torque handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_force_.empty() && actuator_force_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE("Not enough valid or required actuator force handles were present. \n{}"), + get_handles_info())); + } + if (!actuator_absolute_position_.empty() && actuator_absolute_position_.size() != 2) + { + throw Exception( + fmt::format( + FMT_COMPILE( + "Not enough valid or required actuator absolute position handles were " + "present. \n{}"), get_handles_info())); } - if ( - joint_position_.size() != actuator_position_.size() && - 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()) + if (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()) + { + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on velocity interfaces. \n{}"), get_handles_info())); + } + if (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()) { throw Exception( - fmt::format(FMT_COMPILE("Pair-wise mismatch on interfaces. \n{}"), get_handles_info())); + fmt::format( + FMT_COMPILE("Pair-wise mismatch on torque interfaces. \n{}"), get_handles_info())); + } + if (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()) + { + throw Exception( + fmt::format( + FMT_COMPILE("Pair-wise mismatch on absolute position interfaces. \n{}"), + get_handles_info())); } }