Skip to content

Commit

Permalink
Remove mimic parameter from ros2_control tag
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich
christophfroehlich committed May 26, 2024
1 parent 9f1007b commit 594c39c
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 145 deletions.
105 changes: 30 additions & 75 deletions hardware_interface/src/component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -836,98 +836,53 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
for (auto i = 0u; i < hw_info.joints.size(); ++i)
{
const auto & joint = hw_info.joints.at(i);

// Search for mimic joints defined in ros2_control tag (deprecated)
// TODO(christophfroehlich) delete deprecated config with ROS-J
if (joint.parameters.find("mimic") != joint.parameters.cend())
auto urdf_joint = model.getJoint(joint.name);
if (!urdf_joint)
{
std::cerr << "Warning: Mimic joints defined in ros2_control tag are deprecated. "
<< "Please define mimic joints in URDF." << std::endl;
const auto mimicked_joint_it = std::find_if(
hw_info.joints.begin(), hw_info.joints.end(),
[&mimicked_joint =
joint.parameters.at("mimic")](const hardware_interface::ComponentInfo & joint_info)
{ return joint_info.name == mimicked_joint; });
if (mimicked_joint_it == hw_info.joints.cend())
{
throw std::runtime_error(
"Mimicked joint '" + joint.parameters.at("mimic") + "' not found");
}
hardware_interface::MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.multiplier = 1.0;
mimic_joint.offset = 0.0;
mimic_joint.mimicked_joint_index = std::distance(hw_info.joints.begin(), mimicked_joint_it);
auto param_it = joint.parameters.find("multiplier");
if (param_it != joint.parameters.end())
{
mimic_joint.multiplier = hardware_interface::stod(joint.parameters.at("multiplier"));
}
param_it = joint.parameters.find("offset");
if (param_it != joint.parameters.end())
{
mimic_joint.offset = hardware_interface::stod(joint.parameters.at("offset"));
}
hw_info.mimic_joints.push_back(mimic_joint);
throw std::runtime_error("Joint " + joint.name + " not found in URDF");
}
else
if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE)
{
auto urdf_joint = model.getJoint(joint.name);
if (!urdf_joint)
{
throw std::runtime_error("Joint " + joint.name + " not found in URDF");
}
if (!urdf_joint->mimic && joint.is_mimic == MimicAttribute::TRUE)
throw std::runtime_error(
"Joint '" + joint.name + "' has no mimic information in the URDF.");
}
if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE)
{
if (joint.command_interfaces.size() > 0)
{
throw std::runtime_error(
"Joint '" + joint.name + "' has no mimic information in the URDF.");
"Joint '" + joint.name +
"' has mimic attribute not set to false: Activated mimic joints cannot have command "
"interfaces.");
}
if (urdf_joint->mimic && joint.is_mimic != MimicAttribute::FALSE)
auto find_joint = [&hw_info](const std::string & name)
{
if (joint.command_interfaces.size() > 0)
auto it = std::find_if(
hw_info.joints.begin(), hw_info.joints.end(),
[&name](const auto & j) { return j.name == name; });
if (it == hw_info.joints.end())
{
throw std::runtime_error(
"Joint '" + joint.name +
"' has mimic attribute not set to false: Activated mimic joints cannot have command "
"interfaces.");
throw std::runtime_error("Mimic joint '" + name + "' not found in <ros2_control> tag");
}
auto find_joint = [&hw_info](const std::string & name)
{
auto it = std::find_if(
hw_info.joints.begin(), hw_info.joints.end(),
[&name](const auto & j) { return j.name == name; });
if (it == hw_info.joints.end())
{
throw std::runtime_error(
"Mimic joint '" + name + "' not found in <ros2_control> tag");
}
return std::distance(hw_info.joints.begin(), it);
};

MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
mimic_joint.multiplier = urdf_joint->mimic->multiplier;
mimic_joint.offset = urdf_joint->mimic->offset;
hw_info.mimic_joints.push_back(mimic_joint);
}
}
// TODO(christophfroehlich) remove this code if deprecated mimic attribute - branch
// from above is removed (double check here, but throws already above if not found in URDF)
auto urdf_joint = model.getJoint(joint.name);
if (!urdf_joint)
{
std::cerr << "Joint: '" + joint.name + "' not found in URDF. Skipping limits parsing!"
<< std::endl;
continue;
return std::distance(hw_info.joints.begin(), it);
};

