Joint limits resource manager integration

hardware_interface/src/resource_manager.cpp

@@ -34,6 +34,10 @@
 #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_interface.hpp"
+#include "joint_limits/joint_limiter_struct.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 "rcutils/logging_macros.h"
@@ -662,6 +666,130 @@ 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::unique_ptr<
+          joint_limits::JointLimiterInterface<joint_limits::JointControlInterfacesData>>
+          limits_interface;
+        const joint_limits::JointLimits hard_limits{limits};
+        joint_limits::JointInterfacesCommandLimiterData data;
+        data.joint_name = joint_name;
+        limiters_data_[hw_info.name].push_back(data);
+        // Check if soft limits exist. If yes then extract them and use soft limiter
+        if (hw_info.soft_limits.find(joint_name) != hw_info.soft_limits.end())
+        {
+          std::vector<joint_limits::SoftJointLimits> soft_limits = {
+            hw_info.soft_limits.at(joint_name)};
+          limits_interface = std::make_unique<
+            joint_limits::SoftJointLimiter<joint_limits::JointControlInterfacesData>>();
+        }
+        // no soft limits given so we use the joint saturation limiter (joint_range_limiter.hpp)
+        else
+        {
+          limits_interface = std::make_unique<
+            joint_limits::JointSaturationLimiter<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
@@ -980,6 +1108,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_;
@@ -1022,6 +1158,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)
@@ -1689,6 +1826,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_soft_limiter.hpp

@@ -0,0 +1,70 @@
+// 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 Adrià Roig Moreno
+
+#ifndef JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_
+#define JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_
+
+#include <cmath>
+#include "joint_limits/joint_limiter_struct.hpp"
+#include "joint_limits/joint_limits_helpers.hpp"
+#include "joint_limits/joint_saturation_limiter.hpp"
+
+constexpr double VALUE_CONSIDERED_ZERO = 1e-10;
+
+namespace joint_limits
+{
+
+class SoftJointLimiter : public JointSaturationLimiter<JointControlInterfacesData>
+{
+public:
+  bool on_init() override
+  {
+    const bool result = (number_of_joints_ == 1);
+    if (!result && has_logging_interface())
+    {
+      RCLCPP_ERROR(
+        node_logging_itf_->get_logger(),
+        "JointInterfacesSaturationLimiter: Expects the number of joints to be 1, but given : "
+        "%zu",
+        number_of_joints_);
+    }
+    prev_command_ = JointControlInterfacesData();
+    return result;
+  }
+
+  bool on_enforce(
+    JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+    const rclcpp::Duration & dt) override;
+
+  bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits)
+  {
+    return std::isfinite(soft_joint_limits.min_position) &&
+           std::isfinite(soft_joint_limits.max_position) &&
+           (soft_joint_limits.max_position - soft_joint_limits.min_position) >
+             VALUE_CONSIDERED_ZERO;
+  }
+
+  bool has_soft_limits(const joint_limits::SoftJointLimits & soft_joint_limits)
+  {
+    return has_soft_position_limits(soft_joint_limits) &&
+           std::isfinite(soft_joint_limits.k_position) &&
+           std::abs(soft_joint_limits.k_position) > VALUE_CONSIDERED_ZERO;
+  }
+};
+
+}  // namespace joint_limits
+
+#endif  // JOINT_LIMITS__JOINT_SOFT_LIMITER_HPP_