From f37ae78977f15cdcda9aaea62fdceaaa58a85253 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 13:44:50 +0100 Subject: [PATCH 01/31] Add joint_limits dependency and add limits map to the HardwareInfo struct --- hardware_interface/CMakeLists.txt | 1 + .../include/hardware_interface/hardware_info.hpp | 4 ++++ hardware_interface/package.xml | 1 + 3 files changed, 6 insertions(+) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 4ea7fdc2f7..a5debef3b7 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils TinyXML2 tinyxml2_vendor + joint_limits urdf ) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 97abc00438..1d30933b48 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -19,6 +19,8 @@ #include #include +#include "joint_limits/joint_limits.hpp" + namespace hardware_interface { /** @@ -163,6 +165,8 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; + + std::unordered_map limits; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 41daac50cd..083337549c 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + joint_limits urdf rcutils From 7d1077a863a3f5536cc77f0648ffc020534d8563 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 13:47:11 +0100 Subject: [PATCH 02/31] Retrieve limits from the URDF as well as ros2_control tag from robot_description --- hardware_interface/src/component_parser.cpp | 159 ++++++++++++++++++++ 1 file changed, 159 insertions(+) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0841265e81..374e0a29ff 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -26,6 +26,9 @@ #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "joint_limits/joint_limits_urdf.hpp" +#include "urdf/model.h" namespace { @@ -629,6 +632,141 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } + // Retrieve the limits from ros2_control command interface tags and override if restrictive + auto update_interface_limits = [](const auto & interfaces, joint_limits::JointLimits & limits) + { + auto retrieve_min_max_interface_values = []( + const auto & itf, double & min, double & max) -> bool + { + try + { + if (!(itf.min.empty() && itf.max.empty())) + { + // If the limits don't exist then return false as they are not retrieved + return false; + } + if (!itf.min.empty()) + { + min = hardware_interface::stod(itf.min); + } + if (!itf.max.empty()) + { + max = hardware_interface::stod(itf.max); + } + return true; + } + catch (const std::invalid_argument & err) + { + std::cerr << "Error parsing the limits for the interface : " << itf.name + << "from the tags [" << kMinTag << " : '" << itf.min << "' and " << kMaxTag + << " : '" << itf.max << "'] within " << kROS2ControlTag + << " tag inside the URDF. Skipping it" << std::endl; + return false; + } + }; + for (auto & itr : interfaces) + { + if (itr.name == hardware_interface::HW_IF_POSITION) + { + if (limits.has_position_limits) + { + double min_pos(limits.min_position), max_pos(limits.max_position); + if (retrieve_min_max_interface_values(itr, min_pos, max_pos)) + { + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + } + } + else + { + limits.min_position = std::numeric_limits::min(); + limits.max_position = std::numeric_limits::max(); + } + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + if (limits.has_velocity_limits) + { // Apply the most restrictive one in the case + double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); + if (retrieve_min_max_interface_values(itr, min_vel, max_vel)) + { + max_vel = std::min(std::abs(min_vel), max_vel); + limits.max_velocity = std::min(max_vel, limits.max_velocity); + } + } + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + if (limits.has_effort_limits) + { // Apply the most restrictive one in the case + double min_eff(-limits.max_effort), max_eff(limits.max_effort); + if (retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + max_eff = std::min(std::abs(min_eff), max_eff); + limits.max_effort = std::min(max_eff, limits.max_effort); + } + } + } + else if (itr.name == hardware_interface::HW_IF_ACCELERATION) + { + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (retrieve_min_max_interface_values(itr, max_decel, max_accel)) + { + if (std::isfinite(max_decel)) + { + limits.max_deceleration = max_decel; + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + } + if (std::isfinite(max_accel)) + { + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = -limits.max_acceleration; + } + } + limits.has_acceleration_limits = true; + } + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + std::cerr << "Error parsing the limits for the interface : " << itr.name + << "from the tag : " << kMinTag << " within " << kROS2ControlTag + << " tag inside the URDF. Jerk only accepts max limits." << std::endl; + } + double min_jerk(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if (!itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk)) + { + if (std::isfinite(max_jerk)) + { + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = true; + } + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + std::cerr << "Unable to parse the limits for the interface : " << itr.name + << "from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are : " + "position, velocity, effort, acceleration and jerk." + << std::endl; + } + } + } + }; + // parse full URDF for mimic options urdf::Model model; if (!model.initString(urdf)) @@ -716,6 +854,27 @@ std::vector parse_control_resources_from_urdf(const std::string & hw_info.mimic_joints.push_back(mimic_joint); } } + auto urdf_joint = model.getJoint(joint.name); + if (!urdf_joint) + { + throw std::runtime_error("Joint " + joint.name + " not found in URDF"); + } + joint_limits::JointLimits limits; + const bool has_hard_limts = getJointLimits(urdf_joint, limits); + if ( + !has_hard_limts && + (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC || + urdf_joint->type == urdf::Joint::CONTINUOUS)) + { + throw std::runtime_error("Missing URDF joint limits for the Joint : " + joint.name); + } + if (has_hard_limts) + { + // Take the most restricted one + update_interface_limits(joint.command_interfaces, limits); + update_interface_limits(joint.state_interfaces, limits); + hw_info.limits[joint.name] = limits; + } } } From f43124d5597bec7e8e4cab37181d614cd3a92f66 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 17:11:23 +0100 Subject: [PATCH 03/31] print error and continue instead of throwing exception --- hardware_interface/src/component_parser.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 374e0a29ff..d61971b0f5 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -857,7 +857,9 @@ std::vector parse_control_resources_from_urdf(const std::string & auto urdf_joint = model.getJoint(joint.name); if (!urdf_joint) { - throw std::runtime_error("Joint " + joint.name + " not found in URDF"); + std::cerr << "Joint : '" + joint.name + "' not found in URDF. Skipping limits parsing!" + << std::endl; + continue; } joint_limits::JointLimits limits; const bool has_hard_limts = getJointLimits(urdf_joint, limits); From b39f1cfc3dcba7efa0ddabeba3ab80b45b0d1273 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 17:56:45 +0100 Subject: [PATCH 04/31] Fix the logic of the retrieval of min and max elements --- hardware_interface/src/component_parser.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index d61971b0f5..70ab5ead9d 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -640,7 +640,7 @@ std::vector parse_control_resources_from_urdf(const std::string & { try { - if (!(itf.min.empty() && itf.max.empty())) + if (itf.min.empty() && itf.max.empty()) { // If the limits don't exist then return false as they are not retrieved return false; @@ -658,7 +658,7 @@ std::vector parse_control_resources_from_urdf(const std::string & catch (const std::invalid_argument & err) { std::cerr << "Error parsing the limits for the interface : " << itf.name - << "from the tags [" << kMinTag << " : '" << itf.min << "' and " << kMaxTag + << " from the tags [" << kMinTag << " : '" << itf.min << "' and " << kMaxTag << " : '" << itf.max << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" << std::endl; return false; @@ -738,7 +738,7 @@ std::vector parse_control_resources_from_urdf(const std::string & if (!itr.min.empty()) { std::cerr << "Error parsing the limits for the interface : " << itr.name - << "from the tag : " << kMinTag << " within " << kROS2ControlTag + << " from the tag : " << kMinTag << " within " << kROS2ControlTag << " tag inside the URDF. Jerk only accepts max limits." << std::endl; } double min_jerk(std::numeric_limits::quiet_NaN()), @@ -757,7 +757,7 @@ std::vector parse_control_resources_from_urdf(const std::string & if (!itr.min.empty() || !itr.max.empty()) { std::cerr << "Unable to parse the limits for the interface : " << itr.name - << "from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " << kROS2ControlTag << " tag inside the URDF. Supported interfaces for joint limits are : " "position, velocity, effort, acceleration and jerk." From f4586f313b649a9aad8314a61d01779f268e11ea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 18:31:47 +0100 Subject: [PATCH 05/31] update tests adding the limits part --- .../test/test_component_parser.cpp | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 968d41ed97..25fcab703b 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -137,6 +137,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_one_interface) EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].max, "1"); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface) @@ -174,6 +189,22 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_interface ASSERT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(1)); ASSERT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(3)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[2].name, HW_IF_EFFORT); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // 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)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sensor) @@ -218,6 +249,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_sens EXPECT_EQ(hardware_info.sensors[0].parameters.at("frame_id"), "kuka_tcp"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("lower_limits"), "-100"); EXPECT_EQ(hardware_info.sensors[0].parameters.at("upper_limits"), "100"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_external_sensor) @@ -248,6 +294,21 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte ASSERT_THAT(hardware_info.sensors, SizeIs(0)); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } + hardware_info = control_hardware.at(1); EXPECT_EQ(hardware_info.name, "RRBotForceTorqueSensor2D"); @@ -259,6 +320,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_exte EXPECT_EQ(hardware_info.sensors[0].name, "tcp_fts_sensor"); 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)); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot) @@ -281,6 +343,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } hardware_info = control_hardware.at(1); @@ -294,6 +370,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].name, "joint2"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + // position limits will be limited than original range, as their range is limited in the + // ros2_control tags + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot_with_sensors) @@ -328,6 +418,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot hardware_info.transmissions[0].joints[0].mechanical_reduction, DoubleNear(1024.0 / M_PI, 0.01)); ASSERT_THAT(hardware_info.transmissions[0].actuators, SizeIs(1)); EXPECT_THAT(hardware_info.transmissions[0].actuators[0].name, "actuator1"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // 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)); + } hardware_info = control_hardware.at(1); @@ -345,6 +448,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_VELOCITY); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].min, "-1"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].max, "1"); + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // 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)); + } hardware_info = control_hardware.at(2); @@ -362,6 +478,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); 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)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // 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)); + } hardware_info = control_hardware.at(3); @@ -379,6 +508,19 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_modular_robot ASSERT_THAT(hardware_info.joints[0].command_interfaces, IsEmpty()); ASSERT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); 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)); + for (const auto & joint : {"joint2"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at(joint).has_effort_limits); + // 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)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_multi_joints_transmission) From aec2e6fc894822ec9fac73b7c3f4a3c70e9e3905 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 18:34:28 +0100 Subject: [PATCH 06/31] remove limits enforcing when defined under state interface --- hardware_interface/src/component_parser.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 70ab5ead9d..91180b0d58 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -874,7 +874,6 @@ std::vector parse_control_resources_from_urdf(const std::string & { // Take the most restricted one update_interface_limits(joint.command_interfaces, limits); - update_interface_limits(joint.state_interfaces, limits); hw_info.limits[joint.name] = limits; } } From 3c7a4d798cfd24c3957774dff1a5c6ed2e7a9281 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 19:15:39 +0100 Subject: [PATCH 07/31] add more testing for other cases --- .../test/test_component_parser.cpp | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 25fcab703b..a23707e3a0 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -593,6 +593,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_sensor_only) EXPECT_EQ(hardware_info.sensors[1].type, "sensor"); ASSERT_THAT(hardware_info.sensors[1].state_interfaces, SizeIs(1)); 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)); } TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) @@ -642,6 +644,17 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_actuator_only) EXPECT_THAT(actuator.offset, DoubleEq(0.0)); ASSERT_THAT(transmission.parameters, SizeIs(1)); EXPECT_EQ(transmission.parameters.at("additional_special_parameter"), "1337"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + // 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)); } TEST_F(TestComponentParser, successfully_parse_locale_independent_double) @@ -710,6 +723,20 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ(hardware_info.gpios[1].command_interfaces[0].name, "vacuum"); EXPECT_THAT(hardware_info.transmissions, IsEmpty()); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(2)); + for (const auto & joint : {"joint1", "joint2"}) + { + // Position limits are limited in the ros2_control tag + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } } TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_data_type) @@ -782,6 +809,19 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_write_for_sec"), ""); EXPECT_EQ(hardware_info.hardware_parameters.at("example_param_read_for_sec"), "2"); + + // Verify limits parsed from the URDF + ASSERT_THAT(hardware_info.limits, SizeIs(1)); + for (const auto & joint : {"joint1"}) + { + EXPECT_TRUE(hardware_info.limits.at(joint).has_position_limits); + EXPECT_THAT(hardware_info.limits.at(joint).max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at(joint).min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at(joint).has_velocity_limits); + 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)); + } } TEST_F(TestComponentParser, negative_size_throws_error) From ce26ef286a0b445c750e185686014eda1c240127 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 2 Apr 2024 19:20:32 +0200 Subject: [PATCH 08/31] added comments to the limits map and removed extra include --- .../include/hardware_interface/hardware_info.hpp | 4 +++- hardware_interface/src/component_parser.cpp | 1 - 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 1d30933b48..94de9ce15a 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -165,7 +165,9 @@ struct HardwareInfo * The XML contents prior to parsing */ std::string original_xml; - + /** + * The URDF parsed limits of the hardware components joint command interfaces + */ std::unordered_map limits; }; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 91180b0d58..0233082398 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -28,7 +28,6 @@ #include "hardware_interface/lexical_casts.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_limits/joint_limits_urdf.hpp" -#include "urdf/model.h" namespace { From 8e0faba73cdc75645a210c5f5695bacfb042e870 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Apr 2024 13:57:05 +0200 Subject: [PATCH 09/31] Added a way to enable and disable the joint interface limits --- .../hardware_interface/hardware_info.hpp | 2 ++ hardware_interface/src/component_parser.cpp | 34 ++++++++++++++++--- 2 files changed, 32 insertions(+), 4 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 94de9ce15a..cb548b9bf4 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -44,6 +44,8 @@ struct InterfaceInfo std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; + /// (Optional) enable or disable the limits for the command interfaces + bool enable_limits; }; /// @brief This structure stores information about a joint that is mimicking another joint diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 0233082398..103be8f6b6 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -45,6 +45,8 @@ constexpr const auto kCommandInterfaceTag = "command_interface"; constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; +constexpr const auto kLimitsTag = "limits"; +constexpr const auto kEnableTag = "enable"; constexpr const auto kInitialValueTag = "initial_value"; constexpr const auto kMimicAttribute = "mimic"; constexpr const auto kDataTypeAttribute = "data_type"; @@ -291,6 +293,15 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( interface.max = interface_param->second; } + // Option enable or disable the interface limits, by default they are enabled + interface.enable_limits = true; + const auto * limits_it = interfaces_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + interface.enable_limits = + parse_bool(get_attribute_value(limits_it, kEnableTag, limits_it->Name())); + } + // Optional initial_value attribute interface_param = interface_params.find(kInitialValueTag); if (interface_param != interface_params.end()) @@ -335,11 +346,21 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it } } + // Option enable or disable the interface limits, by default they are enabled + bool enable_limits = true; + const auto * limits_it = component_it->FirstChildElement(kLimitsTag); + if (limits_it) + { + enable_limits = parse_bool(get_attribute_value(limits_it, kEnableTag, limits_it->Name())); + } + // Parse all command interfaces const auto * command_interfaces_it = component_it->FirstChildElement(kCommandInterfaceTag); while (command_interfaces_it) { - component.command_interfaces.push_back(parse_interfaces_from_xml(command_interfaces_it)); + InterfaceInfo cmd_info = parse_interfaces_from_xml(command_interfaces_it); + cmd_info.enable_limits &= enable_limits; + component.command_interfaces.push_back(cmd_info); command_interfaces_it = command_interfaces_it->NextSiblingElement(kCommandInterfaceTag); } @@ -347,7 +368,9 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it const auto * state_interfaces_it = component_it->FirstChildElement(kStateInterfaceTag); while (state_interfaces_it) { - component.state_interfaces.push_back(parse_interfaces_from_xml(state_interfaces_it)); + InterfaceInfo state_info = parse_interfaces_from_xml(state_interfaces_it); + state_info.enable_limits &= enable_limits; + component.state_interfaces.push_back(state_info); state_interfaces_it = state_interfaces_it->NextSiblingElement(kStateInterfaceTag); } @@ -667,6 +690,7 @@ std::vector parse_control_resources_from_urdf(const std::string & { if (itr.name == hardware_interface::HW_IF_POSITION) { + limits.has_position_limits &= itr.enable_limits; if (limits.has_position_limits) { double min_pos(limits.min_position), max_pos(limits.max_position); @@ -684,6 +708,7 @@ std::vector parse_control_resources_from_urdf(const std::string & } else if (itr.name == hardware_interface::HW_IF_VELOCITY) { + limits.has_velocity_limits &= itr.enable_limits; if (limits.has_velocity_limits) { // Apply the most restrictive one in the case double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); @@ -696,6 +721,7 @@ std::vector parse_control_resources_from_urdf(const std::string & } else if (itr.name == hardware_interface::HW_IF_EFFORT) { + limits.has_effort_limits &= itr.enable_limits; if (limits.has_effort_limits) { // Apply the most restrictive one in the case double min_eff(-limits.max_effort), max_eff(limits.max_effort); @@ -729,7 +755,7 @@ std::vector parse_control_resources_from_urdf(const std::string & limits.max_deceleration = -limits.max_acceleration; } } - limits.has_acceleration_limits = true; + limits.has_acceleration_limits = true && itr.enable_limits; } } else if (itr.name == "jerk") @@ -747,7 +773,7 @@ std::vector parse_control_resources_from_urdf(const std::string & if (std::isfinite(max_jerk)) { limits.max_jerk = std::abs(max_jerk); - limits.has_jerk_limits = true; + limits.has_jerk_limits = true && itr.enable_limits; } } } From 1779bc0ce866a4032d5f826a2c2202cc6eaade9a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Apr 2024 13:57:25 +0200 Subject: [PATCH 10/31] Added tests for the newly added disabling of joint limits --- .../test/test_component_parser.cpp | 84 +++++++++++++++++++ .../components_urdfs.hpp | 39 +++++++++ 2 files changed, 123 insertions(+) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index a23707e3a0..6107f03e55 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -785,6 +785,90 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(3)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2); + EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(std::numeric_limits::min(), 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_position, DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint2").min_position, DoubleNear(std::numeric_limits::min(), 1e-5)); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_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)); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 7b2dd46e7a..59f032a298 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -400,6 +400,45 @@ const auto valid_urdf_ros2_control_system_robot_with_size_and_data_type = )"; +// 12. Industrial Robots with integrated GPIO with few disabled limits in joints +const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + + + + + -1 + 1 + + + + + + + + + + + +)"; + const auto valid_urdf_ros2_control_parameter_empty = R"( From e3847f96de67d8da99dbe71f1e51fb91763f0121 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 18:18:50 +0200 Subject: [PATCH 11/31] Add the missing limits to the diffbot urdf continuous joints --- .../include/ros2_control_test_assets/descriptions.hpp | 2 ++ 1 file changed, 2 insertions(+) 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 308751e0d8..3511a32fc8 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 @@ -503,6 +503,7 @@ const auto diffbot_urdf = + @@ -538,6 +539,7 @@ const auto diffbot_urdf = + From 68874f50189d86f0d945d9eb450dbb9bb020664b Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Apr 2024 21:45:49 +0200 Subject: [PATCH 12/31] Fix formatting changes --- .../test/test_component_parser.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6107f03e55..a132f17d37 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -851,16 +851,24 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); - EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(std::numeric_limits::max(), 1e-5)); - EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(std::numeric_limits::min(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(std::numeric_limits::min(), 1e-5)); EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); - EXPECT_THAT(hardware_info.limits.at("joint2").max_position, DoubleNear(std::numeric_limits::max(), 1e-5)); - EXPECT_THAT(hardware_info.limits.at("joint2").min_position, DoubleNear(std::numeric_limits::min(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(std::numeric_limits::min(), 1e-5)); EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); From 7725dd77c1f625e5dc05b4f04ec10273a90fd21a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 17:42:03 +0200 Subject: [PATCH 13/31] Change kEnableTag to kEnableAttribute and other minor typos MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- hardware_interface/src/component_parser.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 103be8f6b6..cfa47f54ef 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -46,7 +46,7 @@ constexpr const auto kStateInterfaceTag = "state_interface"; constexpr const auto kMinTag = "min"; constexpr const auto kMaxTag = "max"; constexpr const auto kLimitsTag = "limits"; -constexpr const auto kEnableTag = "enable"; +constexpr const auto kEnableAttribute = "enable"; constexpr const auto kInitialValueTag = "initial_value"; constexpr const auto kMimicAttribute = "mimic"; constexpr const auto kDataTypeAttribute = "data_type"; @@ -299,7 +299,7 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( if (limits_it) { interface.enable_limits = - parse_bool(get_attribute_value(limits_it, kEnableTag, limits_it->Name())); + parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); } // Optional initial_value attribute @@ -351,7 +351,7 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it const auto * limits_it = component_it->FirstChildElement(kLimitsTag); if (limits_it) { - enable_limits = parse_bool(get_attribute_value(limits_it, kEnableTag, limits_it->Name())); + enable_limits = parse_bool(get_attribute_value(limits_it, kEnableAttribute, limits_it->Name())); } // Parse all command interfaces @@ -887,15 +887,15 @@ std::vector parse_control_resources_from_urdf(const std::string & continue; } joint_limits::JointLimits limits; - const bool has_hard_limts = getJointLimits(urdf_joint, limits); + const bool has_hard_limits = getJointLimits(urdf_joint, limits); if ( - !has_hard_limts && + !has_hard_limits && (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC || urdf_joint->type == urdf::Joint::CONTINUOUS)) { throw std::runtime_error("Missing URDF joint limits for the Joint : " + joint.name); } - if (has_hard_limts) + if (has_hard_limits) { // Take the most restricted one update_interface_limits(joint.command_interfaces, limits); From 2016885e3f91ef0e39e01010636d841be5cc84d2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 18:51:49 +0200 Subject: [PATCH 14/31] Add tests for the acceleration and jerk limits parsing --- .../test/test_component_parser.cpp | 28 +++++++++++++++++-- .../components_urdfs.hpp | 18 ++++++++++++ 2 files changed, 44 insertions(+), 2 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index a132f17d37..6720c88f73 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -32,6 +32,7 @@ class TestComponentParser : public Test void SetUp() override {} }; +using hardware_interface::HW_IF_ACCELERATION; using hardware_interface::HW_IF_EFFORT; using hardware_interface::HW_IF_POSITION; using hardware_interface::HW_IF_VELOCITY; @@ -805,13 +806,22 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); - EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(2)); + EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5)); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1); EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); @@ -819,7 +829,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_EQ(hardware_info.joints[1].name, "joint2"); EXPECT_EQ(hardware_info.joints[1].type, "joint"); - EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(3)); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5)); EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); @@ -829,6 +839,12 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1); EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); @@ -859,8 +875,15 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in DoubleNear(std::numeric_limits::min(), 1e-5)); EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); EXPECT_THAT( @@ -873,6 +896,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + 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)); } diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 59f032a298..2c739a9c85 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -419,6 +419,17 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface -0.05 0.1 + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + @@ -429,6 +440,13 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface + + -0.3 + 0.3 + + + 2.0 + From 78c80e8a10291c77dbed6ea517133c4f359abc1e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 18:52:08 +0200 Subject: [PATCH 15/31] Fix minor bug in the acceleration and deceleration signs --- hardware_interface/src/component_parser.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index cfa47f54ef..96085533a9 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -740,11 +740,12 @@ std::vector parse_control_resources_from_urdf(const std::string & { if (std::isfinite(max_decel)) { - limits.max_deceleration = max_decel; + limits.max_deceleration = std::fabs(max_decel); if (!std::isfinite(max_accel)) { limits.max_acceleration = std::fabs(limits.max_deceleration); } + limits.has_deceleration_limits = true && itr.enable_limits; } if (std::isfinite(max_accel)) { @@ -752,10 +753,10 @@ std::vector parse_control_resources_from_urdf(const std::string & if (!std::isfinite(limits.max_deceleration)) { - limits.max_deceleration = -limits.max_acceleration; + limits.max_deceleration = std::fabs(limits.max_acceleration); } + limits.has_acceleration_limits = true && itr.enable_limits; } - limits.has_acceleration_limits = true && itr.enable_limits; } } else if (itr.name == "jerk") From 98c95fe007a99b9a170468109764b907176a0202 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 19:14:07 +0200 Subject: [PATCH 16/31] Add tests for specific use conditions --- .../test/test_component_parser.cpp | 23 +++++++++++++++++-- .../components_urdfs.hpp | 6 +++++ 2 files changed, 27 insertions(+), 2 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 6720c88f73..21ddf12918 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -802,7 +802,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); - ASSERT_THAT(hardware_info.joints, SizeIs(2)); + ASSERT_THAT(hardware_info.joints, SizeIs(3)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); @@ -850,6 +850,13 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[2].name, "joint3"); + EXPECT_EQ(hardware_info.joints[2].type, "joint"); + EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); @@ -880,7 +887,6 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); - EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); @@ -899,6 +905,19 @@ 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_TRUE(hardware_info.limits.at("joint3").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); } TEST_F(TestComponentParser, successfully_parse_parameter_empty) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 2c739a9c85..4a79654bf8 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -445,10 +445,16 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface 0.3 + -2.0 2.0 + + + 1.0 + + From 18fae5323fa3876003326c4651c4d0423b0c1789 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 22 Apr 2024 21:59:04 +0000 Subject: [PATCH 17/31] Format error strings --- hardware_interface/src/component_parser.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 96085533a9..04905ea87f 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -679,9 +679,9 @@ std::vector parse_control_resources_from_urdf(const std::string & } catch (const std::invalid_argument & err) { - std::cerr << "Error parsing the limits for the interface : " << itf.name - << " from the tags [" << kMinTag << " : '" << itf.min << "' and " << kMaxTag - << " : '" << itf.max << "'] within " << kROS2ControlTag + std::cerr << "Error parsing the limits for the interface: " << itf.name + << " from the tags [" << kMinTag << ": '" << itf.min << "' and " << kMaxTag + << ": '" << itf.max << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" << std::endl; return false; } @@ -763,8 +763,8 @@ std::vector parse_control_resources_from_urdf(const std::string & { if (!itr.min.empty()) { - std::cerr << "Error parsing the limits for the interface : " << itr.name - << " from the tag : " << kMinTag << " within " << kROS2ControlTag + std::cerr << "Error parsing the limits for the interface: " << itr.name + << " from the tag: " << kMinTag << " within " << kROS2ControlTag << " tag inside the URDF. Jerk only accepts max limits." << std::endl; } double min_jerk(std::numeric_limits::quiet_NaN()), @@ -782,10 +782,10 @@ std::vector parse_control_resources_from_urdf(const std::string & { if (!itr.min.empty() || !itr.max.empty()) { - std::cerr << "Unable to parse the limits for the interface : " << itr.name + std::cerr << "Unable to parse the limits for the interface: " << itr.name << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " << kROS2ControlTag - << " tag inside the URDF. Supported interfaces for joint limits are : " + << " tag inside the URDF. Supported interfaces for joint limits are: " "position, velocity, effort, acceleration and jerk." << std::endl; } @@ -894,7 +894,7 @@ std::vector parse_control_resources_from_urdf(const std::string & (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC || urdf_joint->type == urdf::Joint::CONTINUOUS)) { - throw std::runtime_error("Missing URDF joint limits for the Joint : " + joint.name); + throw std::runtime_error("Missing URDF joint limits for the Joint: " + joint.name); } if (has_hard_limits) { From 6ac3140ed7fc7857be11f657b3079c68f0cb59ef Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 22 Apr 2024 22:00:59 +0000 Subject: [PATCH 18/31] Add comment to remove duplicate code --- hardware_interface/src/component_parser.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 04905ea87f..411a0b5aaf 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -769,13 +769,12 @@ std::vector parse_control_resources_from_urdf(const std::string & } double min_jerk(std::numeric_limits::quiet_NaN()), max_jerk(std::numeric_limits::quiet_NaN()); - if (!itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk)) + if ( + !itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) { - if (std::isfinite(max_jerk)) - { - limits.max_jerk = std::abs(max_jerk); - limits.has_jerk_limits = true && itr.enable_limits; - } + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = true && itr.enable_limits; } } else @@ -880,6 +879,8 @@ std::vector parse_control_resources_from_urdf(const std::string & hw_info.mimic_joints.push_back(mimic_joint); } } + // TODO(christophfroehlich) remove this code if deprecated mimic attribute - branch + // from above is removed (double check here, but throws already above if not found in URDF) auto urdf_joint = model.getJoint(joint.name); if (!urdf_joint) { From a4d98b15a899e2e6d8a8bd48b8db02c4cfa6dea4 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 22 Apr 2024 22:01:31 +0000 Subject: [PATCH 19/31] Add another test with different min/max values --- .../test/test_component_parser.cpp | 134 ++++++++++++++++++ .../components_urdfs.hpp | 62 ++++++++ 2 files changed, 196 insertions(+) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 21ddf12918..1717e37936 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -920,6 +920,140 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces_2) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets:: + valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits_2 + + ros2_control_test_assets::urdf_tail; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + EXPECT_EQ(hardware_info.name, "RRBotSystemWithGPIO"); + EXPECT_EQ(hardware_info.type, "system"); + EXPECT_EQ( + hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + + ASSERT_THAT(hardware_info.joints, SizeIs(3)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[0].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[0].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[1].name, "joint2"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_THAT(hardware_info.joints[1].command_interfaces, SizeIs(5)); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[0].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].name, HW_IF_VELOCITY); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[1].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].name, HW_IF_EFFORT); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[2].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[3].size, 1); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].name, "jerk"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].command_interfaces[4].size, 1); + EXPECT_THAT(hardware_info.joints[1].state_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].name, HW_IF_POSITION); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[1].state_interfaces[0].size, 1); + + EXPECT_EQ(hardware_info.joints[2].name, "joint3"); + EXPECT_EQ(hardware_info.joints[2].type, "joint"); + EXPECT_THAT(hardware_info.joints[2].command_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].name, HW_IF_ACCELERATION); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); + + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1)); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool"); + EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2); + EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2)); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat"); + EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); + + EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint1").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint1").min_position, + DoubleNear(std::numeric_limits::min(), 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_THAT( + hardware_info.limits.at("joint2").max_position, + DoubleNear(std::numeric_limits::max(), 1e-5)); + EXPECT_THAT( + hardware_info.limits.at("joint2").min_position, + DoubleNear(std::numeric_limits::min(), 1e-5)); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + 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_TRUE(hardware_info.limits.at("joint3").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").min_position, DoubleNear(-M_PI, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_position, DoubleNear(M_PI, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint3").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint3").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint3").max_velocity, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_effort, DoubleNear(0.1, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_acceleration, DoubleNear(1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 4a79654bf8..07ce9b5761 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -462,6 +462,68 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface )"; +const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits_2 = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + -1 + 1 + + + + + -0.3 + 0.3 + + + + + + + 1.0 + + + -0.3 + 0.3 + + + + + + + + +)"; const auto valid_urdf_ros2_control_parameter_empty = R"( From 078415228bf7ce0a0e79859549a8d1112df1c3d2 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 22 Apr 2024 22:12:05 +0000 Subject: [PATCH 20/31] Rename tests --- hardware_interface/test/test_component_parser.cpp | 5 ++--- .../include/ros2_control_test_assets/components_urdfs.hpp | 7 +------ 2 files changed, 3 insertions(+), 9 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 1717e37936..637355c905 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -920,12 +920,11 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); } -TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_interfaces_2) +TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable_interfaces) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + - ros2_control_test_assets:: - valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits_2 + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces + ros2_control_test_assets::urdf_tail; const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); ASSERT_THAT(control_hardware, SizeIs(1)); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 07ce9b5761..e2d044244d 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -462,7 +462,7 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface )"; -const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface_limits_2 = +const auto valid_urdf_ros2_control_system_robot_with_unavailable_interfaces = R"( @@ -517,11 +517,6 @@ const auto valid_urdf_ros2_control_system_robot_with_gpio_and_disabled_interface 0.3 - - - - - )"; From 5e8f3f0c5c75d814c641217c149f1d9160c910c3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Apr 2024 08:47:55 +0200 Subject: [PATCH 21/31] Fix the missing GPIOs part in the tests --- .../test/test_component_parser.cpp | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 637355c905..0244501450 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -990,21 +990,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].data_type, "double"); EXPECT_EQ(hardware_info.joints[2].command_interfaces[0].size, 1); - ASSERT_THAT(hardware_info.gpios, SizeIs(1)); - - EXPECT_EQ(hardware_info.gpios[0].name, "flange_IOS"); - EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); - EXPECT_THAT(hardware_info.gpios[0].command_interfaces, SizeIs(1)); - EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].name, "digital_output"); - EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].data_type, "bool"); - EXPECT_EQ(hardware_info.gpios[0].command_interfaces[0].size, 2); - EXPECT_THAT(hardware_info.gpios[0].state_interfaces, SizeIs(2)); - EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].name, "analog_input"); - EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].data_type, "double"); - EXPECT_EQ(hardware_info.gpios[0].state_interfaces[0].size, 3); - EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].name, "image"); - EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].data_type, "cv::Mat"); - EXPECT_EQ(hardware_info.gpios[0].state_interfaces[1].size, 1); + ASSERT_THAT(hardware_info.gpios, SizeIs(0)); EXPECT_FALSE(hardware_info.limits.at("joint1").has_position_limits); EXPECT_THAT( From 2f6cf66e47bc88afb15b3d570a7bd57f2e40ffc6 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Apr 2024 13:38:24 +0000 Subject: [PATCH 22/31] Remove unnecessary check and add test if it still throws --- hardware_interface/src/component_parser.cpp | 10 +- .../test/test_component_parser.cpp | 9 ++ .../ros2_control_test_assets/descriptions.hpp | 132 ++++++++++++++++-- 3 files changed, 134 insertions(+), 17 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 411a0b5aaf..ce604b702b 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -889,15 +889,7 @@ std::vector parse_control_resources_from_urdf(const std::string & continue; } joint_limits::JointLimits limits; - const bool has_hard_limits = getJointLimits(urdf_joint, limits); - if ( - !has_hard_limits && - (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC || - urdf_joint->type == urdf::Joint::CONTINUOUS)) - { - throw std::runtime_error("Missing URDF joint limits for the Joint: " + joint.name); - } - if (has_hard_limits) + if (getJointLimits(urdf_joint, limits)) { // Take the most restricted one update_interface_limits(joint.command_interfaces, limits); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 0244501450..d43e7a1910 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1039,6 +1039,15 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable EXPECT_THAT(hardware_info.limits.at("joint3").max_deceleration, DoubleNear(1.0, 1e-5)); } +TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = 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 3511a32fc8..1871cc7096 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 @@ -23,10 +23,6 @@ namespace ros2_control_test_assets const auto urdf_head = R"( - - - - @@ -149,13 +145,133 @@ const auto urdf_head = )"; +const auto urdf_head_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; const auto urdf_head_mimic = R"( - - - - From 611e60ae1df952acbb57f724491cf0fb4b78e87c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Apr 2024 14:01:50 +0000 Subject: [PATCH 23/31] Test if it throws on revolute+prismatic without limits it does not throw on continuous --- .../test/test_component_parser.cpp | 18 +- .../ros2_control_test_assets/descriptions.hpp | 167 ++++++++++++++++-- 2 files changed, 173 insertions(+), 12 deletions(-) diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index d43e7a1910..5d735d112b 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1042,10 +1042,24 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits) { std::string urdf_to_test = - std::string(ros2_control_test_assets::urdf_head_missing_limits) + - ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_unavailable_interfaces + + std::string(ros2_control_test_assets::urdf_head_revolute_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + ros2_control_test_assets::urdf_tail; EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); + + urdf_to_test = std::string(ros2_control_test_assets::urdf_head_prismatic_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + +TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); } TEST_F(TestComponentParser, successfully_parse_parameter_empty) 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 1871cc7096..df77d63359 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 @@ -145,7 +145,8 @@ const auto urdf_head = )"; -const auto urdf_head_missing_limits = + +const auto urdf_head_revolute_missing_limits = R"( @@ -233,13 +234,75 @@ const auto urdf_head_missing_limits = - +)"; + +const auto urdf_head_continuous_missing_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + - + @@ -261,14 +324,98 @@ const auto urdf_head_missing_limits = - - - - +)"; + +const auto urdf_head_prismatic_missing_limits = + R"( + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + )"; + const auto urdf_head_mimic = R"( From 3cfce6be7981c803221c922ebf2c3b990c9ec4ae Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Apr 2024 14:45:56 +0000 Subject: [PATCH 24/31] Fix continuous joint limits --- hardware_interface/src/component_parser.cpp | 172 +++++++++++++++--- .../test/test_component_parser.cpp | 29 ++- .../components_urdfs.hpp | 40 ++++ .../ros2_control_test_assets/descriptions.hpp | 3 +- 4 files changed, 213 insertions(+), 31 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index ce604b702b..22de610cbb 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -654,38 +654,39 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } - // Retrieve the limits from ros2_control command interface tags and override if restrictive - auto update_interface_limits = [](const auto & interfaces, joint_limits::JointLimits & limits) + auto retrieve_min_max_interface_values = [](const auto & itf, double & min, double & max) -> bool { - auto retrieve_min_max_interface_values = []( - const auto & itf, double & min, double & max) -> bool + try { - try + if (itf.min.empty() && itf.max.empty()) { - if (itf.min.empty() && itf.max.empty()) - { - // If the limits don't exist then return false as they are not retrieved - return false; - } - if (!itf.min.empty()) - { - min = hardware_interface::stod(itf.min); - } - if (!itf.max.empty()) - { - max = hardware_interface::stod(itf.max); - } - return true; + // If the limits don't exist then return false as they are not retrieved + return false; } - catch (const std::invalid_argument & err) + if (!itf.min.empty()) { - std::cerr << "Error parsing the limits for the interface: " << itf.name - << " from the tags [" << kMinTag << ": '" << itf.min << "' and " << kMaxTag - << ": '" << itf.max << "'] within " << kROS2ControlTag - << " tag inside the URDF. Skipping it" << std::endl; - return false; + min = hardware_interface::stod(itf.min); } - }; + if (!itf.max.empty()) + { + max = hardware_interface::stod(itf.max); + } + return true; + } + catch (const std::invalid_argument & err) + { + std::cerr << "Error parsing the limits for the interface: " << itf.name << " from the tags [" + << kMinTag << ": '" << itf.min << "' and " << kMaxTag << ": '" << itf.max + << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" + << std::endl; + return false; + } + }; + + // Retrieve the limits from ros2_control command interface tags and override if restrictive + auto update_interface_limits = + [retrieve_min_max_interface_values](const auto & interfaces, joint_limits::JointLimits & limits) + { for (auto & itr : interfaces) { if (itr.name == hardware_interface::HW_IF_POSITION) @@ -792,6 +793,116 @@ std::vector parse_control_resources_from_urdf(const std::string & } }; + // Retrieve the limits from ros2_control command interface tags + auto copy_interface_limits = + [retrieve_min_max_interface_values](const auto & interfaces, joint_limits::JointLimits & limits) + { + for (auto & itr : interfaces) + { + if (itr.name == hardware_interface::HW_IF_POSITION) + { + double min_pos, max_pos; + if (retrieve_min_max_interface_values(itr, min_pos, max_pos)) + { + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + limits.has_position_limits = true && itr.enable_limits; + } + else + { + limits.min_position = std::numeric_limits::min(); + limits.max_position = std::numeric_limits::max(); + limits.has_position_limits = false; + } + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + double min_vel, max_vel; + if (retrieve_min_max_interface_values(itr, min_vel, max_vel)) + { + limits.max_velocity = std::min(std::abs(min_vel), max_vel); + limits.has_velocity_limits = true && itr.enable_limits; + } + else + { + limits.max_velocity = std::numeric_limits::max(); + limits.has_velocity_limits = false; + } + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + double min_eff, max_eff; + if (retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + limits.max_effort = std::min(std::abs(min_eff), max_eff); + limits.has_effort_limits = true && itr.enable_limits; + } + else + { + limits.max_effort = std::numeric_limits::max(); + limits.has_effort_limits = false; + } + } + else if (itr.name == hardware_interface::HW_IF_ACCELERATION) + { + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (retrieve_min_max_interface_values(itr, max_decel, max_accel)) + { + if (std::isfinite(max_decel)) + { + limits.max_deceleration = std::fabs(max_decel); + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + limits.has_deceleration_limits = true && itr.enable_limits; + } + if (std::isfinite(max_accel)) + { + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = std::fabs(limits.max_acceleration); + } + limits.has_acceleration_limits = true && itr.enable_limits; + } + } + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + std::cerr << "Error parsing the limits for the interface: " << itr.name + << " from the tag: " << kMinTag << " within " << kROS2ControlTag + << " tag inside the URDF. Jerk only accepts max limits." << std::endl; + } + double min_jerk(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if ( + !itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) + { + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = true && itr.enable_limits; + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + std::cerr << "Unable to parse the limits for the interface: " << itr.name + << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are: " + "position, velocity, effort, acceleration and jerk." + << std::endl; + } + } + } + }; + // parse full URDF for mimic options urdf::Model model; if (!model.initString(urdf)) @@ -884,7 +995,7 @@ std::vector parse_control_resources_from_urdf(const std::string & auto urdf_joint = model.getJoint(joint.name); if (!urdf_joint) { - std::cerr << "Joint : '" + joint.name + "' not found in URDF. Skipping limits parsing!" + std::cerr << "Joint: '" + joint.name + "' not found in URDF. Skipping limits parsing!" << std::endl; continue; } @@ -893,8 +1004,13 @@ std::vector parse_control_resources_from_urdf(const std::string & { // Take the most restricted one update_interface_limits(joint.command_interfaces, limits); - hw_info.limits[joint.name] = limits; } + else + { + // valid for continuous-joint type + copy_interface_limits(joint.command_interfaces, limits); + } + hw_info.limits[joint.name] = limits; } } diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 5d735d112b..ef408920ee 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1057,9 +1057,36 @@ TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_lim { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) + - ros2_control_test_assets::valid_urdf_ros2_control_system_one_interface + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + ros2_control_test_assets::urdf_tail; EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } TEST_F(TestComponentParser, successfully_parse_parameter_empty) diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index e2d044244d..762f2dd98a 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -519,6 +519,46 @@ const auto valid_urdf_ros2_control_system_robot_with_unavailable_interfaces = )"; +const auto valid_urdf_ros2_control_system_robot_with_all_interfaces = + R"( + + + ros2_control_demo_hardware/RRBotSystemWithGPIOHardware + 2 + 2 + + + + -1 + 1 + + + -0.05 + 0.1 + + + -0.2 + 0.2 + + + -0.5 + 0.5 + + + 5.0 + + + + + + + + + + + + +)"; const auto valid_urdf_ros2_control_parameter_empty = R"( 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 df77d63359..9c2522561d 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 @@ -296,11 +296,10 @@ const auto urdf_head_continuous_missing_limits = - + - From 9b21ba460bf30db0a4af5813799233af7dfc501d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Apr 2024 15:45:44 +0000 Subject: [PATCH 25/31] Successfully parse continuous joint limits if set in URDF --- hardware_interface/src/component_parser.cpp | 10 +- .../test/test_component_parser.cpp | 50 +++++++++- .../ros2_control_test_assets/descriptions.hpp | 91 +++++++++++++++++++ 3 files changed, 145 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 22de610cbb..1098eb1217 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -691,11 +691,15 @@ std::vector parse_control_resources_from_urdf(const std::string & { if (itr.name == hardware_interface::HW_IF_POSITION) { + double min_pos(limits.min_position), max_pos(limits.max_position); + bool has_min_max_interface_values = + retrieve_min_max_interface_values(itr, min_pos, max_pos); + // position_limits can be restricted for continuous joints + limits.has_position_limits |= has_min_max_interface_values; limits.has_position_limits &= itr.enable_limits; if (limits.has_position_limits) { - double min_pos(limits.min_position), max_pos(limits.max_position); - if (retrieve_min_max_interface_values(itr, min_pos, max_pos)) + if (has_min_max_interface_values) { limits.min_position = std::max(min_pos, limits.min_position); limits.max_position = std::min(max_pos, limits.max_position); @@ -1007,7 +1011,7 @@ std::vector parse_control_resources_from_urdf(const std::string & } else { - // valid for continuous-joint type + // valid for continuous-joint type only copy_interface_limits(joint.command_interfaces, limits); } hw_info.limits[joint.name] = limits; diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index ef408920ee..68752e92d5 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1053,7 +1053,7 @@ TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits) EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } -TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_limits) +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head_continuous_missing_limits) + @@ -1074,8 +1074,10 @@ TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_lim EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); - EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)); - EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)) + << "velocity constraint from ros2_control_tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.2, 1e-5)) + << "effort constraint from ros2_control_tag"; EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); @@ -1089,6 +1091,48 @@ TEST_F(TestComponentParser, no_throw_on_parse_urdf_system_continuous_missing_lim EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } +TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limits) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head_continuous_with_limits) + + ros2_control_test_assets::valid_urdf_ros2_control_system_robot_with_all_interfaces + + ros2_control_test_assets::urdf_tail; + EXPECT_NO_THROW(parse_control_resources_from_urdf(urdf_to_test)); + + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(1)); + auto hardware_info = control_hardware.front(); + + ASSERT_GT(hardware_info.limits.count("joint1"), 0); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_position_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").min_position, DoubleNear(-1.0, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_position, DoubleNear(1.0, 1e-5)); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_deceleration_limits); + EXPECT_TRUE(hardware_info.limits.at("joint1").has_jerk_limits); + EXPECT_THAT(hardware_info.limits.at("joint1").max_velocity, DoubleNear(0.05, 1e-5)) + << "velocity URDF constraint overridden by ros2_control tag"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint1").max_acceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_deceleration, DoubleNear(0.5, 1e-5)); + EXPECT_THAT(hardware_info.limits.at("joint1").max_jerk, DoubleNear(5.0, 1e-5)); + + ASSERT_GT(hardware_info.limits.count("joint2"), 0); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_position_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_velocity_limits); + EXPECT_TRUE(hardware_info.limits.at("joint2").has_effort_limits); + EXPECT_THAT(hardware_info.limits.at("joint2").max_velocity, DoubleNear(0.2, 1e-5)) + << "velocity constraint from URDF"; + EXPECT_THAT(hardware_info.limits.at("joint2").max_effort, DoubleNear(0.1, 1e-5)) + << "effort constraint from URDF"; + EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_deceleration_limits); + EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = 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 9c2522561d..f5f668fd12 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 @@ -325,6 +325,97 @@ const auto urdf_head_continuous_missing_limits = )"; +const auto urdf_head_continuous_with_limits = + R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + const auto urdf_head_prismatic_missing_limits = R"( From 74e22b9bc8e7865bfa49b4d3b5051fd8f1d3566f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 23 Apr 2024 17:24:45 +0000 Subject: [PATCH 26/31] Throw if trying to configure a fixed joint --- hardware_interface/src/component_parser.cpp | 7 +++++++ hardware_interface/test/test_component_parser.cpp | 9 +++++++++ .../ros2_control_test_assets/components_urdfs.hpp | 15 +++++++++++++++ 3 files changed, 31 insertions(+) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 1098eb1217..f1245e8fc6 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -1003,6 +1003,13 @@ std::vector parse_control_resources_from_urdf(const std::string & << std::endl; continue; } + if (urdf_joint->type == urdf::Joint::FIXED) + { + throw std::runtime_error( + "Joint '" + joint.name + + "' is of type 'fixed'. " + "Fixed joints do not make sense in ros2_control."); + } joint_limits::JointLimits limits; if (getJointLimits(urdf_joint, limits)) { diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 68752e92d5..407a8242c4 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -1053,6 +1053,15 @@ TEST_F(TestComponentParser, throw_on_parse_invalid_urdf_system_missing_limits) EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); } +TEST_F(TestComponentParser, throw_on_parse_urdf_system_with_command_fixed_joint) +{ + std::string urdf_to_test = + std::string(ros2_control_test_assets::urdf_head) + + ros2_control_test_assets::invalid_urdf_ros2_control_system_with_command_fixed_joint + + ros2_control_test_assets::urdf_tail; + EXPECT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_missing_limits) { std::string urdf_to_test = diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp index 762f2dd98a..7b46eda9c0 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/components_urdfs.hpp @@ -727,6 +727,21 @@ const auto invalid_urdf2_transmission_given_too_many_joints = )"; +const auto invalid_urdf_ros2_control_system_with_command_fixed_joint = + R"( + + + ros2_control_demo_hardware/2DOF_System_Hardware_Position_Only + 2 + 2 + + + + + + +)"; + } // namespace ros2_control_test_assets #endif // ROS2_CONTROL_TEST_ASSETS__COMPONENTS_URDFS_HPP_ From b414970e14c1a5291a6be7c85c71e12a0c24b2f1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 24 Apr 2024 15:46:22 +0000 Subject: [PATCH 27/31] Remove duplicate code --- hardware_interface/src/component_parser.cpp | 187 ++++++++------------ 1 file changed, 73 insertions(+), 114 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index f1245e8fc6..f19ae909c0 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -654,6 +654,7 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } + // get min/max from ros2_control tag auto retrieve_min_max_interface_values = [](const auto & itf, double & min, double & max) -> bool { try @@ -683,9 +684,73 @@ std::vector parse_control_resources_from_urdf(const std::string & } }; - // Retrieve the limits from ros2_control command interface tags and override if restrictive - auto update_interface_limits = - [retrieve_min_max_interface_values](const auto & interfaces, joint_limits::JointLimits & limits) + // set custom values for acceleration and jerk limits (no URDF standard) + auto set_custom_interface_values = + [retrieve_min_max_interface_values](const auto & itr, joint_limits::JointLimits & limits) + { + if (itr.name == hardware_interface::HW_IF_ACCELERATION) + { + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (retrieve_min_max_interface_values(itr, max_decel, max_accel)) + { + if (std::isfinite(max_decel)) + { + limits.max_deceleration = std::fabs(max_decel); + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + limits.has_deceleration_limits = true && itr.enable_limits; + } + if (std::isfinite(max_accel)) + { + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = std::fabs(limits.max_acceleration); + } + limits.has_acceleration_limits = true && itr.enable_limits; + } + } + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + std::cerr << "Error parsing the limits for the interface: " << itr.name + << " from the tag: " << kMinTag << " within " << kROS2ControlTag + << " tag inside the URDF. Jerk only accepts max limits." << std::endl; + } + double min_jerk(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if ( + !itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) + { + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = true && itr.enable_limits; + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + std::cerr << "Unable to parse the limits for the interface: " << itr.name + << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are: " + "position, velocity, effort, acceleration and jerk." + << std::endl; + } + } + }; + + // Retrieve the limits from ros2_control command interface tags and override URDF limits if + // restrictive + auto update_interface_limits = [retrieve_min_max_interface_values, set_custom_interface_values]( + const auto & interfaces, joint_limits::JointLimits & limits) { for (auto & itr : interfaces) { @@ -737,69 +802,16 @@ std::vector parse_control_resources_from_urdf(const std::string & } } } - else if (itr.name == hardware_interface::HW_IF_ACCELERATION) - { - double max_decel(std::numeric_limits::quiet_NaN()), - max_accel(std::numeric_limits::quiet_NaN()); - if (retrieve_min_max_interface_values(itr, max_decel, max_accel)) - { - if (std::isfinite(max_decel)) - { - limits.max_deceleration = std::fabs(max_decel); - if (!std::isfinite(max_accel)) - { - limits.max_acceleration = std::fabs(limits.max_deceleration); - } - limits.has_deceleration_limits = true && itr.enable_limits; - } - if (std::isfinite(max_accel)) - { - limits.max_acceleration = max_accel; - - if (!std::isfinite(limits.max_deceleration)) - { - limits.max_deceleration = std::fabs(limits.max_acceleration); - } - limits.has_acceleration_limits = true && itr.enable_limits; - } - } - } - else if (itr.name == "jerk") - { - if (!itr.min.empty()) - { - std::cerr << "Error parsing the limits for the interface: " << itr.name - << " from the tag: " << kMinTag << " within " << kROS2ControlTag - << " tag inside the URDF. Jerk only accepts max limits." << std::endl; - } - double min_jerk(std::numeric_limits::quiet_NaN()), - max_jerk(std::numeric_limits::quiet_NaN()); - if ( - !itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && - std::isfinite(max_jerk)) - { - limits.max_jerk = std::abs(max_jerk); - limits.has_jerk_limits = true && itr.enable_limits; - } - } else { - if (!itr.min.empty() || !itr.max.empty()) - { - std::cerr << "Unable to parse the limits for the interface: " << itr.name - << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " - << kROS2ControlTag - << " tag inside the URDF. Supported interfaces for joint limits are: " - "position, velocity, effort, acceleration and jerk." - << std::endl; - } + set_custom_interface_values(itr, limits); } } }; - // Retrieve the limits from ros2_control command interface tags - auto copy_interface_limits = - [retrieve_min_max_interface_values](const auto & interfaces, joint_limits::JointLimits & limits) + // Copy the limits from ros2_control command interface tags if not URDF-limit tag is present + auto copy_interface_limits = [retrieve_min_max_interface_values, set_custom_interface_values]( + const auto & interfaces, joint_limits::JointLimits & limits) { for (auto & itr : interfaces) { @@ -847,62 +859,9 @@ std::vector parse_control_resources_from_urdf(const std::string & limits.has_effort_limits = false; } } - else if (itr.name == hardware_interface::HW_IF_ACCELERATION) - { - double max_decel(std::numeric_limits::quiet_NaN()), - max_accel(std::numeric_limits::quiet_NaN()); - if (retrieve_min_max_interface_values(itr, max_decel, max_accel)) - { - if (std::isfinite(max_decel)) - { - limits.max_deceleration = std::fabs(max_decel); - if (!std::isfinite(max_accel)) - { - limits.max_acceleration = std::fabs(limits.max_deceleration); - } - limits.has_deceleration_limits = true && itr.enable_limits; - } - if (std::isfinite(max_accel)) - { - limits.max_acceleration = max_accel; - - if (!std::isfinite(limits.max_deceleration)) - { - limits.max_deceleration = std::fabs(limits.max_acceleration); - } - limits.has_acceleration_limits = true && itr.enable_limits; - } - } - } - else if (itr.name == "jerk") - { - if (!itr.min.empty()) - { - std::cerr << "Error parsing the limits for the interface: " << itr.name - << " from the tag: " << kMinTag << " within " << kROS2ControlTag - << " tag inside the URDF. Jerk only accepts max limits." << std::endl; - } - double min_jerk(std::numeric_limits::quiet_NaN()), - max_jerk(std::numeric_limits::quiet_NaN()); - if ( - !itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && - std::isfinite(max_jerk)) - { - limits.max_jerk = std::abs(max_jerk); - limits.has_jerk_limits = true && itr.enable_limits; - } - } else { - if (!itr.min.empty() || !itr.max.empty()) - { - std::cerr << "Unable to parse the limits for the interface: " << itr.name - << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " - << kROS2ControlTag - << " tag inside the URDF. Supported interfaces for joint limits are: " - "position, velocity, effort, acceleration and jerk." - << std::endl; - } + set_custom_interface_values(itr, limits); } } }; From 48bbef385ce7b9fb03165e95c2901e679ca363ed Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 3 May 2024 15:35:09 +0200 Subject: [PATCH 28/31] separate the lambdas into isolated helper methods --- hardware_interface/src/component_parser.cpp | 419 ++++++++++---------- 1 file changed, 218 insertions(+), 201 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index f19ae909c0..f03e6e4e76 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -614,257 +614,274 @@ HardwareInfo parse_resource_from_xml( return hardware; } -} // namespace detail - -std::vector parse_control_resources_from_urdf(const std::string & urdf) +/** + * @brief Retrieve the min and max values from the interface tag. + * @param itf The interface tag to retrieve the values from. + * @param min The minimum value to be retrieved. + * @param max The maximum value to be retrieved. + * @return true if the values are retrieved, false otherwise. + */ +bool retrieve_min_max_interface_values(const InterfaceInfo & itf, double & min, double & max) { - // Check if everything OK with URDF string - if (urdf.empty()) - { - throw std::runtime_error("empty URDF passed to robot"); - } - tinyxml2::XMLDocument doc; - if (!doc.Parse(urdf.c_str()) && doc.Error()) - { - throw std::runtime_error("invalid URDF passed in to robot parser"); - } - if (doc.Error()) - { - throw std::runtime_error("invalid URDF passed in to robot parser"); - } - - // Find robot tag - const tinyxml2::XMLElement * robot_it = doc.RootElement(); - - if (std::string(kRobotTag) != robot_it->Name()) + try { - throw std::runtime_error("the robot tag is not root element in URDF"); - } - - const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); - if (!ros2_control_it) - { - throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); + if (itf.min.empty() && itf.max.empty()) + { + // If the limits don't exist then return false as they are not retrieved + return false; + } + if (!itf.min.empty()) + { + min = hardware_interface::stod(itf.min); + } + if (!itf.max.empty()) + { + max = hardware_interface::stod(itf.max); + } + return true; } - - std::vector hardware_info; - while (ros2_control_it) + catch (const std::invalid_argument & err) { - hardware_info.push_back(detail::parse_resource_from_xml(ros2_control_it, urdf)); - ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); + std::cerr << "Error parsing the limits for the interface: " << itf.name << " from the tags [" + << kMinTag << ": '" << itf.min << "' and " << kMaxTag << ": '" << itf.max + << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" + << std::endl; + return false; } +} - // get min/max from ros2_control tag - auto retrieve_min_max_interface_values = [](const auto & itf, double & min, double & max) -> bool +/** + * @brief Set custom values for acceleration and jerk limits (no URDF standard) + * @param itr The interface tag to retrieve the values from. + * @param limits The joint limits to be set. + */ +void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointLimits & limits) +{ + if (itr.name == hardware_interface::HW_IF_ACCELERATION) { - try + double max_decel(std::numeric_limits::quiet_NaN()), + max_accel(std::numeric_limits::quiet_NaN()); + if (detail::retrieve_min_max_interface_values(itr, max_decel, max_accel)) { - if (itf.min.empty() && itf.max.empty()) - { - // If the limits don't exist then return false as they are not retrieved - return false; - } - if (!itf.min.empty()) + if (std::isfinite(max_decel)) { - min = hardware_interface::stod(itf.min); + limits.max_deceleration = std::fabs(max_decel); + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } + limits.has_deceleration_limits = true && itr.enable_limits; } - if (!itf.max.empty()) + if (std::isfinite(max_accel)) { - max = hardware_interface::stod(itf.max); + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = std::fabs(limits.max_acceleration); + } + limits.has_acceleration_limits = true && itr.enable_limits; } - return true; } - catch (const std::invalid_argument & err) + } + else if (itr.name == "jerk") + { + if (!itr.min.empty()) + { + std::cerr << "Error parsing the limits for the interface: " << itr.name + << " from the tag: " << kMinTag << " within " << kROS2ControlTag + << " tag inside the URDF. Jerk only accepts max limits." << std::endl; + } + double min_jerk(std::numeric_limits::quiet_NaN()), + max_jerk(std::numeric_limits::quiet_NaN()); + if ( + !itr.max.empty() && detail::retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && + std::isfinite(max_jerk)) { - std::cerr << "Error parsing the limits for the interface: " << itf.name << " from the tags [" - << kMinTag << ": '" << itf.min << "' and " << kMaxTag << ": '" << itf.max - << "'] within " << kROS2ControlTag << " tag inside the URDF. Skipping it" + limits.max_jerk = std::abs(max_jerk); + limits.has_jerk_limits = true && itr.enable_limits; + } + } + else + { + if (!itr.min.empty() || !itr.max.empty()) + { + std::cerr << "Unable to parse the limits for the interface: " << itr.name + << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are: " + "position, velocity, effort, acceleration and jerk." << std::endl; - return false; } - }; + } +} - // set custom values for acceleration and jerk limits (no URDF standard) - auto set_custom_interface_values = - [retrieve_min_max_interface_values](const auto & itr, joint_limits::JointLimits & limits) +/** + * @brief Retrieve the limits from ros2_control command interface tags and override URDF limits if + * restrictive + * @param interfaces The interfaces to retrieve the limits from. + * @param limits The joint limits to be set. + */ +void update_interface_limits( + const std::vector & interfaces, joint_limits::JointLimits & limits) +{ + for (auto & itr : interfaces) { - if (itr.name == hardware_interface::HW_IF_ACCELERATION) + if (itr.name == hardware_interface::HW_IF_POSITION) { - double max_decel(std::numeric_limits::quiet_NaN()), - max_accel(std::numeric_limits::quiet_NaN()); - if (retrieve_min_max_interface_values(itr, max_decel, max_accel)) + double min_pos(limits.min_position), max_pos(limits.max_position); + bool has_min_max_interface_values = + detail::retrieve_min_max_interface_values(itr, min_pos, max_pos); + // position_limits can be restricted for continuous joints + limits.has_position_limits |= has_min_max_interface_values; + limits.has_position_limits &= itr.enable_limits; + if (limits.has_position_limits) { - if (std::isfinite(max_decel)) + if (has_min_max_interface_values) { - limits.max_deceleration = std::fabs(max_decel); - if (!std::isfinite(max_accel)) - { - limits.max_acceleration = std::fabs(limits.max_deceleration); - } - limits.has_deceleration_limits = true && itr.enable_limits; + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); } - if (std::isfinite(max_accel)) + } + else + { + limits.min_position = std::numeric_limits::min(); + limits.max_position = std::numeric_limits::max(); + } + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + limits.has_velocity_limits &= itr.enable_limits; + if (limits.has_velocity_limits) + { // Apply the most restrictive one in the case + double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); + if (detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) { - limits.max_acceleration = max_accel; - - if (!std::isfinite(limits.max_deceleration)) - { - limits.max_deceleration = std::fabs(limits.max_acceleration); - } - limits.has_acceleration_limits = true && itr.enable_limits; + max_vel = std::min(std::abs(min_vel), max_vel); + limits.max_velocity = std::min(max_vel, limits.max_velocity); } } } - else if (itr.name == "jerk") + else if (itr.name == hardware_interface::HW_IF_EFFORT) { - if (!itr.min.empty()) - { - std::cerr << "Error parsing the limits for the interface: " << itr.name - << " from the tag: " << kMinTag << " within " << kROS2ControlTag - << " tag inside the URDF. Jerk only accepts max limits." << std::endl; - } - double min_jerk(std::numeric_limits::quiet_NaN()), - max_jerk(std::numeric_limits::quiet_NaN()); - if ( - !itr.max.empty() && retrieve_min_max_interface_values(itr, min_jerk, max_jerk) && - std::isfinite(max_jerk)) - { - limits.max_jerk = std::abs(max_jerk); - limits.has_jerk_limits = true && itr.enable_limits; + limits.has_effort_limits &= itr.enable_limits; + if (limits.has_effort_limits) + { // Apply the most restrictive one in the case + double min_eff(-limits.max_effort), max_eff(limits.max_effort); + if (detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + max_eff = std::min(std::abs(min_eff), max_eff); + limits.max_effort = std::min(max_eff, limits.max_effort); + } } } else { - if (!itr.min.empty() || !itr.max.empty()) - { - std::cerr << "Unable to parse the limits for the interface: " << itr.name - << " from the tags [" << kMinTag << " and " << kMaxTag << "] within " - << kROS2ControlTag - << " tag inside the URDF. Supported interfaces for joint limits are: " - "position, velocity, effort, acceleration and jerk." - << std::endl; - } + detail::set_custom_interface_values(itr, limits); } - }; + } +} - // Retrieve the limits from ros2_control command interface tags and override URDF limits if - // restrictive - auto update_interface_limits = [retrieve_min_max_interface_values, set_custom_interface_values]( - const auto & interfaces, joint_limits::JointLimits & limits) +/** + * @brief Copy the limits from ros2_control command interface tags if not URDF-limit tag is present + * @param interfaces The interfaces to retrieve the limits from. + * @param limits The joint limits to be set. + */ +void copy_interface_limits( + const std::vector & interfaces, joint_limits::JointLimits & limits) +{ + for (auto & itr : interfaces) { - for (auto & itr : interfaces) + if (itr.name == hardware_interface::HW_IF_POSITION) { - if (itr.name == hardware_interface::HW_IF_POSITION) - { - double min_pos(limits.min_position), max_pos(limits.max_position); - bool has_min_max_interface_values = - retrieve_min_max_interface_values(itr, min_pos, max_pos); - // position_limits can be restricted for continuous joints - limits.has_position_limits |= has_min_max_interface_values; - limits.has_position_limits &= itr.enable_limits; - if (limits.has_position_limits) - { - if (has_min_max_interface_values) - { - limits.min_position = std::max(min_pos, limits.min_position); - limits.max_position = std::min(max_pos, limits.max_position); - } - } - else - { - limits.min_position = std::numeric_limits::min(); - limits.max_position = std::numeric_limits::max(); - } - } - else if (itr.name == hardware_interface::HW_IF_VELOCITY) + double min_pos, max_pos; + if (detail::retrieve_min_max_interface_values(itr, min_pos, max_pos)) { - limits.has_velocity_limits &= itr.enable_limits; - if (limits.has_velocity_limits) - { // Apply the most restrictive one in the case - double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); - if (retrieve_min_max_interface_values(itr, min_vel, max_vel)) - { - max_vel = std::min(std::abs(min_vel), max_vel); - limits.max_velocity = std::min(max_vel, limits.max_velocity); - } - } - } - else if (itr.name == hardware_interface::HW_IF_EFFORT) - { - limits.has_effort_limits &= itr.enable_limits; - if (limits.has_effort_limits) - { // Apply the most restrictive one in the case - double min_eff(-limits.max_effort), max_eff(limits.max_effort); - if (retrieve_min_max_interface_values(itr, min_eff, max_eff)) - { - max_eff = std::min(std::abs(min_eff), max_eff); - limits.max_effort = std::min(max_eff, limits.max_effort); - } - } + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + limits.has_position_limits = true && itr.enable_limits; } else { - set_custom_interface_values(itr, limits); + limits.min_position = std::numeric_limits::min(); + limits.max_position = std::numeric_limits::max(); + limits.has_position_limits = false; } } - }; - - // Copy the limits from ros2_control command interface tags if not URDF-limit tag is present - auto copy_interface_limits = [retrieve_min_max_interface_values, set_custom_interface_values]( - const auto & interfaces, joint_limits::JointLimits & limits) - { - for (auto & itr : interfaces) + else if (itr.name == hardware_interface::HW_IF_VELOCITY) { - if (itr.name == hardware_interface::HW_IF_POSITION) + double min_vel, max_vel; + if (detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) { - double min_pos, max_pos; - if (retrieve_min_max_interface_values(itr, min_pos, max_pos)) - { - limits.min_position = std::max(min_pos, limits.min_position); - limits.max_position = std::min(max_pos, limits.max_position); - limits.has_position_limits = true && itr.enable_limits; - } - else - { - limits.min_position = std::numeric_limits::min(); - limits.max_position = std::numeric_limits::max(); - limits.has_position_limits = false; - } + limits.max_velocity = std::min(std::abs(min_vel), max_vel); + limits.has_velocity_limits = true && itr.enable_limits; } - else if (itr.name == hardware_interface::HW_IF_VELOCITY) + else { - double min_vel, max_vel; - if (retrieve_min_max_interface_values(itr, min_vel, max_vel)) - { - limits.max_velocity = std::min(std::abs(min_vel), max_vel); - limits.has_velocity_limits = true && itr.enable_limits; - } - else - { - limits.max_velocity = std::numeric_limits::max(); - limits.has_velocity_limits = false; - } + limits.max_velocity = std::numeric_limits::max(); + limits.has_velocity_limits = false; } - else if (itr.name == hardware_interface::HW_IF_EFFORT) + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + double min_eff, max_eff; + if (detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) { - double min_eff, max_eff; - if (retrieve_min_max_interface_values(itr, min_eff, max_eff)) - { - limits.max_effort = std::min(std::abs(min_eff), max_eff); - limits.has_effort_limits = true && itr.enable_limits; - } - else - { - limits.max_effort = std::numeric_limits::max(); - limits.has_effort_limits = false; - } + limits.max_effort = std::min(std::abs(min_eff), max_eff); + limits.has_effort_limits = true && itr.enable_limits; } else { - set_custom_interface_values(itr, limits); + limits.max_effort = std::numeric_limits::max(); + limits.has_effort_limits = false; } } - }; + else + { + detail::set_custom_interface_values(itr, limits); + } + } +} + +} // namespace detail + +std::vector parse_control_resources_from_urdf(const std::string & urdf) +{ + // Check if everything OK with URDF string + if (urdf.empty()) + { + throw std::runtime_error("empty URDF passed to robot"); + } + tinyxml2::XMLDocument doc; + if (!doc.Parse(urdf.c_str()) && doc.Error()) + { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + if (doc.Error()) + { + throw std::runtime_error("invalid URDF passed in to robot parser"); + } + + // Find robot tag + const tinyxml2::XMLElement * robot_it = doc.RootElement(); + + if (std::string(kRobotTag) != robot_it->Name()) + { + throw std::runtime_error("the robot tag is not root element in URDF"); + } + + const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement(kROS2ControlTag); + if (!ros2_control_it) + { + throw std::runtime_error("no " + std::string(kROS2ControlTag) + " tag"); + } + + std::vector hardware_info; + while (ros2_control_it) + { + hardware_info.push_back(detail::parse_resource_from_xml(ros2_control_it, urdf)); + ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); + } // parse full URDF for mimic options urdf::Model model; @@ -973,12 +990,12 @@ std::vector parse_control_resources_from_urdf(const std::string & if (getJointLimits(urdf_joint, limits)) { // Take the most restricted one - update_interface_limits(joint.command_interfaces, limits); + detail::update_interface_limits(joint.command_interfaces, limits); } else { // valid for continuous-joint type only - copy_interface_limits(joint.command_interfaces, limits); + detail::copy_interface_limits(joint.command_interfaces, limits); } hw_info.limits[joint.name] = limits; } From 9d08858f6e7b5de6694c6df1eda0644a26254673 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 3 May 2024 15:38:19 +0200 Subject: [PATCH 29/31] fix the has_limits logic for different interfaces --- hardware_interface/src/component_parser.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index f03e6e4e76..e592acc7ab 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -670,7 +670,7 @@ void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointL { limits.max_acceleration = std::fabs(limits.max_deceleration); } - limits.has_deceleration_limits = true && itr.enable_limits; + limits.has_deceleration_limits = itr.enable_limits; } if (std::isfinite(max_accel)) { @@ -680,7 +680,7 @@ void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointL { limits.max_deceleration = std::fabs(limits.max_acceleration); } - limits.has_acceleration_limits = true && itr.enable_limits; + limits.has_acceleration_limits = itr.enable_limits; } } } @@ -699,7 +699,7 @@ void set_custom_interface_values(const InterfaceInfo & itr, joint_limits::JointL std::isfinite(max_jerk)) { limits.max_jerk = std::abs(max_jerk); - limits.has_jerk_limits = true && itr.enable_limits; + limits.has_jerk_limits = itr.enable_limits; } } else @@ -799,7 +799,7 @@ void copy_interface_limits( { limits.min_position = std::max(min_pos, limits.min_position); limits.max_position = std::min(max_pos, limits.max_position); - limits.has_position_limits = true && itr.enable_limits; + limits.has_position_limits = itr.enable_limits; } else { @@ -814,7 +814,7 @@ void copy_interface_limits( if (detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) { limits.max_velocity = std::min(std::abs(min_vel), max_vel); - limits.has_velocity_limits = true && itr.enable_limits; + limits.has_velocity_limits = itr.enable_limits; } else { @@ -828,7 +828,7 @@ void copy_interface_limits( if (detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) { limits.max_effort = std::min(std::abs(min_eff), max_eff); - limits.has_effort_limits = true && itr.enable_limits; + limits.has_effort_limits = itr.enable_limits; } else { From 616b4911d08142a91d82f4eb9c628717f9aafcaa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 3 May 2024 21:11:31 +0200 Subject: [PATCH 30/31] make update_interface_limits more generic and fix minor bug --- hardware_interface/src/component_parser.cpp | 65 +++++++++---------- .../test/test_component_parser.cpp | 8 +-- 2 files changed, 35 insertions(+), 38 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index e592acc7ab..6309764ecc 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -729,51 +729,48 @@ void update_interface_limits( { if (itr.name == hardware_interface::HW_IF_POSITION) { + limits.min_position = limits.has_position_limits && itr.enable_limits + ? limits.min_position + : -std::numeric_limits::max(); + limits.max_position = limits.has_position_limits && itr.enable_limits + ? limits.max_position + : std::numeric_limits::max(); double min_pos(limits.min_position), max_pos(limits.max_position); - bool has_min_max_interface_values = - detail::retrieve_min_max_interface_values(itr, min_pos, max_pos); - // position_limits can be restricted for continuous joints - limits.has_position_limits |= has_min_max_interface_values; - limits.has_position_limits &= itr.enable_limits; - if (limits.has_position_limits) - { - if (has_min_max_interface_values) - { - limits.min_position = std::max(min_pos, limits.min_position); - limits.max_position = std::min(max_pos, limits.max_position); - } - } - else + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_pos, max_pos)) { - limits.min_position = std::numeric_limits::min(); - limits.max_position = std::numeric_limits::max(); + limits.min_position = std::max(min_pos, limits.min_position); + limits.max_position = std::min(max_pos, limits.max_position); + limits.has_position_limits = true; } + limits.has_position_limits &= itr.enable_limits; } else if (itr.name == hardware_interface::HW_IF_VELOCITY) { - limits.has_velocity_limits &= itr.enable_limits; - if (limits.has_velocity_limits) - { // Apply the most restrictive one in the case - double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); - if (detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) - { - max_vel = std::min(std::abs(min_vel), max_vel); - limits.max_velocity = std::min(max_vel, limits.max_velocity); - } + limits.max_velocity = + limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_vel(-limits.max_velocity), max_vel(limits.max_velocity); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) + { + max_vel = std::min(std::abs(min_vel), max_vel); + limits.max_velocity = std::min(max_vel, limits.max_velocity); + limits.has_velocity_limits = true; } + limits.has_velocity_limits &= itr.enable_limits; } else if (itr.name == hardware_interface::HW_IF_EFFORT) { - limits.has_effort_limits &= itr.enable_limits; - if (limits.has_effort_limits) - { // Apply the most restrictive one in the case - double min_eff(-limits.max_effort), max_eff(limits.max_effort); - if (detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) - { - max_eff = std::min(std::abs(min_eff), max_eff); - limits.max_effort = std::min(max_eff, limits.max_effort); - } + limits.max_effort = + limits.has_effort_limits ? limits.max_effort : std::numeric_limits::max(); + // Apply the most restrictive one in the case + double min_eff(-limits.max_effort), max_eff(limits.max_effort); + if (itr.enable_limits && detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) + { + max_eff = std::min(std::abs(min_eff), max_eff); + limits.max_effort = std::min(max_eff, limits.max_effort); + limits.has_effort_limits = true; } + limits.has_effort_limits &= itr.enable_limits; } else { diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 407a8242c4..946fda7c15 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -879,7 +879,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in DoubleNear(std::numeric_limits::max(), 1e-5)); EXPECT_THAT( hardware_info.limits.at("joint1").min_position, - DoubleNear(std::numeric_limits::min(), 1e-5)); + DoubleNear(-std::numeric_limits::max(), 1e-5)); EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); @@ -897,7 +897,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_disabled_in DoubleNear(std::numeric_limits::max(), 1e-5)); EXPECT_THAT( hardware_info.limits.at("joint2").min_position, - DoubleNear(std::numeric_limits::min(), 1e-5)); + DoubleNear(-std::numeric_limits::max(), 1e-5)); EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); @@ -998,7 +998,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable DoubleNear(std::numeric_limits::max(), 1e-5)); EXPECT_THAT( hardware_info.limits.at("joint1").min_position, - DoubleNear(std::numeric_limits::min(), 1e-5)); + DoubleNear(-std::numeric_limits::max(), 1e-5)); EXPECT_TRUE(hardware_info.limits.at("joint1").has_velocity_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_effort_limits); EXPECT_TRUE(hardware_info.limits.at("joint1").has_acceleration_limits); @@ -1016,7 +1016,7 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_and_unavailable DoubleNear(std::numeric_limits::max(), 1e-5)); EXPECT_THAT( hardware_info.limits.at("joint2").min_position, - DoubleNear(std::numeric_limits::min(), 1e-5)); + DoubleNear(-std::numeric_limits::max(), 1e-5)); EXPECT_FALSE(hardware_info.limits.at("joint2").has_velocity_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_effort_limits); EXPECT_FALSE(hardware_info.limits.at("joint2").has_acceleration_limits); From dc0d4147c4a11532520b3f800d9bb7fa967cde61 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 3 May 2024 21:13:05 +0200 Subject: [PATCH 31/31] Remove the copy_interface_limits method as generic update_interface_limits works --- hardware_interface/src/component_parser.cpp | 74 +-------------------- 1 file changed, 3 insertions(+), 71 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 6309764ecc..aad1d827ec 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -779,67 +779,6 @@ void update_interface_limits( } } -/** - * @brief Copy the limits from ros2_control command interface tags if not URDF-limit tag is present - * @param interfaces The interfaces to retrieve the limits from. - * @param limits The joint limits to be set. - */ -void copy_interface_limits( - const std::vector & interfaces, joint_limits::JointLimits & limits) -{ - for (auto & itr : interfaces) - { - if (itr.name == hardware_interface::HW_IF_POSITION) - { - double min_pos, max_pos; - if (detail::retrieve_min_max_interface_values(itr, min_pos, max_pos)) - { - limits.min_position = std::max(min_pos, limits.min_position); - limits.max_position = std::min(max_pos, limits.max_position); - limits.has_position_limits = itr.enable_limits; - } - else - { - limits.min_position = std::numeric_limits::min(); - limits.max_position = std::numeric_limits::max(); - limits.has_position_limits = false; - } - } - else if (itr.name == hardware_interface::HW_IF_VELOCITY) - { - double min_vel, max_vel; - if (detail::retrieve_min_max_interface_values(itr, min_vel, max_vel)) - { - limits.max_velocity = std::min(std::abs(min_vel), max_vel); - limits.has_velocity_limits = itr.enable_limits; - } - else - { - limits.max_velocity = std::numeric_limits::max(); - limits.has_velocity_limits = false; - } - } - else if (itr.name == hardware_interface::HW_IF_EFFORT) - { - double min_eff, max_eff; - if (detail::retrieve_min_max_interface_values(itr, min_eff, max_eff)) - { - limits.max_effort = std::min(std::abs(min_eff), max_eff); - limits.has_effort_limits = itr.enable_limits; - } - else - { - limits.max_effort = std::numeric_limits::max(); - limits.has_effort_limits = false; - } - } - else - { - detail::set_custom_interface_values(itr, limits); - } - } -} - } // namespace detail std::vector parse_control_resources_from_urdf(const std::string & urdf) @@ -984,16 +923,9 @@ std::vector parse_control_resources_from_urdf(const std::string & "Fixed joints do not make sense in ros2_control."); } joint_limits::JointLimits limits; - if (getJointLimits(urdf_joint, limits)) - { - // Take the most restricted one - detail::update_interface_limits(joint.command_interfaces, limits); - } - else - { - // valid for continuous-joint type only - detail::copy_interface_limits(joint.command_interfaces, limits); - } + getJointLimits(urdf_joint, limits); + // 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; } }