MimicJoint mimic_joint;
mimic_joint.joint_index = i;
mimic_joint.mimicked_joint_index = find_joint(urdf_joint->mimic->joint_name);
mimic_joint.multiplier = urdf_joint->mimic->multiplier;
mimic_joint.offset = urdf_joint->mimic->offset;
hw_info.mimic_joints.push_back(mimic_joint);
}

if (urdf_joint->type == urdf::Joint::FIXED)
{
throw std::runtime_error(
"Joint '" + joint.name +
"' is of type 'fixed'. "
"Fixed joints do not make sense in ros2_control.");
}

joint_limits::JointLimits limits;
getJointLimits(urdf_joint, limits);
// Take the most restricted one. Also valid for continuous-joint type only
Expand Down
29 changes: 0 additions & 29 deletions hardware_interface/test/test_component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1341,35 +1341,6 @@ TEST_F(TestComponentParser, gripper_no_mimic_valid_config)
EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
}

// TODO(christophfroehlich) delete deprecated config test
TEST_F(TestComponentParser, gripper_mimic_deprecated_valid_config)
{
const auto urdf_to_test =
std::string(ros2_control_test_assets::gripper_urdf_head) +
std::string(ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated) +
std::string(ros2_control_test_assets::urdf_tail);
std::vector<hardware_interface::HardwareInfo> hw_info;
ASSERT_NO_THROW(hw_info = parse_control_resources_from_urdf(urdf_to_test));
ASSERT_THAT(hw_info, SizeIs(1));
ASSERT_THAT(hw_info[0].mimic_joints, SizeIs(1));
EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].multiplier, 2.0);
EXPECT_DOUBLE_EQ(hw_info[0].mimic_joints[0].offset, 1.0);
EXPECT_EQ(hw_info[0].mimic_joints[0].mimicked_joint_index, 0);
EXPECT_EQ(hw_info[0].mimic_joints[0].joint_index, 1);
}

TEST_F(TestComponentParser, gripper_mimic_deprecated_unknown_joint_throws_error)
{
const auto urdf_to_test =
std::string(ros2_control_test_assets::gripper_urdf_head) +
std::string(
ros2_control_test_assets::gripper_hardware_resources_mimic_deprecated_unknown_joint) +
std::string(ros2_control_test_assets::urdf_tail);
std::vector<hardware_interface::HardwareInfo> hw_info;
ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
}
// end delete deprecated config test

TEST_F(TestComponentParser, gripper_mimic_with_unknown_joint_throws_error)
{
const auto urdf_to_test =
Expand Down
41 changes: 0 additions & 41 deletions ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1290,47 +1290,6 @@ const auto gripper_hardware_resources_mimic_true_no_command_if =
</ros2_control>
)";

// TODO(christophfroehlich) delete deprecated config test
const auto gripper_hardware_resources_mimic_deprecated =
R"(
<ros2_control name="TestGripper" type="system">
<joint name="right_finger_joint">
<command_interface name="effort"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="left_finger_joint">
<param name="mimic">right_finger_joint</param>
<param name="multiplier">2</param>
<param name="offset">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
)";

const auto gripper_hardware_resources_mimic_deprecated_unknown_joint =
R"(
<ros2_control name="TestGripper" type="system">
<joint name="right_finger_joint">
<command_interface name="effort"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
<state_interface name="effort"/>
</joint>
<joint name="left_finger_joint">
<param name="mimic">middle_finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
</ros2_control>
)";
// end delete deprecated config test

const auto gripper_hardware_resources_mimic_true_command_if =
R"(
<ros2_control name="TestGripper" type="system">
Expand Down

0 comments on commit 594c39c

Please sign in to comment.