diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes/Jazzy.rst index 52fc54a071..4074cbca63 100644 --- a/doc/release_notes/Jazzy.rst +++ b/doc/release_notes/Jazzy.rst @@ -70,6 +70,30 @@ hardware_interface ****************** * A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 `_) * ``test_components`` was moved to its own package (`#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 `_) + + .. code:: xml + + + + ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware + 2.0 + 3.0 + 2.0 + + + + -1 + 1 + + + + + + + + +* Soft limits are also parsed from the URDF into the ``HardwareInfo`` structure for the defined joints (`#1488 `_) joint_limits ************ diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index cb548b9bf4..d94573bf5e 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -171,6 +171,13 @@ struct HardwareInfo * The URDF parsed limits of the hardware components joint command interfaces */ std::unordered_map 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 soft_limits; }; } // namespace hardware_interface diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index aad1d827ec..14e016df21 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -927,6 +927,16 @@ std::vector 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; + } } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 946fda7c15..6a0c11cf72 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -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); @@ -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)); + } } } @@ -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); @@ -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)); + } } } @@ -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); @@ -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)); + } } } @@ -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); @@ -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); @@ -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) @@ -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); @@ -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); @@ -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)); + } } } @@ -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); @@ -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); @@ -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); @@ -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); @@ -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); @@ -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)); + } } } @@ -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) @@ -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) @@ -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 @@ -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)); + } } } @@ -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)); @@ -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); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index f5f668fd12..31e630059b 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -86,6 +86,7 @@ const auto urdf_head = +