✨ Joint limits resource manager integration

hardware_interface/CMakeLists.txt

@@ -15,6 +15,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rcutils
   TinyXML2
   tinyxml2_vendor
+  joint_limits
   urdf
 )
 

hardware_interface/include/hardware_interface/hardware_info.hpp

@@ -18,6 +18,7 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+#include "joint_limits/joint_limits.hpp"
 
 namespace hardware_interface
 {
@@ -163,6 +164,17 @@ 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<std::string, joint_limits::JointLimits> limits;
+
+  /**
+   * Map of software joint limits used for clamping the command where the key is the joint name.
+   * Optional If not specified or less restrictive than the JointLimits uses the previous
+   * JointLimits.
+   */
+  std::unordered_map<std::string, joint_limits::SoftJointLimits> soft_limits;
 };
 
 }  // namespace hardware_interface

hardware_interface/package.xml

@@ -16,6 +16,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>rcpputils</depend>
   <depend>tinyxml2_vendor</depend>
+  <depend>joint_limits</depend>
   <depend>urdf</depend>
 
   <build_depend>rcutils</build_depend>

hardware_interface/src/resource_manager.cpp

@@ -34,6 +34,7 @@
 #include "hardware_interface/system.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "joint_limits/joint_limiter_struct.hpp"
 #include "lifecycle_msgs/msg/state.hpp"
 #include "pluginlib/class_loader.hpp"
 #include "rcutils/logging_macros.h"
@@ -454,6 +455,123 @@ class ResourceStorage
     hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces);
   }
 
