Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Remove deprecated mimic parameter from ros2_control tag #1553

Merged
merged 3 commits into from
Jul 3, 2024
Merged

Remove deprecated mimic parameter from ros2_control tag #1553

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
594c39c
Remove mimic parameter from ros2_control tag
christophfroehlich May 26, 2024
1f4b932
Merge branch 'master' into remove_mimic_legacy
christophfroehlich Jun 19, 2024
683c9f6
Merge branch 'master' into remove_mimic_legacy
christophfroehlich Jul 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
105 changes: 30 additions & 75 deletions hardware_interface/src/component_parser.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -838,98 +838,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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1664,47 +1664,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
Loading