Add new joint range limiter for hardware interfaces

joint_limits/CMakeLists.txt

@@ -45,6 +45,7 @@ target_compile_definitions(joint_limiter_interface PRIVATE "JOINT_LIMITS_BUILDIN
 
 add_library(joint_saturation_limiter SHARED
   src/joint_saturation_limiter.cpp
+  src/joint_range_limiter.cpp
 )
 target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17)
 target_include_directories(joint_saturation_limiter PUBLIC
@@ -93,6 +94,15 @@ if(BUILD_TESTING)
     rclcpp
   )
 
+  ament_add_gtest(test_joint_range_limiter test/test_joint_range_limiter.cpp)
+  target_include_directories(test_joint_range_limiter PRIVATE include)
+  target_link_libraries(test_joint_range_limiter joint_limiter_interface)
+  ament_target_dependencies(
+    test_joint_saturation_limiter
+    pluginlib
+    rclcpp
+  )
+
 endif()
 
 install(

joint_limits/include/joint_limits/joint_limiter_interface.hpp

@@ -30,9 +30,8 @@
 
 namespace joint_limits
 {
-using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint;
 
-template <typename LimitsType>
+template <typename LimitsType, typename JointLimitsStateDataType>
 class JointLimiterInterface
 {
 public:
@@ -70,55 +69,58 @@ class JointLimiterInterface
     // TODO(destogl): get limits from URDF
 
     // Initialize and get joint limits from parameter server
-    for (size_t i = 0; i < number_of_joints_; ++i)
+    if (has_parameter_interface())
     {
-      if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_))
-      {
-        RCLCPP_ERROR(
-          node_logging_itf_->get_logger(),
-          "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str());
-        result = false;
-        break;
-      }
-      if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i]))
+      for (size_t i = 0; i < number_of_joints_; ++i)
       {
-        RCLCPP_ERROR(
-          node_logging_itf_->get_logger(),
-          "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str());
-        result = false;
-        break;
+        if (!declare_parameters(joint_names[i], node_param_itf_, node_logging_itf_))
+        {
+          RCLCPP_ERROR(
+            node_logging_itf_->get_logger(),
+            "JointLimiter: Joint '%s': parameter declaration has failed", joint_names[i].c_str());
+          result = false;
+          break;
+        }
+        if (!get_joint_limits(joint_names[i], node_param_itf_, node_logging_itf_, joint_limits_[i]))
+        {
+          RCLCPP_ERROR(
+            node_logging_itf_->get_logger(),
+            "JointLimiter: Joint '%s': getting parameters has failed", joint_names[i].c_str());
+          result = false;
+          break;
+        }
+        RCLCPP_INFO(
+          node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i,
+          joint_names[i].c_str(), joint_limits_[i].to_string().c_str());
       }
-      RCLCPP_INFO(
-        node_logging_itf_->get_logger(), "Limits for joint %zu (%s) are:\n%s", i,
-        joint_names[i].c_str(), joint_limits_[i].to_string().c_str());
-    }
-    updated_limits_.writeFromNonRT(joint_limits_);
+      updated_limits_.writeFromNonRT(joint_limits_);
 
-    auto on_parameter_event_callback = [this](const std::vector<rclcpp::Parameter> & parameters)
-    {
-      rcl_interfaces::msg::SetParametersResult set_parameters_result;
-      set_parameters_result.successful = true;
+      auto on_parameter_event_callback = [this](const std::vector<rclcpp::Parameter> & parameters)
+      {
+        rcl_interfaces::msg::SetParametersResult set_parameters_result;
+        set_parameters_result.successful = true;
 
-      std::vector<LimitsType> updated_joint_limits = joint_limits_;
-      bool changed = false;
+        std::vector<LimitsType> updated_joint_limits = joint_limits_;
+        bool changed = false;
 
-      for (size_t i = 0; i < number_of_joints_; ++i)
-      {
-        changed |= joint_limits::check_for_limits_update(
-          joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]);
-      }
+        for (size_t i = 0; i < number_of_joints_; ++i)
+        {
+          changed |= joint_limits::check_for_limits_update(
+            joint_names_[i], parameters, node_logging_itf_, updated_joint_limits[i]);
+        }
 
-      if (changed)
-      {
-        updated_limits_.writeFromNonRT(updated_joint_limits);
-        RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!");
-      }
+        if (changed)
+        {
+          updated_limits_.writeFromNonRT(updated_joint_limits);
+          RCLCPP_INFO(node_logging_itf_->get_logger(), "Limits are dynamically updated!");
+        }
 
-      return set_parameters_result;
-    };
+        return set_parameters_result;
+      };
 