+  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;
+        limits_interface = std::make_unique<
+          joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>();
+        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_state(
+    const std::string & joint_name, const std::map<std::string, T> & interface_map,
+    joint_limits::JointControlInterfacesData & state)
+  {
+    state.joint_name = joint_name;
+    // update the actual state of the limiters
+    if (
+      interface_map.find(joint_name + "/" + hardware_interface::HW_IF_POSITION) !=
+      interface_map.end())
+    {
+      state.position =
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_POSITION).get_value();
+    }
+    if (
+      interface_map.find(joint_name + "/" + hardware_interface::HW_IF_VELOCITY) !=
+      interface_map.end())
+    {
+      state.velocity =
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_VELOCITY).get_value();
+    }
+    if (
+      interface_map.find(joint_name + "/" + hardware_interface::HW_IF_EFFORT) !=
+      interface_map.end())
+    {
+      state.effort =
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_EFFORT).get_value();
+    }
+    if (
+      interface_map.find(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) !=
+      interface_map.end())
+    {
+      state.acceleration =
+        interface_map.at(joint_name + "/" + hardware_interface::HW_IF_ACCELERATION).get_value();
+    }
+  }
+
+  template <typename T>
+  void update_joint_limiters_commands(
+    const joint_limits::JointControlInterfacesData & state,
+    std::map<std::string, T> & interface_map)
+  {
+    if (
+      interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_POSITION) !=
+        interface_map.end() &&
+      state.position.has_value())
+    {
+      interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_POSITION)
+        .set_value(state.position.value());
+    }
+    if (
+      interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY) !=
+        interface_map.end() &&
+      state.velocity.has_value())
+    {
+      interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_VELOCITY)
+        .set_value(state.velocity.value());
+    }
+    if (
+      interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT) !=
+        interface_map.end() &&
+      state.effort.has_value())
+    {
+      interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_EFFORT)
+        .set_value(state.effort.value());
+    }
+    if (
+      interface_map.find(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION) !=
+        interface_map.end() &&
+      state.acceleration.has_value())
+    {
+      interface_map.at(state.joint_name + "/" + hardware_interface::HW_IF_ACCELERATION)
+        .set_value(state.acceleration.value());
+    }
+  }
+
+  void update_joint_limiters_data()
+  {
+    for (auto & joint_limiter_data : limiters_data_)
+    {
+      for (auto & data : joint_limiter_data.second)
+      {
+        update_joint_limiters_state(data.joint_name, state_interface_map_, data.actual);
+        update_joint_limiters_state(data.joint_name, command_interface_map_, data.command);
+        data.limited = data.command;
+      }
+    }
+  }
+
   /// Adds exported command interfaces into internal storage.
   /**
    * Add command interfaces to the internal storage. Command interfaces exported from hardware or
@@ -723,6 +841,14 @@ class ResourceStorage
   /// List of async components by type
   std::unordered_map<std::string, AsyncComponentThread> async_component_threads_;
 
+  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_;
+
   // Update rate of the controller manager, and the clock interface of its node
   // Used by async components.
   unsigned int cm_update_rate_;
@@ -765,6 +891,7 @@ void ResourceManager::load_urdf(
   const std::string actuator_type = "actuator";
 
   const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
+  resource_storage_->import_joint_limiters(hardware_info);
   if (load_and_initialize_components)
   {
     for (const auto & individual_hardware_info : hardware_info)
@@ -1372,6 +1499,22 @@ HardwareReadWriteStatus ResourceManager::write(
   read_write_status.ok = true;
   read_write_status.failed_hardware_names.clear();
 
+  // 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];
+        limiters[i]->enforce(data.actual, data.limited, period);
+        resource_storage_->update_joint_limiters_commands(
+          data.limited, resource_storage_->command_interface_map_);
+      }
+    }
+  }
+
   auto write_components = [&](auto & components)
   {
     for (auto & component : components)

joint_limits/include/joint_limits/joint_limiter_struct.hpp

@@ -0,0 +1,150 @@
+// 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 Sai Kishor Kothakota
+
+#ifndef JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_
+#define JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_
+
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include "joint_limits/joint_limits.hpp"
+#include "joint_limits/joint_limits_rosparam.hpp"
+
+namespace joint_limits
+{
+
+struct JointControlInterfacesData
+{
+  std::string joint_name;
+  std::optional<double> position = std::nullopt;
+  std::optional<double> velocity = std::nullopt;
+  std::optional<double> effort = std::nullopt;
+  std::optional<double> acceleration = std::nullopt;
+  std::optional<double> jerk = std::nullopt;
+
+  bool has_data() const
+  {
+    return has_position() || has_velocity() || has_effort() || has_acceleration() || has_jerk();
+  }
+
+  bool has_position() const { return position.has_value(); }
+
+  bool has_velocity() const { return velocity.has_value(); }
+
+  bool has_effort() const { return effort.has_value(); }
+
+  bool has_acceleration() const { return acceleration.has_value(); }
+
+  bool has_jerk() const { return jerk.has_value(); }
+};
+
+struct JointInterfacesCommandLimiterData
+{
+  std::string joint_name;
+  JointControlInterfacesData actual;
+  JointControlInterfacesData command;
+  JointControlInterfacesData prev_command;
+  JointControlInterfacesData limited;
+
+  bool has_actual_data() const { return actual.has_data(); }
+
+  bool has_command_data() const { return command.has_data(); }
+};
+
+template <typename JointLimitsStateDataType>
+class JointLimiterInterface
+{
+public:
+  JointLimiterInterface() = default;
+
+  virtual ~JointLimiterInterface() = default;
+
+  /**
+   * Wrapper init method that accepts the joint names and their limits directly
+   */
+  virtual bool init(
+    const std::vector<std::string> & joint_names,
+    const std::vector<joint_limits::JointLimits> & joint_limits,
+    const std::vector<joint_limits::SoftJointLimits> & soft_limits,
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf)
+  {
+    return on_init();
+  }
+
+  /** \brief Initialize joint limits limiter.
+   *
+   * Generic initialization method that calls implementation-specific `on_init` method.
+   * \returns true if initialization was successful, otherwise false.
+   */
+  virtual bool configure(const JointLimitsStateDataType & current_joint_states)
+  {
+    return on_configure(current_joint_states);
+  }
+
+  /** \brief Enforce joint limits to desired joint state for multiple physical quantities.
+   *
+   * Generic enforce method that calls implementation-specific `on_enforce` method.
+   *
+   * \param[in] current_joint_states current joint states a robot is in.
+   * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+   * \param[in] dt time delta to calculate missing integrals and derivation in joint limits.
+   * \returns true if limits are enforced, otherwise false.
+   */
+  virtual bool enforce(
+    JointLimitsStateDataType & current_joint_states,
+    JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
+  {
+    return on_enforce(current_joint_states, desired_joint_states, dt);
+  }
+
+protected:
+  /** \brief Method is realized by an implementation.
+   *
+   * Implementation-specific initialization of limiter's internal states and libraries.
+   * \returns true if initialization was successful, otherwise false.
+   */
+  virtual bool on_init() { return true; }
+
+  /** \brief Method is realized by an implementation.
+   *
+   * Implementation-specific configuration of limiter's internal states and libraries.
+   * \returns true if initialization was successful, otherwise false.
+   */
+  virtual bool on_configure(const JointLimitsStateDataType & current_joint_states) { return true; }
+
+  /** \brief Method is realized by an implementation.
+   *
+   * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent
+   * physical quantities.
+   *
+   * \param[in] current_joint_states current joint states a robot is in.
+   * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
+   * \param[in] dt time delta to calculate missing integrals and derivation in joint limits.
+   * \returns true if limits are enforced, otherwise false.
+   */
+  virtual bool on_enforce(
+    JointLimitsStateDataType & current_joint_states,
+    JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
+  {
+    return false;
+  }
+};
+
+}  // namespace joint_limits
+#endif  // JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_