Skip to content

Commit

Permalink
Extend test_component_parser to check soft_limits
Browse files Browse the repository at this point in the history
  • Loading branch information
@adriaroig @saikishor
adriaroig authored and saikishor committed Apr 24, 2024
1 parent 0461800 commit 58c2b85
Show file tree
Hide file tree
Showing 2 changed files with 83 additions and 0 deletions.
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 58c2b85

Please sign in to comment.