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

Jump to bottom

Parse URDF soft_limits into the HardwareInfo structure #1488

Merged
merged 5 commits into from
May 9, 2024
Merged

Parse URDF soft_limits into the HardwareInfo structure #1488

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
7e36ed5
Add SoftJointLimits in the HardwareInfo
adriaroig Apr 17, 2024
cb81690
Parse SoftJointLimits from the URDF
adriaroig Apr 18, 2024
e364267
Extend test_component_parser to check soft_limits
adriaroig Apr 18, 2024
7b2865e
Formatting changes
adriaroig Apr 18, 2024
c51b880
Update Jazzy release notes
saikishor May 7, 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
7 changes: 7 additions & 0 deletions hardware_interface/include/hardware_interface/hardware_info.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading