Integrate joint limit enforcement into ros2_control framework

controller_manager/doc/userdoc.rst

@@ -57,6 +57,11 @@ robot_description [std_msgs::msg::String]
 Parameters
 -----------
 
+enforce_command_limits (optional; bool; default: ``false`` for Jazzy and earlier distro and ``true`` for Rolling and newer distros)
+  Enforces the joint limits to the joint command interfaces.
+  If the command is outside the limits, the command is clamped to be within the limits depending on the type of configured joint limits in the URDF.
+  If the command is within the limits, the command is passed through without any changes.
+
 <controller_name>.type
   Name of a plugin exported using ``pluginlib`` for a controller.
   This is a class from which controller's instance with name "``controller_name``" is created.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -563,6 +563,7 @@ class ControllerManager : public rclcpp::Node
   std::string robot_description_;
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr robot_description_subscription_;
   rclcpp::TimerBase::SharedPtr robot_description_notification_timer_;
+  bool enforce_command_limits_;
 
   controller_manager::MovingAverageStatistics periodicity_stats_;
 

controller_manager/src/controller_manager.cpp

@@ -278,7 +278,8 @@ ControllerManager::ControllerManager(
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
       kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
-  cm_node_options_(options)
+  cm_node_options_(options),
+  robot_description_(resource_manager_->get_urdf())
 {
   initialize_parameters();
   init_controller_manager();
@@ -289,6 +290,7 @@ void ControllerManager::init_controller_manager()
   // Get parameters needed for RT "update" loop to work
   if (is_resource_manager_initialized())
   {
+    resource_manager_->import_joint_limiters(robot_description_);
     init_services();
   }
   else
@@ -322,6 +324,13 @@ void ControllerManager::init_controller_manager()
   diagnostics_updater_.add(
     "Controller Manager Activity", this,
     &ControllerManager::controller_manager_diagnostic_callback);
+
+  // Declare the enforce_command_limits parameter such a way that it is enabled by default for
+  // rolling and newer alone
+  enforce_command_limits_ =
+    this->get_parameter_or("enforce_command_limits", RCLCPP_VERSION_MAJOR >= 29 ? true : false);
+  RCLCPP_INFO_EXPRESSION(
+    get_logger(), enforce_command_limits_, "Enforcing command limits is enabled...");
 }
 
 void ControllerManager::initialize_parameters()
@@ -363,6 +372,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       get_logger(),
       "Resource Manager has been successfully initialized. Starting Controller Manager "
       "services...");
+    resource_manager_->import_joint_limiters(robot_description_);
     init_services();
   }
 }
@@ -2600,6 +2610,11 @@ controller_interface::return_type ControllerManager::update(
     }
   }
 
+  if (enforce_command_limits_)
+  {
+    resource_manager_->enforce_command_limits(period);
+  }
+
   // there are controllers to (de)activate
   if (switch_params_.do_switch)
   {

controller_manager/test/test_controller_manager_hardware_error_handling.cpp

@@ -66,6 +66,12 @@ class TestControllerManagerWithTestableCM
   public testing::WithParamInterface<Strictness>
 {
 public:
+  TestControllerManagerWithTestableCM()
+  : ControllerManagerFixture<TestableControllerManager>(
+      ros2_control_test_assets::minimal_robot_urdf_no_limits)
+  {
+  }
+
   void SetupAndConfigureControllers(int strictness)
   {
     test_controller_actuator = std::make_shared<test_controller::TestController>();

controller_manager/test/test_controllers_chaining_with_controller_manager.cpp

@@ -14,6 +14,7 @@
 
 #include <gtest/gtest.h>
 #include <memory>
+#include <regex>
 #include <string>
 #include <unordered_map>
 #include <utility>
@@ -106,9 +107,13 @@ class TestControllerChainingWithControllerManager
   void SetUp()
   {
     executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    const std::regex velocity_pattern(R"(velocity\s*=\s*"-?[0-9]+(\.[0-9]+)?")");
+    const std::string velocity_replacement = R"(velocity="10000.0")";
+    const std::string diffbot_urdf_large_limits = std::regex_replace(
+      ros2_control_test_assets::diffbot_urdf, velocity_pattern, velocity_replacement);
     cm_ = std::make_shared<TestableControllerManager>(
       std::make_unique<hardware_interface::ResourceManager>(
-        ros2_control_test_assets::diffbot_urdf, rm_node_->get_node_clock_interface(),
+        diffbot_urdf_large_limits, rm_node_->get_node_clock_interface(),
         rm_node_->get_node_logging_interface(), true),
       executor_, TEST_CM_NAME);
     run_updater_ = false;

doc/release_notes.rst

@@ -86,6 +86,7 @@ controller_manager
 * The ``--service-call-timeout`` was added as parameter to the helper scripts ``spawner.py``. Useful when the CPU load is high at startup and the service call does not return immediately (`#1808 <https://github.com/ros-controls/ros2_control/pull/1808>`_).
 * The ``cpu_affinity`` parameter can now accept of types ``int`` or ``int_array`` to bind the process to a specific CPU core or multiple CPU cores. (`#1915 <https://github.com/ros-controls/ros2_control/pull/1915>`_).
 * A python module ``test_utils`` was added to the ``controller_manager`` package to help with integration testing (`#1955 <https://github.com/ros-controls/ros2_control/pull/1955>`_).
+* A new parameter ``enforce_command_limits`` is introduced to be able to enable and disable the enforcement of the command limits (`#1989 <https://github.com/ros-controls/ros2_control/pull/1989>`_).
 
 hardware_interface
 ******************

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -89,6 +89,12 @@ class ResourceManager
   virtual bool load_and_initialize_components(
     const std::string & urdf, const unsigned int update_rate = 100);
 
+  /**
+   * @brief Import joint limiters from the URDF.
+   * @param urdf string containing the URDF.
+   */
+  void import_joint_limiters(const std::string & urdf);
+
   /**
    * @brief if the resource manager load_and_initialize_components(...) function has been called
    * this returns true. We want to permit to loading the urdf later on, but we currently don't want
@@ -426,6 +432,14 @@ class ResourceManager
   return_type set_component_state(
     const std::string & component_name, rclcpp_lifecycle::State & target_state);
 
+  /**
+   * Enforce the command limits for the position, velocity, effort, and acceleration interfaces.
+   * @note This method is RT-safe
+   * @return true if the command interfaces are out of limits and the limits are enforced
+   * @return false if the command interfaces values are within limits
+   */
+  bool enforce_command_limits(const rclcpp::Duration & period);
+
   /// Reads all loaded hardware components.
   /**
    * Reads from all active hardware components.
@@ -457,6 +471,8 @@ class ResourceManager
    */
   bool state_interface_exists(const std::string & key) const;
 
+  const std::string & get_urdf() const;
+
 protected:
   /// Gets the logger for the resource manager
   /**

hardware_interface/src/resource_manager.cpp

@@ -32,6 +32,9 @@
 #include "hardware_interface/sensor_interface.hpp"
 #include "hardware_interface/system.hpp"
 #include "hardware_interface/system_interface.hpp"
+#include "joint_limits/joint_limits_helpers.hpp"
+#include "joint_limits/joint_saturation_limiter.hpp"
+#include "joint_limits/joint_soft_limiter.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "rclcpp/logging.hpp"
@@ -657,6 +660,106 @@ class ResourceStorage
     }
   }
 
+  void import_joint_limiters(const std::vector<HardwareInfo> & hardware_infos)
+  {
+    for (const auto & hw_info : hardware_infos)
+    {
+      limiters_data_[hw_info.name] = {};
+      for (const auto & [joint_name, limits] : hw_info.limits)
+      {
+        std::vector<joint_limits::SoftJointLimits> soft_limits;
+        const std::vector<joint_limits::JointLimits> hard_limits{limits};
+        joint_limits::JointInterfacesCommandLimiterData data;
+        data.joint_name = joint_name;
+        limiters_data_[hw_info.name].push_back(data);
+        // If the joint limits is found in the softlimits, then extract it
+        if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end())
+        {
+          soft_limits = {hw_info.soft_limits.at(joint_name)};
+        }
+        std::unique_ptr<
+          joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>
+          limits_interface;
+        if (soft_limits.empty())
+        {
+          limits_interface = std::make_unique<
+            joint_limits::JointSaturationLimiter<joint_limits::JointControlInterfacesData>>();
+        }
+        else
+        {
+          limits_interface = std::make_unique<joint_limits::JointSoftLimiter>();
+        }
+        limits_interface->init({joint_name}, hard_limits, soft_limits, nullptr, nullptr);
+        joint_limiters_interface_[hw_info.name].push_back(std::move(limits_interface));
+      }
+    }
+  }
+
+  template <typename T>
+  void update_joint_limiters_data(
+    const std::string & joint_name, const std::map<std::string, T> & interface_map,
+    joint_limits::JointControlInterfacesData & data, bool is_command_itf = false)
+  {
+    data.joint_name = joint_name;
+
+    const auto fill_interface_data =
+      [&](const std::string & interface_type, std::optional<double> & value)
+    {
+      const std::string interface_name = joint_name + "/" + interface_type;
+      if (interface_map.find(interface_name) != interface_map.end())
+      {
+        // If the command interface is not claimed, then the value is not set
+        if (is_command_itf && !claimed_command_interface_map_.at(interface_name))
+        {
+          value = std::nullopt;
+        }
+        else
+        {
+          value = interface_map.at(interface_name)->get_value();
+        }
+      }
+    };
+    // update the actual data of the limiters
+    fill_interface_data(hardware_interface::HW_IF_POSITION, data.position);
+    fill_interface_data(hardware_interface::HW_IF_VELOCITY, data.velocity);
+    fill_interface_data(hardware_interface::HW_IF_EFFORT, data.effort);
+    fill_interface_data(hardware_interface::HW_IF_ACCELERATION, data.acceleration);
+  }
+
+  template <typename T>
+  void update_joint_limiters_commands(
+    const joint_limits::JointControlInterfacesData & state,
+    std::map<std::string, T> & interface_map)
+  {
+    const auto set_interface_command =
+      [&](const std::string & interface_type, const std::optional<double> & data)
+    {
+      const std::string interface_name = state.joint_name + "/" + interface_type;
+      if (interface_map.find(interface_name) != interface_map.end() && data.has_value())
+      {
+        interface_map.at(interface_name)->set_value(data.value());
+      }
+    };
+    // update the command data of the limiters
+    set_interface_command(hardware_interface::HW_IF_POSITION, state.position);
+    set_interface_command(hardware_interface::HW_IF_VELOCITY, state.velocity);
+    set_interface_command(hardware_interface::HW_IF_EFFORT, state.effort);
+    set_interface_command(hardware_interface::HW_IF_ACCELERATION, state.acceleration);
+  }
+
+  void update_joint_limiters_data()
+  {
+    for (auto & joint_limiter_data : limiters_data_)
+    {
+      for (auto & data : joint_limiter_data.second)
+      {
+        update_joint_limiters_data(data.joint_name, state_interface_map_, data.actual);
+        update_joint_limiters_data(data.joint_name, command_interface_map_, data.command, true);
+        data.limited = data.command;
+      }
+    }
+  }
+
   std::string add_state_interface(StateInterface::ConstSharedPtr interface)
   {
     auto interface_name = interface->get_name();
@@ -1007,6 +1110,16 @@ class ResourceStorage
   /// List of all claimed command interfaces
   std::unordered_map<std::string, bool> claimed_command_interface_map_;
 
+  std::unordered_map<std::string, std::vector<joint_limits::JointInterfacesCommandLimiterData>>
+    limiters_data_;
+
+  std::unordered_map<
+    std::string, std::vector<std::unique_ptr<
+                   joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>>>
+    joint_limiters_interface_;
+
+  std::string robot_description_;
+
   // Update rate of the controller manager, and the clock interface of its node
   // Used by async components.
   unsigned int cm_update_rate_ = 100;
@@ -1027,6 +1140,7 @@ ResourceManager::ResourceManager(
   const unsigned int update_rate)
 : resource_storage_(std::make_unique<ResourceStorage>(clock_interface, logger_interface))
 {
+  resource_storage_->robot_description_ = urdf;
   load_and_initialize_components(urdf, update_rate);
 
   if (activate_all)
@@ -1046,6 +1160,7 @@ bool ResourceManager::load_and_initialize_components(
 {
   components_are_loaded_and_initialized_ = true;
 
+  resource_storage_->robot_description_ = urdf;
   resource_storage_->cm_update_rate_ = update_rate;
 
   auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
@@ -1123,6 +1238,12 @@ bool ResourceManager::load_and_initialize_components(
   return components_are_loaded_and_initialized_;
 }
 
+void ResourceManager::import_joint_limiters(const std::string & urdf)
+{
+  const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
+  resource_storage_->import_joint_limiters(hardware_info);
+}
+
 bool ResourceManager::are_components_initialized() const
 {
   return components_are_loaded_and_initialized_;
@@ -1726,6 +1847,26 @@ return_type ResourceManager::set_component_state(
   return result;
 }
 
+// CM API: Called in "update"-thread
+bool ResourceManager::enforce_command_limits(const rclcpp::Duration & period)
+{
+  bool enforce_result = false;
+  // Joint Limiters operations
+  resource_storage_->update_joint_limiters_data();
+  for (auto & [hw_name, limiters] : resource_storage_->joint_limiters_interface_)
+  {
+    for (size_t i = 0; i < limiters.size(); i++)
+    {
+      joint_limits::JointInterfacesCommandLimiterData & data =
+        resource_storage_->limiters_data_[hw_name][i];
+      enforce_result |= limiters[i]->enforce(data.actual, data.limited, period);
+      resource_storage_->update_joint_limiters_commands(
+        data.limited, resource_storage_->command_interface_map_);
+    }
+  }
+  return enforce_result;
+}
+
 // CM API: Called in "update"-thread
 HardwareReadWriteStatus ResourceManager::read(
   const rclcpp::Time & time, const rclcpp::Duration & period)
@@ -1919,7 +2060,10 @@ bool ResourceManager::state_interface_exists(const std::string & key) const
   return resource_storage_->state_interface_map_.find(key) !=
          resource_storage_->state_interface_map_.end();
 }
-
+const std::string & ResourceManager::get_urdf() const
+{
+  return resource_storage_->robot_description_;
+}
 // END: "used only in tests and locally"
 
 rclcpp::Logger ResourceManager::get_logger() const { return resource_storage_->get_logger(); }