-    parameter_callback_ =
-      node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback);
+      parameter_callback_ =
+        node_param_itf_->add_on_set_parameters_callback(on_parameter_event_callback);
+    }
 
     if (result)
     {
@@ -130,6 +132,31 @@ class JointLimiterInterface
     return result;
   }
 
+  /**
+   * Wrapper init method that accepts the joint names and their limits directly
+   */
+  JOINT_LIMITS_PUBLIC virtual bool init(
+    const std::vector<std::string> & joint_names, const std::vector<LimitsType> & joint_limits,
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf,
+    const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf)
+  {
+    number_of_joints_ = joint_names.size();
+    joint_names_ = joint_names;
+    joint_limits_ = joint_limits;
+    node_param_itf_ = param_itf;
+    node_logging_itf_ = logging_itf;
+    updated_limits_.writeFromNonRT(joint_limits_);
+
+    if ((number_of_joints_ != joint_limits_.size()) && has_logging_interface())
+    {
+      RCLCPP_ERROR(
+        node_logging_itf_->get_logger(),
+        "JointLimiter: Number of joint names and limits do not match: %zu != %zu",
+        number_of_joints_, joint_limits_.size());
+    }
+    return (number_of_joints_ == joint_limits_.size()) && on_init();
+  }
+
   /**
    * Wrapper init method that accepts pointer to the Node.
    * For details see other init method.
@@ -179,19 +206,6 @@ class JointLimiterInterface
     return on_enforce(current_joint_states, desired_joint_states, dt);
   }
 
-  /** \brief Enforce joint limits to desired joint state for single physical quantity.
-   *
-   * Generic enforce method that calls implementation-specific `on_enforce` method.
-   *
-   * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
-   * \returns true if limits are enforced, otherwise false.
-   */
-  JOINT_LIMITS_PUBLIC virtual bool enforce(std::vector<double> & desired_joint_states)
-  {
-    joint_limits_ = *(updated_limits_.readFromRT());
-    return on_enforce(desired_joint_states);
-  }
-
 protected:
   /** \brief Method is realized by an implementation.
    *
@@ -222,17 +236,21 @@ class JointLimiterInterface
     JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0;
 
-  /** \brief Method is realized by an implementation.
+  /** \brief Checks if the logging interface is set.
+   * \returns true if the logging interface is available, otherwise false.
    *
-   * Filter-specific implementation of the joint limits enforce algorithm for single physical
-   * quantity.
-   * This method might use "effort" limits since they are often used as wild-card.
-   * Check the documentation of the exact implementation for more details.
+   * \note this way of interfacing would be useful for instances where the logging interface is not
+   * available, for example in the ResourceManager or ResourceStorage classes.
+   */
+  bool has_logging_interface() const { return node_logging_itf_ != nullptr; }
+
+  /** \brief Checks if the parameter interface is set.
+   * \returns true if the parameter interface is available, otherwise false.
    *
-   * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits.
-   * \returns true if limits are enforced, otherwise false.
+   * * \note this way of interfacing would be useful for instances where the logging interface is
+   * not available, for example in the ResourceManager or ResourceStorage classes.
    */
-  JOINT_LIMITS_PUBLIC virtual bool on_enforce(std::vector<double> & desired_joint_states) = 0;
+  bool has_parameter_interface() const { return node_param_itf_ != nullptr; }
 
   size_t number_of_joints_;
   std::vector<std::string> joint_names_;

joint_limits/include/joint_limits/joint_limiter_struct.hpp

@@ -0,0 +1,66 @@
+// 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>
+
+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(); }
+};
+
+}  // namespace joint_limits
+#endif  // JOINT_LIMITS__JOINT_LIMITER_STRUCT_HPP_

joint_limits/include/joint_limits/joint_saturation_limiter.hpp

