Skip to content

Commit

Permalink
Parse URDF soft_limits into the HardwareInfo structure (#1488)
Browse files Browse the repository at this point in the history
  • Loading branch information
@adriaroig
adriaroig authored May 9, 2024
1 parent 736d379 commit edfb971
Show file tree
Hide file tree
Showing 5 changed files with 124 additions and 0 deletions.
24 changes: 24 additions & 0 deletions doc/release_notes/Jazzy.rst
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,30 @@ hardware_interface
******************
* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 <https://github.com/ros-controls/ros2_control/pull/1257>`_)
* ``test_components`` was moved to its own package (`#1325 <https://github.com/ros-controls/ros2_control/pull/1325>`_)
* The ``ros2_control`` tag now supports parsing of the limits from the URDF into the ``HardwareInfo`` structure. More conservative limits can be defined using the ``min`` and ``max`` attributes per interface (`#1472 <https://github.com/ros-controls/ros2_control/pull/1472>`_)

.. code:: xml
<ros2_control name="RRBotSystemMutipleGPIOs" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_hw_start_duration_sec">2.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">2.0</param>
</hardware>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
<param name="max">1</param>
</command_interface>
<command_interface name="velocity">
<limits enable="false"/>
</command_interface>
<state_interface name="position"/>
</joint>
</ros2_control>
* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 <https://github.com/ros-controls/ros2_control/pull/1488>`_)

joint_limits
************
Expand Down
7 changes: 7 additions & 0 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -171,6 +171,13 @@ struct HardwareInfo
* The URDF parsed limits of the hardware components joint command interfaces
*/
std::unordered_map<std::string, joint_limits::JointLimits> limits;

/**
* Map of software joint limits used for clamping the command where the key is the joint name.
* Optional If not specified or less restrictive than the JointLimits uses the previous
* JointLimits.
*/
std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_limits;
};

} // namespace hardware_interface
Expand Down
10 changes: 10 additions & 0 deletions hardware_interface/src/component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -927,6 +927,16 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
// Take the most restricted one. Also valid for continuous-joint type only
detail::update_interface_limits(joint.command_interfaces, limits);
hw_info.limits[joint.name] = limits;
joint_limits::SoftJointLimits soft_limits;
if (getSoftJointLimits(urdf_joint, soft_limits))
{
if (limits.has_position_limits)
{
soft_limits.min_position = std::max(soft_limits.min_position, limits.min_position);
soft_limits.max_position = std::min(soft_limits.max_position, limits.max_position);
}
hw_info.soft_limits[joint.name] = soft_limits;
}
}
}

Expand Down
82 changes: 82 additions & 0 deletions hardware_interface/test/test_component_parser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -141,6 +141,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)

// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(2));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint1", "joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -152,6 +154,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}
}

Expand Down Expand Up @@ -193,6 +202,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface

// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(2));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint1", "joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -205,6 +216,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface
// effort and velocity limits won't change as they are above the main URDF hard limits
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}
}

Expand Down Expand Up @@ -253,6 +271,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens

// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(2));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint1", "joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -264,6 +284,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}
}

Expand Down Expand Up @@ -297,6 +324,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte

// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(2));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint1", "joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -308,6 +337,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}

hardware_info = control_hardware.at(1);
Expand All @@ -322,6 +358,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte
EXPECT_EQ(hardware_info.sensors[0].type, "sensor");
EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp");
ASSERT_THAT(hardware_info.limits, SizeIs(0));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot)
Expand All @@ -346,6 +383,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_EQ(hardware_info.joints[0].type, "joint");
// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
for (const auto & joint : {"joint1"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand Down Expand Up @@ -373,6 +411,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_EQ(hardware_info.joints[0].type, "joint");
// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -384,6 +424,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}
}

Expand Down Expand Up @@ -421,6 +468,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1");
// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
for (const auto & joint : {"joint1"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand Down Expand Up @@ -451,6 +499,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");
// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -461,6 +511,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
// effort and velocity limits won't change as they are above the main URDF hard limits
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}

hardware_info = control_hardware.at(2);
Expand All @@ -481,6 +538,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
for (const auto & joint : {"joint1"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand Down Expand Up @@ -511,6 +569,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
for (const auto & joint : {"joint2"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand All @@ -521,6 +580,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
// effort and velocity limits won't change as they are above the main URDF hard limits
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}
}

Expand Down Expand Up @@ -596,6 +662,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only)
EXPECT_EQ(hardware_info.sensors[1].state_interfaces[0].name, "image");
// There will be no limits as the ros2_control tag has only sensor info
ASSERT_THAT(hardware_info.limits, SizeIs(0));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
}

TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
Expand Down Expand Up @@ -656,6 +723,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only)
// effort and velocity limits won't change as they are above the main URDF hard limits
EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
}

TEST_F(TestComponentParser, successfully_parse_locale_independent_double)
Expand Down Expand Up @@ -727,6 +795,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio

// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(2));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
char joint2_name[] = "joint2";
for (const auto & joint : {"joint1", "joint2"})
{
// Position limits are limited in the ros2_control tag
Expand All @@ -737,6 +807,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
if (strcmp(joint, joint2_name) == 0)
{
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
}
}
}

Expand Down Expand Up @@ -905,6 +982,10 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in
EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at("joint2").max_position, DoubleNear(0.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at("joint2").min_position, DoubleNear(-1.5, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_position, DoubleNear(10.0, 1e-5));
EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_velocity, DoubleNear(20.0, 1e-5));

EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits);
EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5));
Expand Down Expand Up @@ -1169,6 +1250,7 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty)

// Verify limits parsed from the URDF
ASSERT_THAT(hardware_info.limits, SizeIs(1));
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
for (const auto & joint : {"joint1"})
{
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
Expand Down
1 change: 1 addition & 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 @@ -86,6 +86,7 @@ const auto urdf_head =
<parent link="link1"/>
<child link="link2"/>
<limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
<safety_controller soft_lower_limit="-1.5" soft_upper_limit="0.5" k_position="10.0" k_velocity="20.0"/>
</joint>
<link name="link2">
<inertial>
Expand Down

0 comments on commit edfb971

Please sign in to comment.