Skip to content

Commit edfb971

Browse files
adriaroigadriaroig
authored
Parse URDF soft_limits into the HardwareInfo structure (ros-controls#1488)
1 parent 736d379 commit edfb971

File tree

5 files changed

+124
-0
lines changed

5 files changed

+124
-0
lines changed

‎doc/release_notes/Jazzy.rst‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,30 @@ hardware_interface
7070
******************
7171
* A portable version for string-to-double conversion was added: ``hardware_interface::stod`` (`#1257 <https://github.com/ros-controls/ros2_control/pull/1257>`_)
7272
* ``test_components`` was moved to its own package (`#1325 <https://github.com/ros-controls/ros2_control/pull/1325>`_)
73+
* 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>`_)
74+
75+
.. code:: xml
76+
77+
<ros2_control name="RRBotSystemMutipleGPIOs" type="system">
78+
<hardware>
79+
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
80+
<param name="example_param_hw_start_duration_sec">2.0</param>
81+
<param name="example_param_hw_stop_duration_sec">3.0</param>
82+
<param name="example_param_hw_slowdown">2.0</param>
83+
</hardware>
84+
<joint name="joint1">
85+
<command_interface name="position">
86+
<param name="min">-1</param>
87+
<param name="max">1</param>
88+
</command_interface>
89+
<command_interface name="velocity">
90+
<limits enable="false"/>
91+
</command_interface>
92+
<state_interface name="position"/>
93+
</joint>
94+
</ros2_control>
95+
96+
* 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>`_)
7397

7498
joint_limits
7599
************

‎hardware_interface/include/hardware_interface/hardware_info.hpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,13 @@ struct HardwareInfo
171171
* The URDF parsed limits of the hardware components joint command interfaces
172172
*/
173173
std::unordered_map<std::string, joint_limits::JointLimits> limits;
174+
175+
/**
176+
* Map of software joint limits used for clamping the command where the key is the joint name.
177+
* Optional If not specified or less restrictive than the JointLimits uses the previous
178+
* JointLimits.
179+
*/
180+
std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_limits;
174181
};
175182

176183
} // namespace hardware_interface

‎hardware_interface/src/component_parser.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -927,6 +927,16 @@ std::vector<HardwareInfo> parse_control_resources_from_urdf(const std::string &
927927
// Take the most restricted one. Also valid for continuous-joint type only
928928
detail::update_interface_limits(joint.command_interfaces, limits);
929929
hw_info.limits[joint.name] = limits;
930+
joint_limits::SoftJointLimits soft_limits;
931+
if (getSoftJointLimits(urdf_joint, soft_limits))
932+
{
933+
if (limits.has_position_limits)
934+
{
935+
soft_limits.min_position = std::max(soft_limits.min_position, limits.min_position);
936+
soft_limits.max_position = std::min(soft_limits.max_position, limits.max_position);
937+
}
938+
hw_info.soft_limits[joint.name] = soft_limits;
939+
}
930940
}
931941
}
932942

‎hardware_interface/test/test_component_parser.cpp‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -141,6 +141,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
141141

142142
// Verify limits parsed from the URDF
143143
ASSERT_THAT(hardware_info.limits, SizeIs(2));
144+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
145+
char joint2_name[] = "joint2";
144146
for (const auto & joint : {"joint1", "joint2"})
145147
{
146148
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -152,6 +154,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface)
152154
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
153155
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
154156
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
157+
if (strcmp(joint, joint2_name) == 0)
158+
{
159+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
160+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
161+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
162+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
163+
}
155164
}
156165
}
157166

@@ -193,6 +202,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface
193202

194203
// Verify limits parsed from the URDF
195204
ASSERT_THAT(hardware_info.limits, SizeIs(2));
205+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
206+
char joint2_name[] = "joint2";
196207
for (const auto & joint : {"joint1", "joint2"})
197208
{
198209
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -205,6 +216,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface
205216
// effort and velocity limits won't change as they are above the main URDF hard limits
206217
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
207218
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
219+
if (strcmp(joint, joint2_name) == 0)
220+
{
221+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
222+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
223+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
224+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
225+
}
208226
}
209227
}
210228

@@ -253,6 +271,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens
253271

254272
// Verify limits parsed from the URDF
255273
ASSERT_THAT(hardware_info.limits, SizeIs(2));
274+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
275+
char joint2_name[] = "joint2";
256276
for (const auto & joint : {"joint1", "joint2"})
257277
{
258278
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
264284
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
265285
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
266286
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
287+
if (strcmp(joint, joint2_name) == 0)
288+
{
289+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
290+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
291+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
292+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
293+
}
267294
}
268295
}
269296

@@ -297,6 +324,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte
297324

298325
// Verify limits parsed from the URDF
299326
ASSERT_THAT(hardware_info.limits, SizeIs(2));
327+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
328+
char joint2_name[] = "joint2";
300329
for (const auto & joint : {"joint1", "joint2"})
301330
{
302331
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
308337
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
309338
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
310339
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
340+
if (strcmp(joint, joint2_name) == 0)
341+
{
342+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
343+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
344+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
345+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
346+
}
311347
}
312348

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

327364
TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot)
@@ -346,6 +383,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
346383
EXPECT_EQ(hardware_info.joints[0].type, "joint");
347384
// Verify limits parsed from the URDF
348385
ASSERT_THAT(hardware_info.limits, SizeIs(1));
386+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
349387
for (const auto & joint : {"joint1"})
350388
{
351389
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -373,6 +411,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
373411
EXPECT_EQ(hardware_info.joints[0].type, "joint");
374412
// Verify limits parsed from the URDF
375413
ASSERT_THAT(hardware_info.limits, SizeIs(1));
414+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
415+
char joint2_name[] = "joint2";
376416
for (const auto & joint : {"joint2"})
377417
{
378418
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -384,6 +424,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
384424
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
385425
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
386426
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
427+
if (strcmp(joint, joint2_name) == 0)
428+
{
429+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
430+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
431+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
432+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
433+
}
387434
}
388435
}
389436

@@ -421,6 +468,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
421468
EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1");
422469
// Verify limits parsed from the URDF
423470
ASSERT_THAT(hardware_info.limits, SizeIs(1));
471+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
424472
for (const auto & joint : {"joint1"})
425473
{
426474
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -451,6 +499,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
451499
EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1");
452500
// Verify limits parsed from the URDF
453501
ASSERT_THAT(hardware_info.limits, SizeIs(1));
502+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
503+
char joint2_name[] = "joint2";
454504
for (const auto & joint : {"joint2"})
455505
{
456506
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -461,6 +511,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
461511
// effort and velocity limits won't change as they are above the main URDF hard limits
462512
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
463513
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
514+
if (strcmp(joint, joint2_name) == 0)
515+
{
516+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
517+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5));
518+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
519+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
520+
}
464521
}
465522

466523
hardware_info = control_hardware.at(2);
@@ -481,6 +538,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
481538
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
482539
// Verify limits parsed from the URDF
483540
ASSERT_THAT(hardware_info.limits, SizeIs(1));
541+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
484542
for (const auto & joint : {"joint1"})
485543
{
486544
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -511,6 +569,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
511569
EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION);
512570
// Verify limits parsed from the URDF
513571
ASSERT_THAT(hardware_info.limits, SizeIs(1));
572+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
514573
for (const auto & joint : {"joint2"})
515574
{
516575
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);
@@ -521,6 +580,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot
521580
// effort and velocity limits won't change as they are above the main URDF hard limits
522581
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
523582
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
583+
if (strcmp(joint, joint2_name) == 0)
584+
{
585+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
586+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.5, 1e-5));
587+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
588+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
589+
}
524590
}
525591
}
526592

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

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

661729
TEST_F(TestComponentParser, successfully_parse_locale_independent_double)
@@ -727,6 +795,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio
727795

728796
// Verify limits parsed from the URDF
729797
ASSERT_THAT(hardware_info.limits, SizeIs(2));
798+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(1));
799+
char joint2_name[] = "joint2";
730800
for (const auto & joint : {"joint1", "joint2"})
731801
{
732802
// Position limits are limited in the ros2_control tag
@@ -737,6 +807,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio
737807
EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits);
738808
EXPECT_THAT(hardware_info.limits.at(joint).max_velocity, DoubleNear(0.2, 1e-5));
739809
EXPECT_THAT(hardware_info.limits.at(joint).max_effort, DoubleNear(0.1, 1e-5));
810+
if (strcmp(joint, joint2_name) == 0)
811+
{
812+
EXPECT_THAT(hardware_info.soft_limits.at(joint).max_position, DoubleNear(0.5, 1e-5));
813+
EXPECT_THAT(hardware_info.soft_limits.at(joint).min_position, DoubleNear(-1.0, 1e-5));
814+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_position, DoubleNear(10.0, 1e-5));
815+
EXPECT_THAT(hardware_info.soft_limits.at(joint).k_velocity, DoubleNear(20.0, 1e-5));
816+
}
740817
}
741818
}
742819

@@ -905,6 +982,10 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in
905982
EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits);
906983
EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5));
907984
EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5));
985+
EXPECT_THAT(hardware_info.soft_limits.at("joint2").max_position, DoubleNear(0.5, 1e-5));
986+
EXPECT_THAT(hardware_info.soft_limits.at("joint2").min_position, DoubleNear(-1.5, 1e-5));
987+
EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_position, DoubleNear(10.0, 1e-5));
988+
EXPECT_THAT(hardware_info.soft_limits.at("joint2").k_velocity, DoubleNear(20.0, 1e-5));
908989

909990
EXPECT_TRUE(hardware_info.limits.at("joint3").has_position_limits);
910991
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)
11691250

11701251
// Verify limits parsed from the URDF
11711252
ASSERT_THAT(hardware_info.limits, SizeIs(1));
1253+
ASSERT_THAT(hardware_info.soft_limits, SizeIs(0));
11721254
for (const auto & joint : {"joint1"})
11731255
{
11741256
EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits);

‎ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@ const auto urdf_head =
8686
<parent link="link1"/>
8787
<child link="link2"/>
8888
<limit effort="0.1" lower="-3.14159265359" upper="3.14159265359" velocity="0.2"/>
89+
<safety_controller soft_lower_limit="-1.5" soft_upper_limit="0.5" k_position="10.0" k_velocity="20.0"/>
8990
</joint>
9091
<link name="link2">
9192
<inertial>

0 commit comments

Comments
 (0)