@@ -36,8 +36,8 @@ namespace joint_limits
  * limit. For example, if a joint is close to its position limit, velocity and acceleration will be
  * reduced accordingly.
  */
-template <typename LimitsType>
-class JointSaturationLimiter : public JointLimiterInterface<LimitsType>
+template <typename LimitsType, typename JointLimitsStateDataType>
+class JointSaturationLimiter : public JointLimiterInterface<LimitsType, JointLimitsStateDataType>
 {
 public:
   /** \brief Constructor */
@@ -49,8 +49,9 @@ class JointSaturationLimiter : public JointLimiterInterface<LimitsType>
   JOINT_LIMITS_PUBLIC bool on_init() override { return true; }
 
   JOINT_LIMITS_PUBLIC bool on_configure(
-    const trajectory_msgs::msg::JointTrajectoryPoint & /*current_joint_states*/) override
+    const JointLimitsStateDataType & current_joint_states) override
   {
+    prev_command_ = current_joint_states;
     return true;
   }
 
@@ -70,33 +71,23 @@ class JointSaturationLimiter : public JointLimiterInterface<LimitsType>
    * \returns true if limits are enforced, otherwise false.
    */
   JOINT_LIMITS_PUBLIC bool on_enforce(
-    trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
-    trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states,
-    const rclcpp::Duration & dt) override;
-
-  /** \brief Enforce joint limits to desired arbitrary physical quantity.
-   * Additional, commonly used method for enforcing limits by clamping desired input value.
-   * The limit is defined in effort limits of the `joint::limits/JointLimit` structure.
-   *
-   * If `has_effort_limits` is set to false, the limits will be not enforced to a joint.
-   *
-   * \param[in,out] desired_joint_states physical quantity that should be adjusted to obey the
-   * limits. \returns true if limits are enforced, otherwise false.
-   */
-  JOINT_LIMITS_PUBLIC bool on_enforce(std::vector<double> & desired_joint_states) override;
+    JointLimitsStateDataType & current_joint_states,
+    JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override;
 
 private:
   rclcpp::Clock::SharedPtr clock_;
+  JointLimitsStateDataType prev_command_;
 };
 
-template <typename LimitsType>
-JointSaturationLimiter<LimitsType>::JointSaturationLimiter() : JointLimiterInterface<LimitsType>()
+template <typename LimitsType, typename JointLimitsStateDataType>
+JointSaturationLimiter<LimitsType, JointLimitsStateDataType>::JointSaturationLimiter()
+: JointLimiterInterface<LimitsType, JointLimitsStateDataType>()
 {
   clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock(RCL_ROS_TIME));
 }
 
-template <typename LimitsType>
-JointSaturationLimiter<LimitsType>::~JointSaturationLimiter()
+template <typename LimitsType, typename JointLimitsStateDataType>
+JointSaturationLimiter<LimitsType, JointLimitsStateDataType>::~JointSaturationLimiter()
 {
 }
 

joint_limits/joint_limiters.xml

@@ -1,11 +1,18 @@
 <class_libraries>
   <library path="joint_saturation_limiter">
-    <class name="joint_limits/JointSaturationLimiter"
-          type="joint_limits::JointSaturationLimiter&lt;joint_limits::JointLimits&gt;"
-          base_class_type="joint_limits::JointLimiterInterface&lt;joint_limits::JointLimits&gt;">
+    <class name="joint_limits/JointTrajectoryPointSaturationLimiter"
+          type="JointTrajectoryPointSaturationLimiter"
+          base_class_type="joint_limits::JointLimiterInterface&lt;joint_limits::JointLimits, trajectory_msgs::msg::JointTrajectoryPoint&gt;">
       <description>
         Simple joint limiter using clamping approach. Warning: clamping can cause discontinuities.
       </description>
     </class>
+    <class name="joint_limits/JointInterfacesSaturationLimiter"
+          type="JointInterfacesSaturationLimiter"
+          base_class_type="joint_limits::JointLimiterInterface&lt;joint_limits::JointLimits, joint_limits::JointControlInterfacesData&gt;">
+      <description>
+        Simple joint range limiter using clamping approach with the parsed limits.
+      </description>
+    </class>
   </library>
 </class_libraries>