From c4325763c10f57ef9867ca300471e087f3ab9be9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Jan 2024 17:51:51 +0100 Subject: [PATCH 01/16] Added joint limits urdf header for importing limits from URDF model --- joint_limits/CMakeLists.txt | 4 + .../joint_limits/joint_limits_urdf.hpp | 85 +++++++++++++++++++ joint_limits/package.xml | 1 + 3 files changed, 90 insertions(+) create mode 100644 joint_limits/include/joint_limits/joint_limits_urdf.hpp diff --git a/joint_limits/CMakeLists.txt b/joint_limits/CMakeLists.txt index a788b6de4f..1ed74c7603 100644 --- a/joint_limits/CMakeLists.txt +++ b/joint_limits/CMakeLists.txt @@ -8,6 +8,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rclcpp_lifecycle + urdf ) find_package(ament_cmake REQUIRED) @@ -31,6 +32,9 @@ if(BUILD_TESTING) ament_add_gtest_executable(joint_limits_rosparam_test test/joint_limits_rosparam_test.cpp) target_link_libraries(joint_limits_rosparam_test joint_limits) + ament_add_gtest(joint_limits_urdf_test test/joint_limits_urdf_test.cpp) + target_link_libraries(joint_limits_urdf_test joint_limits) + add_launch_test(test/joint_limits_rosparam.launch.py) install( TARGETS joint_limits_rosparam_test diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp new file mode 100644 index 0000000000..b5db39d9fb --- /dev/null +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -0,0 +1,85 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian + +#ifndef JOINT_LIMITS_URDF_HPP +#define JOINT_LIMITS_URDF_HPP + +#include "joint_limits/joint_limits.hpp" +#include "urdf_model/joint.h" + +namespace joint_limits +{ + +/** + * \brief Populate a JointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. + * \return True if \e urdf_joint has a valid limits specification, false otherwise. + */ +inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) +{ + if (!urdf_joint || !urdf_joint->limits) + { + return false; + } + + limits.has_position_limits = + urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::PRISMATIC; + if (limits.has_position_limits) + { + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + } + + if (!limits.has_position_limits && urdf_joint->type == urdf::Joint::CONTINUOUS) + { + limits.angle_wraparound = true; + } + + limits.has_velocity_limits = true; + limits.max_velocity = urdf_joint->limits->velocity; + + limits.has_acceleration_limits = false; + + limits.has_effort_limits = true; + limits.max_effort = urdf_joint->limits->effort; + + return true; +} + +/** + * \brief Populate a SoftJointLimits instance from URDF joint data. + * \param[in] urdf_joint URDF joint. + * \param[out] soft_limits Where URDF soft joint limit data gets written into. + * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + */ +inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) +{ + if (!urdf_joint || !urdf_joint->safety) + { + return false; + } + + soft_limits.min_position = urdf_joint->safety->soft_lower_limit; + soft_limits.max_position = urdf_joint->safety->soft_upper_limit; + soft_limits.k_position = urdf_joint->safety->k_position; + soft_limits.k_velocity = urdf_joint->safety->k_velocity; + + return true; +} +} // namespace joint_limits +#endif // JOINT_LIMITS_URDF_HPP diff --git a/joint_limits/package.xml b/joint_limits/package.xml index e1832fa776..98890eb03a 100644 --- a/joint_limits/package.xml +++ b/joint_limits/package.xml @@ -16,6 +16,7 @@ rclcpp rclcpp_lifecycle + urdf launch_testing_ament_cmake ament_cmake_gtest From f22dae41fa331c368cdb00c43a460069b00cf15e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Jan 2024 17:52:30 +0100 Subject: [PATCH 02/16] Added tests for the newly added joint limits URDF header --- joint_limits/test/joint_limits_urdf_test.cpp | 177 +++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 joint_limits/test/joint_limits_urdf_test.cpp diff --git a/joint_limits/test/joint_limits_urdf_test.cpp b/joint_limits/test/joint_limits_urdf_test.cpp new file mode 100644 index 0000000000..27d660afd7 --- /dev/null +++ b/joint_limits/test/joint_limits_urdf_test.cpp @@ -0,0 +1,177 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// \author Adolfo Rodriguez Tsouroukdissian +#include "joint_limits/joint_limits_urdf.hpp" +#include "gtest/gtest.h" + +using std::string; +using namespace joint_limits; + +class JointLimitsUrdfTest : public ::testing::Test +{ +public: + JointLimitsUrdfTest() + { + urdf_limits.reset(new urdf::JointLimits); + urdf_limits->effort = 8.0; + urdf_limits->velocity = 2.0; + urdf_limits->lower = -1.0; + urdf_limits->upper = 1.0; + + urdf_safety.reset(new urdf::JointSafety); + urdf_safety->k_position = 20.0; + urdf_safety->k_velocity = 40.0; + urdf_safety->soft_lower_limit = -0.8; + urdf_safety->soft_upper_limit = 0.8; + + urdf_joint.reset(new urdf::Joint); + urdf_joint->limits = urdf_limits; + urdf_joint->safety = urdf_safety; + + urdf_joint->type = urdf::Joint::UNKNOWN; + } + +protected: + urdf::JointLimitsSharedPtr urdf_limits; + urdf::JointSafetySharedPtr urdf_safety; + urdf::JointSharedPtr urdf_joint; +}; + +TEST_F(JointLimitsUrdfTest, GetJointLimits) +{ + // Unset URDF joint + { + JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Unset URDF limits + { + JointLimits limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getJointLimits(urdf_joint_bad, limits)); + } + + // Valid URDF joint, CONTINUOUS type + { + urdf_joint->type = urdf::Joint::CONTINUOUS; + + JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_FALSE(limits.has_position_limits); + EXPECT_TRUE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, REVOLUTE type + { + urdf_joint->type = urdf::Joint::REVOLUTE; + + JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } + + // Valid URDF joint, PRISMATIC type + { + urdf_joint->type = urdf::Joint::PRISMATIC; + + JointLimits limits; + EXPECT_TRUE(getJointLimits(urdf_joint, limits)); + + // Position + EXPECT_TRUE(limits.has_position_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->lower, limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->limits->upper, limits.max_position); + EXPECT_FALSE(limits.angle_wraparound); + + // Velocity + EXPECT_TRUE(limits.has_velocity_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->velocity, limits.max_velocity); + + // Acceleration + EXPECT_FALSE(limits.has_acceleration_limits); + + // Effort + EXPECT_TRUE(limits.has_effort_limits); + EXPECT_DOUBLE_EQ(urdf_joint->limits->effort, limits.max_effort); + } +} + +TEST_F(JointLimitsUrdfTest, GetSoftJointLimits) +{ + using namespace joint_limits; + + // Unset URDF joint + { + SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad; + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Unset URDF limits + { + SoftJointLimits soft_limits; + urdf::JointSharedPtr urdf_joint_bad(new urdf::Joint); + EXPECT_FALSE(getSoftJointLimits(urdf_joint_bad, soft_limits)); + } + + // Valid URDF joint + { + SoftJointLimits soft_limits; + EXPECT_TRUE(getSoftJointLimits(urdf_joint, soft_limits)); + + // Soft limits + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_lower_limit, soft_limits.min_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->soft_upper_limit, soft_limits.max_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_position, soft_limits.k_position); + EXPECT_DOUBLE_EQ(urdf_joint->safety->k_velocity, soft_limits.k_velocity); + } +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From c00e1fec81d3e5d061e1f1b84dcd976f294db746 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Jan 2024 18:22:27 +0100 Subject: [PATCH 03/16] Use the absolute floating point value for velocity and effort --- joint_limits/include/joint_limits/joint_limits_urdf.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp index b5db39d9fb..4882bb2696 100644 --- a/joint_limits/include/joint_limits/joint_limits_urdf.hpp +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -51,12 +51,12 @@ inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & l } limits.has_velocity_limits = true; - limits.max_velocity = urdf_joint->limits->velocity; + limits.max_velocity = fabs(urdf_joint->limits->velocity); limits.has_acceleration_limits = false; limits.has_effort_limits = true; - limits.max_effort = urdf_joint->limits->effort; + limits.max_effort = fabs(urdf_joint->limits->effort); return true; } From e2bf4ceceaecc729b49d8561714d061f3d4de131 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 11:31:01 +0100 Subject: [PATCH 04/16] use std::abs instead of fabs --- joint_limits/include/joint_limits/joint_limits_urdf.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp index 4882bb2696..daa0d707d8 100644 --- a/joint_limits/include/joint_limits/joint_limits_urdf.hpp +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -51,12 +51,12 @@ inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & l } limits.has_velocity_limits = true; - limits.max_velocity = fabs(urdf_joint->limits->velocity); + limits.max_velocity = std::abs(urdf_joint->limits->velocity); limits.has_acceleration_limits = false; limits.has_effort_limits = true; - limits.max_effort = fabs(urdf_joint->limits->effort); + limits.max_effort = std::abs(urdf_joint->limits->effort); return true; } From 07dd0bde74b065be02e30f1151062634fc6187c6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Jan 2024 17:23:53 +0100 Subject: [PATCH 05/16] Added Limits struct to the InterfaceInfo --- .../include/hardware_interface/hardware_info.hpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index eb6b63cfc3..50ec841ff4 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,12 +15,19 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include #include #include #include namespace hardware_interface { +struct Limits +{ + std::optional min = std::nullopt; + std::optional max = std::nullopt; +}; + /** * This structure stores information about components defined for a specific hardware * in robot's URDF. @@ -42,6 +49,9 @@ struct InterfaceInfo std::string data_type; /// (Optional) If the handle is an array, the size of the array. Used by GPIOs. int size; + /// (Optional) Stores the information of the limits of the interfaces such as "position", + /// "velocity" etc from the URDF + Limits limits; }; /** From bb392f2b2c623ec48b3be3df8c1ff414d2e9fc60 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Jan 2024 18:12:29 +0100 Subject: [PATCH 06/16] Added methods to retrieve limits from the URDF joints and ros2_control tags --- hardware_interface/CMakeLists.txt | 1 + hardware_interface/package.xml | 1 + hardware_interface/src/component_parser.cpp | 56 +++++++++++++++++++++ 3 files changed, 58 insertions(+) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 35823b3ce9..175259c36a 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -14,6 +14,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rcutils TinyXML2 tinyxml2_vendor + urdf ) find_package(ament_cmake REQUIRED) diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index b3685a7ee2..2ebad585b2 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -15,6 +15,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + urdf rcutils rcutils diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 4f67d3e8b6..e4908a12f7 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -24,6 +24,8 @@ #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 "urdf/model.h" namespace { @@ -611,6 +613,60 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } + // Retrieve the limits from the URDF joints as well as the ros2_control command interface tags + auto update_interface_limits = [](const urdf::JointConstSharedPtr & urdf_joint, auto & interfaces) + { + for (auto & itr : interfaces) + { + if (itr.name == hardware_interface::HW_IF_POSITION) + { + itr.limits.min = urdf_joint->limits->lower; + itr.limits.max = urdf_joint->limits->upper; + } + else if (itr.name == hardware_interface::HW_IF_VELOCITY) + { + itr.limits.min = -1.0 * urdf_joint->limits->velocity; + itr.limits.max = urdf_joint->limits->velocity; + } + else if (itr.name == hardware_interface::HW_IF_EFFORT) + { + itr.limits.min = -1.0 * urdf_joint->limits->effort; + itr.limits.max = urdf_joint->limits->effort; + } + else // Acceleration and other custom types interfaces can use the standard min and max tags + { + if (!itr.min.empty()) + { + itr.limits.min = hardware_interface::stod(itr.min); + } + if (!itr.max.empty()) + { + itr.limits.max = hardware_interface::stod(itr.max); + } + } + } + }; + + // Parse robot_description to URDF Model + urdf::Model model; + if (!model.initString(urdf)) + { + throw std::runtime_error("Failed to parse URDF file"); + } + for (auto & hw_info : hardware_info) + { + for (auto & joint_info : hw_info.joints) + { + auto urdf_joint = model.getJoint(joint_info.name); + if (!urdf_joint) + { + throw std::runtime_error("Joint " + joint_info.name + " not found in URDF"); + } + update_interface_limits(urdf_joint, joint_info.command_interfaces); + update_interface_limits(urdf_joint, joint_info.state_interfaces); + } + } + return hardware_info; } From 4b5c8a85ba2dd0c539d0b0ad77360d9e172249b6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Jan 2024 11:29:20 +0100 Subject: [PATCH 07/16] Make the main Limits variable optional and added exception handling for general scenario --- .../hardware_interface/hardware_info.hpp | 7 +++-- hardware_interface/src/component_parser.cpp | 31 +++++++++++++------ 2 files changed, 25 insertions(+), 13 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 50ec841ff4..019c768295 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -15,6 +15,7 @@ #ifndef HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ #define HARDWARE_INTERFACE__HARDWARE_INFO_HPP_ +#include #include #include #include @@ -24,8 +25,8 @@ namespace hardware_interface { struct Limits { - std::optional min = std::nullopt; - std::optional max = std::nullopt; + double min = std::numeric_limits::quiet_NaN(); + double max = std::numeric_limits::quiet_NaN(); }; /** @@ -51,7 +52,7 @@ struct InterfaceInfo int size; /// (Optional) Stores the information of the limits of the interfaces such as "position", /// "velocity" etc from the URDF - Limits limits; + std::optional limits = std::nullopt; }; /** diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index e4908a12f7..f4ec1dd453 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -618,32 +618,43 @@ std::vector parse_control_resources_from_urdf(const std::string & { for (auto & itr : interfaces) { + Limits limits; if (itr.name == hardware_interface::HW_IF_POSITION) { - itr.limits.min = urdf_joint->limits->lower; - itr.limits.max = urdf_joint->limits->upper; + limits.min = urdf_joint->limits->lower; + limits.max = urdf_joint->limits->upper; } else if (itr.name == hardware_interface::HW_IF_VELOCITY) { - itr.limits.min = -1.0 * urdf_joint->limits->velocity; - itr.limits.max = urdf_joint->limits->velocity; + limits.min = -1.0 * urdf_joint->limits->velocity; + limits.max = urdf_joint->limits->velocity; } else if (itr.name == hardware_interface::HW_IF_EFFORT) { - itr.limits.min = -1.0 * urdf_joint->limits->effort; - itr.limits.max = urdf_joint->limits->effort; + limits.min = -1.0 * urdf_joint->limits->effort; + limits.max = urdf_joint->limits->effort; } else // Acceleration and other custom types interfaces can use the standard min and max tags { - if (!itr.min.empty()) + try { - itr.limits.min = hardware_interface::stod(itr.min); + if (!itr.min.empty()) + { + limits.min = hardware_interface::stod(itr.min); + } + if (!itr.max.empty()) + { + limits.max = hardware_interface::stod(itr.max); + } } - if (!itr.max.empty()) + catch (const std::invalid_argument & err) { - itr.limits.max = hardware_interface::stod(itr.max); + std::cerr << "Error parsing the limits for the interface : " << itr.name + << "from the tags [" << kMinTag << " and " << kMaxTag << "] within " + << kROS2ControlTag << " tag inside the URDF" << std::endl; } } + itr.limits = limits; } }; From 2f1cfce700e7a8f63ab227c37e990542fa8f76b2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Jan 2024 18:37:07 +0100 Subject: [PATCH 08/16] Added the first version of the SaturationHandle class --- .../hardware_interface/limits_handle.hpp | 182 ++++++++++++++++++ 1 file changed, 182 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/limits_handle.hpp diff --git a/hardware_interface/include/hardware_interface/limits_handle.hpp b/hardware_interface/include/hardware_interface/limits_handle.hpp new file mode 100644 index 0000000000..54c49d7a52 --- /dev/null +++ b/hardware_interface/include/hardware_interface/limits_handle.hpp @@ -0,0 +1,182 @@ +// Copyright 2024 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__LIMITS_HANDLE_HPP +#define HARDWARE_INTERFACE__LIMITS_HANDLE_HPP + +#include +#include "hardware_interface/handle.hpp" +#include "joint_limits/joint_limits.hpp" +#include "rclcpp/duration.hpp" + +namespace hardware_interface +{ +class SaturationHandle +{ +public: + SaturationHandle() {} + + void enforce_limits(const rclcpp::Duration & period) + { + enforce_position_limits(period); + enforce_velocity_limits(period); + } + +private: + void enforce_position_limits(const rclcpp::Duration & period) + { + if (reference_position_.has_value()) + { + // Initially, the previous command will be nan, set it to the current position value + if (!std::isfinite(prev_pos_cmd_)) + { + if (actual_position_.has_value()) + { + prev_pos_cmd_ = actual_position_.value().get_value(); + } + else + { + prev_pos_cmd_ = reference_position_->get_value(); + } + } + + // Check if the joint has joint limits, if not set the limits to maximum + double min_pos_limit, max_pos_limit; + if (limits_.has_position_limits) + { + min_pos_limit = limits_.min_position; + max_pos_limit = limits_.max_position; + } + else + { + min_pos_limit = std::numeric_limits::min(); + max_pos_limit = std::numeric_limits::max(); + } + + // Evalute and clamp the position command to the maximum reachable value by hardware + double min_pos, max_pos; + if (limits_.has_velocity_limits) + { + const double delta_pos = limits_.max_velocity * period.seconds(); + min_pos = std::max(prev_pos_cmd_ - delta_pos, min_pos_limit); + max_pos = std::min(prev_pos_cmd_ + delta_pos, max_pos_limit); + } + else + { + min_pos = min_pos_limit; + max_pos = max_pos_limit; + } + + // Saturate position command according to limits + const double cmd = std::clamp(reference_position_.value().get_value(), min_pos, max_pos); + reference_position_->set_value(cmd); + prev_pos_cmd_ = cmd; + } + } + + void enforce_velocity_limits(const rclcpp::Duration & period) + { + if (limits_.has_velocity_limits && reference_velocity_.has_value()) + { + // Velocity bounds + double vel_low; + double vel_high; + + if (!std::isfinite(reference_velocity_->get_value())) + { + reference_velocity_->set_value(0.0); + } + if (!std::isfinite(prev_vel_cmd_)) + { + prev_vel_cmd_ = 0.0; + } + + if (limits_.has_acceleration_limits) + { + assert(period.seconds() > 0.0); + const double dt = period.seconds(); + + vel_low = + std::max(prev_vel_cmd_ - fabs(limits_.max_deceleration) * dt, -limits_.max_velocity); + vel_high = std::min(prev_vel_cmd_ + limits_.max_acceleration * dt, limits_.max_velocity); + } + else + { + vel_low = -limits_.max_velocity; + vel_high = limits_.max_velocity; + } + + // Saturate velocity command according to limits + const double vel_cmd = std::clamp(reference_velocity_.value().get_value(), vel_low, vel_high); + reference_velocity_->set_value(vel_cmd); + prev_vel_cmd_ = vel_cmd; + } + } + + void enforce_effort_limits(const rclcpp::Duration & period) + { + if (limits_.has_effort_limits && reference_effort_.has_value()) + { + double min_eff = -limits_.max_effort; + double max_eff = limits_.max_effort; + + if (limits_.has_position_limits && actual_position_.has_value()) + { + const double pos = actual_position_->get_value(); + if (pos < limits_.min_position) + { + min_eff = 0; + } + else if (pos > limits_.max_position) + { + max_eff = 0; + } + } + + if (limits_.has_velocity_limits && actual_velocity_.has_value()) + { + const double vel = actual_velocity_->get_value(); + if (vel < -limits_.max_velocity) + { + min_eff = 0; + } + else if (vel > limits_.max_velocity) + { + max_eff = 0; + } + } + // Saturate effort command according to limits + const double eff_cmd = std::clamp(reference_effort_.value().get_value(), min_eff, max_eff); + reference_effort_->set_value(eff_cmd); + } + } + + joint_limits::JointLimits limits_; + double prev_pos_cmd_ = {std::numeric_limits::quiet_NaN()}; + double prev_vel_cmd_ = {std::numeric_limits::quiet_NaN()}; + + std::optional actual_position_ = std::nullopt; + std::optional actual_velocity_ = std::nullopt; + std::optional actual_effort_ = std::nullopt; + std::optional actual_acceleration_ = std::nullopt; + + std::optional reference_position_ = std::nullopt; + std::optional reference_velocity_ = std::nullopt; + std::optional reference_effort_ = std::nullopt; + std::optional reference_acceleration_ = std::nullopt; +}; + +} // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__LIMITS_HANDLE_HPP From 1f0ed6661fd780b4d0c35ff1e7caf8c912eff254 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Jan 2024 14:08:51 +0100 Subject: [PATCH 09/16] create SaturationHandle per control mode instead of a single one --- .../hardware_interface/limits_handle.hpp | 268 +++++++++++++----- 1 file changed, 192 insertions(+), 76 deletions(-) diff --git a/hardware_interface/include/hardware_interface/limits_handle.hpp b/hardware_interface/include/hardware_interface/limits_handle.hpp index 54c49d7a52..e0e003ada6 100644 --- a/hardware_interface/include/hardware_interface/limits_handle.hpp +++ b/hardware_interface/include/hardware_interface/limits_handle.hpp @@ -15,91 +15,149 @@ #ifndef HARDWARE_INTERFACE__LIMITS_HANDLE_HPP #define HARDWARE_INTERFACE__LIMITS_HANDLE_HPP +#include +#include #include #include "hardware_interface/handle.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_limits/joint_limits.hpp" #include "rclcpp/duration.hpp" namespace hardware_interface { +const std::string get_component_interface_name( + const std::string & prefix_name, const std::string & interface_name) +{ + return prefix_name + "/" + interface_name; +} + class SaturationHandle { public: - SaturationHandle() {} + SaturationHandle( + const std::string & prefix_name, const std::string & interface_name, + std::map & state_interface_map, + std::map & command_interface_map) + : prefix_name_(prefix_name), + interface_name_(interface_name), + prev_cmd_(std::numeric_limits::quiet_NaN()) + { + } - void enforce_limits(const rclcpp::Duration & period) + virtual void enforce_limits(const rclcpp::Duration & period) = 0; + +protected: + std::string prefix_name_; + std::string interface_name_; + double prev_cmd_; + + std::unordered_map> actual_; + std::unordered_map> reference_; +}; + +class PositionSaturationHandle : public SaturationHandle +{ +public: + PositionSaturationHandle( + const std::string & prefix_name, const joint_limits::JointLimits & limits, + std::map & state_interface_map, + std::map & command_interface_map) + : SaturationHandle(prefix_name, HW_IF_POSITION, state_interface_map, command_interface_map), + limits_(limits) { - enforce_position_limits(period); - enforce_velocity_limits(period); + prev_cmd_ = 0.0; + reference_[HW_IF_POSITION] = + std::ref(command_interface_map[get_component_interface_name(prefix_name, HW_IF_POSITION)]); + // TODO(saikishor): Check and handle the case when the position state interface might not exist + actual_[HW_IF_POSITION] = + std::cref(state_interface_map[get_component_interface_name(prefix_name, HW_IF_POSITION)]); } -private: - void enforce_position_limits(const rclcpp::Duration & period) + virtual void enforce_limits(const rclcpp::Duration & period) override { - if (reference_position_.has_value()) + // Initially, the previous command might be NaN, set it to the current position value + auto & ref_pos_itf = reference_[HW_IF_POSITION].get(); + if (!std::isfinite(prev_cmd_)) { - // Initially, the previous command will be nan, set it to the current position value - if (!std::isfinite(prev_pos_cmd_)) - { - if (actual_position_.has_value()) - { - prev_pos_cmd_ = actual_position_.value().get_value(); - } - else - { - prev_pos_cmd_ = reference_position_->get_value(); - } - } - - // Check if the joint has joint limits, if not set the limits to maximum - double min_pos_limit, max_pos_limit; - if (limits_.has_position_limits) + auto & act_pos_itf = actual_[HW_IF_POSITION].get(); + if (!actual_.empty()) { - min_pos_limit = limits_.min_position; - max_pos_limit = limits_.max_position; + prev_cmd_ = act_pos_itf.get_value(); } else { - min_pos_limit = std::numeric_limits::min(); - max_pos_limit = std::numeric_limits::max(); + prev_cmd_ = ref_pos_itf.get_value(); } + } - // Evalute and clamp the position command to the maximum reachable value by hardware - double min_pos, max_pos; - if (limits_.has_velocity_limits) - { - const double delta_pos = limits_.max_velocity * period.seconds(); - min_pos = std::max(prev_pos_cmd_ - delta_pos, min_pos_limit); - max_pos = std::min(prev_pos_cmd_ + delta_pos, max_pos_limit); - } - else - { - min_pos = min_pos_limit; - max_pos = max_pos_limit; - } + // Check if the joint has joint limits, if not set the limits to maximum + double min_pos_limit, max_pos_limit; + if (limits_.has_position_limits) + { + min_pos_limit = limits_.min_position; + max_pos_limit = limits_.max_position; + } + else + { + min_pos_limit = std::numeric_limits::min(); + max_pos_limit = std::numeric_limits::max(); + } - // Saturate position command according to limits - const double cmd = std::clamp(reference_position_.value().get_value(), min_pos, max_pos); - reference_position_->set_value(cmd); - prev_pos_cmd_ = cmd; + // Evalute and clamp the position command to the maximum reachable value by hardware + double min_pos, max_pos; + if (limits_.has_velocity_limits) + { + const double delta_pos = limits_.max_velocity * period.seconds(); + min_pos = std::max(prev_cmd_ - delta_pos, min_pos_limit); + max_pos = std::min(prev_cmd_ + delta_pos, max_pos_limit); + } + else + { + min_pos = min_pos_limit; + max_pos = max_pos_limit; } + + // Saturate position command according to limits + const double cmd = std::clamp(ref_pos_itf.get_value(), min_pos, max_pos); + ref_pos_itf.set_value(cmd); + prev_cmd_ = cmd; } - void enforce_velocity_limits(const rclcpp::Duration & period) +private: + joint_limits::JointLimits limits_; +}; + +class VelocitySaturationHandle : public SaturationHandle +{ +public: + VelocitySaturationHandle( + const std::string & prefix_name, const joint_limits::JointLimits & limits, + std::map & state_interface_map, + std::map & command_interface_map) + : SaturationHandle(prefix_name, HW_IF_VELOCITY, state_interface_map, command_interface_map), + limits_(limits) + { + prev_cmd_ = 0.0; + reference_[HW_IF_VELOCITY] = + std::ref(command_interface_map[get_component_interface_name(prefix_name, HW_IF_VELOCITY)]); + } + + virtual void enforce_limits(const rclcpp::Duration & period) override { - if (limits_.has_velocity_limits && reference_velocity_.has_value()) + if (limits_.has_velocity_limits) { // Velocity bounds double vel_low; double vel_high; - if (!std::isfinite(reference_velocity_->get_value())) + auto & ref_vel_itf = reference_[HW_IF_VELOCITY].get(); + if (!std::isfinite(ref_vel_itf.get_value())) { - reference_velocity_->set_value(0.0); + ref_vel_itf.set_value(0.0); } - if (!std::isfinite(prev_vel_cmd_)) + if (!std::isfinite(prev_cmd_)) { - prev_vel_cmd_ = 0.0; + prev_cmd_ = 0.0; } if (limits_.has_acceleration_limits) @@ -107,9 +165,8 @@ class SaturationHandle assert(period.seconds() > 0.0); const double dt = period.seconds(); - vel_low = - std::max(prev_vel_cmd_ - fabs(limits_.max_deceleration) * dt, -limits_.max_velocity); - vel_high = std::min(prev_vel_cmd_ + limits_.max_acceleration * dt, limits_.max_velocity); + vel_low = std::max(prev_cmd_ - fabs(limits_.max_deceleration) * dt, -limits_.max_velocity); + vel_high = std::min(prev_cmd_ + limits_.max_acceleration * dt, limits_.max_velocity); } else { @@ -118,22 +175,50 @@ class SaturationHandle } // Saturate velocity command according to limits - const double vel_cmd = std::clamp(reference_velocity_.value().get_value(), vel_low, vel_high); - reference_velocity_->set_value(vel_cmd); - prev_vel_cmd_ = vel_cmd; + const double vel_cmd = std::clamp(ref_vel_itf.get_value(), vel_low, vel_high); + ref_vel_itf.set_value(vel_cmd); + prev_cmd_ = vel_cmd; } } - void enforce_effort_limits(const rclcpp::Duration & period) +private: + joint_limits::JointLimits limits_; +}; + +class EffortSaturationHandle : public SaturationHandle +{ +public: + EffortSaturationHandle( + const std::string & prefix_name, const joint_limits::JointLimits & limits, + std::map & state_interface_map, + std::map & command_interface_map) + : SaturationHandle(prefix_name, HW_IF_EFFORT, state_interface_map, command_interface_map), + limits_(limits) + { + prev_cmd_ = 0.0; + reference_[HW_IF_EFFORT] = + std::ref(command_interface_map[get_component_interface_name(prefix_name, HW_IF_EFFORT)]); + // TODO(saikishor): Check and handle the case when the position (or) velocity (or) both state + // interface might not exist + actual_[HW_IF_POSITION] = + std::cref(state_interface_map[get_component_interface_name(prefix_name, HW_IF_POSITION)]); + actual_[HW_IF_VELOCITY] = + std::cref(state_interface_map[get_component_interface_name(prefix_name, HW_IF_VELOCITY)]); + } + + virtual void enforce_limits(const rclcpp::Duration & period) override { - if (limits_.has_effort_limits && reference_effort_.has_value()) + auto & ref_eff_itf = reference_[HW_IF_EFFORT].get(); + auto & act_pos_itf = actual_[HW_IF_POSITION].get(); + auto & act_vel_itf = actual_[HW_IF_VELOCITY].get(); + if (limits_.has_effort_limits) { double min_eff = -limits_.max_effort; double max_eff = limits_.max_effort; - if (limits_.has_position_limits && actual_position_.has_value()) + if (limits_.has_position_limits) { - const double pos = actual_position_->get_value(); + const double pos = act_pos_itf.get_value(); if (pos < limits_.min_position) { min_eff = 0; @@ -144,9 +229,9 @@ class SaturationHandle } } - if (limits_.has_velocity_limits && actual_velocity_.has_value()) + if (limits_.has_velocity_limits) { - const double vel = actual_velocity_->get_value(); + const double vel = act_vel_itf.get_value(); if (vel < -limits_.max_velocity) { min_eff = 0; @@ -157,26 +242,57 @@ class SaturationHandle } } // Saturate effort command according to limits - const double eff_cmd = std::clamp(reference_effort_.value().get_value(), min_eff, max_eff); - reference_effort_->set_value(eff_cmd); + const double eff_cmd = std::clamp(ref_eff_itf.get_value(), min_eff, max_eff); + ref_eff_itf.set_value(eff_cmd); } } +private: joint_limits::JointLimits limits_; - double prev_pos_cmd_ = {std::numeric_limits::quiet_NaN()}; - double prev_vel_cmd_ = {std::numeric_limits::quiet_NaN()}; - - std::optional actual_position_ = std::nullopt; - std::optional actual_velocity_ = std::nullopt; - std::optional actual_effort_ = std::nullopt; - std::optional actual_acceleration_ = std::nullopt; - - std::optional reference_position_ = std::nullopt; - std::optional reference_velocity_ = std::nullopt; - std::optional reference_effort_ = std::nullopt; - std::optional reference_acceleration_ = std::nullopt; }; +class JointSaturationInterface +{ + JointSaturationInterface( + const std::string & joint_name, const joint_limits::JointLimits & limits, + std::map & state_interface_map, + std::map & command_interface_map) + { + auto interface_exist = + [&]( + const std::string & joint_name, const std::string & interface_name, const auto & interfaces) + { + auto it = interfaces.find(get_component_interface_name(joint_name, interface_name)); + return it != interfaces.end(); + }; + if (interface_exist(joint_name, HW_IF_POSITION, command_interface_map)) + { + impl_.push_back(std::make_shared( + joint_name, limits, state_interface_map, command_interface_map)); + } + if (interface_exist(joint_name, HW_IF_VELOCITY, command_interface_map)) + { + impl_.push_back(std::make_shared( + joint_name, limits, state_interface_map, command_interface_map)); + } + if (interface_exist(joint_name, HW_IF_EFFORT, command_interface_map)) + { + impl_.push_back(std::make_shared( + joint_name, limits, state_interface_map, command_interface_map)); + } + } + + void enforce_limits(const rclcpp::Duration & period) + { + for (auto & impl : impl_) + { + impl->enforce_limits(period); + } + } + +private: + std::vector> impl_; +}; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__LIMITS_HANDLE_HPP From df8f94ddeee670fac8a62dff7acd25c4a249cdd7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 13:44:50 +0100 Subject: [PATCH 10/16] 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 175259c36a..6e12910543 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -14,6 +14,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 019c768295..bbe54e9c4e 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -21,6 +21,8 @@ #include #include +#include "joint_limits/joint_limits.hpp" + namespace hardware_interface { struct Limits @@ -150,6 +152,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 2ebad585b2..aff249bfaa 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -15,6 +15,7 @@ rclcpp_lifecycle rcpputils tinyxml2_vendor + joint_limits urdf rcutils From e8504cd9b5b45238f3c32f86e3c0ff8623a2c181 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 13:47:11 +0100 Subject: [PATCH 11/16] Retrieve limits from the URDF as well as ros2_control tag from robot_description --- hardware_interface/src/component_parser.cpp | 145 +++++++++++++++++--- 1 file changed, 125 insertions(+), 20 deletions(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index f4ec1dd453..fe798d1e21 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -25,6 +25,7 @@ #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 @@ -613,48 +614,138 @@ std::vector parse_control_resources_from_urdf(const std::string & ros2_control_it = ros2_control_it->NextSiblingElement(kROS2ControlTag); } - // Retrieve the limits from the URDF joints as well as the ros2_control command interface tags - auto update_interface_limits = [](const urdf::JointConstSharedPtr & urdf_joint, auto & interfaces) + // 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) { - Limits limits; if (itr.name == hardware_interface::HW_IF_POSITION) { - limits.min = urdf_joint->limits->lower; - limits.max = urdf_joint->limits->upper; + 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) { - limits.min = -1.0 * urdf_joint->limits->velocity; - limits.max = urdf_joint->limits->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) { - limits.min = -1.0 * urdf_joint->limits->effort; - limits.max = urdf_joint->limits->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 // Acceleration and other custom types interfaces can use the standard min and max tags + else 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 (retrieve_min_max_interface_values(itr, max_decel, max_accel)) { - if (!itr.min.empty()) + if (std::isfinite(max_decel)) { - limits.min = hardware_interface::stod(itr.min); + limits.max_deceleration = max_decel; + if (!std::isfinite(max_accel)) + { + limits.max_acceleration = std::fabs(limits.max_deceleration); + } } - if (!itr.max.empty()) + if (std::isfinite(max_accel)) { - limits.max = hardware_interface::stod(itr.max); + limits.max_acceleration = max_accel; + + if (!std::isfinite(limits.max_deceleration)) + { + limits.max_deceleration = -limits.max_acceleration; + } } + limits.has_acceleration_limits = 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() && 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" << std::endl; + << kROS2ControlTag + << " tag inside the URDF. Supported interfaces for joint limits are : " + "position, velocity, effort, acceleration and jerk." + << std::endl; } } - itr.limits = limits; } }; @@ -673,8 +764,22 @@ std::vector parse_control_resources_from_urdf(const std::string & { throw std::runtime_error("Joint " + joint_info.name + " not found in URDF"); } - update_interface_limits(urdf_joint, joint_info.command_interfaces); - update_interface_limits(urdf_joint, joint_info.state_interfaces); + 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_info.name); + } + if (has_hard_limts) + { + // Take the most restricted one + update_interface_limits(joint_info.command_interfaces, limits); + update_interface_limits(joint_info.state_interfaces, limits); + hw_info.limits[joint_info.name] = limits; + } } } From 105b369aa90ce6ada4c06272e3771e19b70e65c3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jan 2024 17:11:23 +0100 Subject: [PATCH 12/16] 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 fe798d1e21..dd4287394e 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -762,7 +762,9 @@ std::vector parse_control_resources_from_urdf(const std::string & auto urdf_joint = model.getJoint(joint_info.name); if (!urdf_joint) { - throw std::runtime_error("Joint " + joint_info.name + " not found in URDF"); + std::cerr << "Joint : '" + joint_info.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 4416a26495115d35061aeec71b1a6980e949dd80 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jan 2024 15:02:48 +0100 Subject: [PATCH 13/16] add public access specifier to JointSaturationInterface --- hardware_interface/include/hardware_interface/limits_handle.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/hardware_interface/include/hardware_interface/limits_handle.hpp b/hardware_interface/include/hardware_interface/limits_handle.hpp index e0e003ada6..44e7255e08 100644 --- a/hardware_interface/include/hardware_interface/limits_handle.hpp +++ b/hardware_interface/include/hardware_interface/limits_handle.hpp @@ -253,6 +253,7 @@ class EffortSaturationHandle : public SaturationHandle class JointSaturationInterface { +public: JointSaturationInterface( const std::string & joint_name, const joint_limits::JointLimits & limits, std::map & state_interface_map, From 8426336af6e4da72661ab0c0571de92f6b737f94 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jan 2024 15:11:18 +0100 Subject: [PATCH 14/16] Add import joint limiters from the hardware_info --- hardware_interface/src/resource_manager.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 0cbb620c76..88059b0b4f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -29,6 +29,7 @@ #include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" +#include "hardware_interface/limits_handle.hpp" #include "hardware_interface/sensor.hpp" #include "hardware_interface/sensor_interface.hpp" #include "hardware_interface/system.hpp" @@ -454,6 +455,17 @@ class ResourceStorage hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } + void import_joint_limiters(const HardwareInfo & hardware_info) + { + std::vector limit_handles; + for (const auto & [key, value] : hardware_info.limits) + { + limit_handles.push_back( + JointSaturationInterface(key, value, state_interface_map_, command_interface_map_)); + } + joint_limiter_[hardware_info.name] = limit_handles; + } + /// Adds exported command interfaces into internal storage. /** * Add command interfaces to the internal storage. Command interfaces exported from hardware or @@ -519,6 +531,7 @@ class ResourceStorage { import_state_interfaces(container.back()); import_command_interfaces(container.back()); + import_joint_limiters(hardware_info); } else { @@ -578,6 +591,7 @@ class ResourceStorage { import_state_interfaces(container.back()); import_command_interfaces(container.back()); + import_joint_limiters(hardware_info); } else { @@ -713,6 +727,9 @@ class ResourceStorage /// Storage of all available command interfaces std::map command_interface_map_; + /// Saturation handle linked to the joints within the hardware + std::unordered_map> joint_limiter_; + /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; std::vector available_command_interfaces_; From bda53eee4023496a2e39ad7b99dc21b8f88f13af Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jan 2024 15:14:29 +0100 Subject: [PATCH 15/16] rename to hardware command limiter instead of joint limiter --- hardware_interface/src/resource_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 88059b0b4f..84e1a24d94 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -463,7 +463,7 @@ class ResourceStorage limit_handles.push_back( JointSaturationInterface(key, value, state_interface_map_, command_interface_map_)); } - joint_limiter_[hardware_info.name] = limit_handles; + hw_command_limiter_[hardware_info.name] = limit_handles; } /// Adds exported command interfaces into internal storage. @@ -728,7 +728,7 @@ class ResourceStorage std::map command_interface_map_; /// Saturation handle linked to the joints within the hardware - std::unordered_map> joint_limiter_; + std::unordered_map> hw_command_limiter_; /// Vectors with interfaces available to controllers (depending on hardware component state) std::vector available_state_interfaces_; From d356f305fe7070983822cabc9f2be550489a7ef1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 23 Jan 2024 15:18:15 +0100 Subject: [PATCH 16/16] Enforce limits before calling write method of the hardware --- hardware_interface/src/resource_manager.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 84e1a24d94..9746835cb9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1403,6 +1403,14 @@ HardwareReadWriteStatus ResourceManager::write( } }; + // Enforce limits on the commands + for (auto & hw_limiter : resource_storage_->hw_command_limiter_) + { + for (auto & joint_limiters : hw_limiter.second) + { + joint_limiters.enforce_limits(period); + } + } write_components(resource_storage_->actuators_); write_components(resource_storage_->systems_);