Skip to content

Commit

Permalink
Successfully parse continuous joint limits if set in URDF
Browse files Browse the repository at this point in the history
  • Loading branch information
@christophfroehlich @saikishor
christophfroehlich authored and saikishor committed Apr 23, 2024
1 parent 3cfce6b commit 9b21ba4
Show file tree
Hide file tree
Showing 3 changed files with 145 additions and 6 deletions.
10 changes: 7 additions & 3 deletions hardware_interface/src/component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -691,11 +691,15 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
{
if (itr.name == hardware_interface::HW_IF_POSITION)
{
double min_pos(limits.min_position), max_pos(limits.max_position);
bool has_min_max_interface_values =
retrieve_min_max_interface_values(itr, min_pos, max_pos);
// position_limits can be restricted for continuous joints
limits.has_position_limits |= has_min_max_interface_values;
limits.has_position_limits &= itr.enable_limits;
if (limits.has_position_limits)
{
double min_pos(limits.min_position), max_pos(limits.max_position);
if (retrieve_min_max_interface_values(itr, min_pos, max_pos))
if (has_min_max_interface_values)
{
limits.min_position = std::max(min_pos, limits.min_position);
limits.max_position = std::min(max_pos, limits.max_position);
Expand Down Expand Up @@ -1007,7 +1011,7 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
}
else
{
// valid for continuous-joint type
// valid for continuous-joint type only
copy_interface_limits(joint.command_interfaces, limits);
}
hw_info.limits[joint.name] = limits;
Expand Down
50 changes: 47 additions & 3 deletions hardware_interface/test/test_component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1053,7 +1053,7 @@ TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits)
EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error);
}

TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_limits)
TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits)
{
std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) +
Expand All @@ -1074,8 +1074,10 @@ TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_lim
EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5))
<< "velocity constraint from ros2_control_tag";
EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5))
<< "effort constraint from ros2_control_tag";
EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));
Expand All @@ -1089,6 +1091,48 @@ TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_lim
EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
}

TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limits)
{
std::string urdf_to_test =
std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) +
ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces +
ros2_control_test_assets::urdf_tail;
EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test));

const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test);
ASSERT_THAT(control_hardware, SizeIs(1));
auto hardware_info = control_hardware.front();

ASSERT_GT(hardware_info.limits.count("joint1"), 0);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits);
EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5));
EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits);
EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits);
EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5))
<< "velocity URDF constraint overridden by ros2_control tag";
EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5))
<< "effort constraint from URDF";
EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5));

ASSERT_GT(hardware_info.limits.count("joint2"), 0);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits);
EXPECT_TRUE(hardware_info.limits.at("joint2").has_velocity_limits);
EXPECT_TRUE(hardware_info.limits.at("joint2").has_effort_limits);
EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5))
<< "velocity constraint from URDF";
EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5))
<< "effort constraint from URDF";
EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits);
EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
}

TEST_F(TestComponentParser, successfully_parse_parameter_empty)
{
const std::string urdf_to_test =
Expand Down
91 changes: 91 additions & 0 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 @@ -325,6 +325,97 @@ const auto urdf_head_continuous_missing_limits =
</link>
)";

const auto urdf_head_continuous_with_limits =
R"(
<?xml version="1.0" encoding="utf-8"?>
<robot name="MinimalRobot">
<!-- Used for fixing robot -->
<link name="world"/>
<joint name="base_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="world"/>
<child link="base_link"/>
</joint>
<link name="base_link">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="0.2" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="joint1" type="continuous">
<origin rpy="-1.57079632679 0 0" xyz="0 0 0.2"/>
<parent link="base_link"/>
<child link="link1"/>
<limit effort="0.1" velocity="0.2"/>
</joint>
<link name="link1">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
<joint name="joint2" type="continuous">
<origin rpy="1.57079632679 0 0" xyz="0 0 0.9"/>
<parent link="link1"/>
<child link="link2"/>
<limit effort="0.1" velocity="0.2"/>
</joint>
<link name="link2">
<inertial>
<mass value="0.01"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
<material name="DarkGrey">
<color rgba="0.4 0.4 0.4 1.0"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</collision>
</link>
)";

const auto urdf_head_prismatic_missing_limits =
R"(
<?xml version="1.0" encoding="utf-8"?>
Expand Down

0 comments on commit 9b21ba4

Please sign in to comment.