Add joint limiter interface plugins to enforce limits defined in the …

…URDF (#1526)

joint_limits/CMakeLists.txt

@@ -44,14 +44,28 @@ target_include_directories(joint_limiter_interface PUBLIC
 )
 ament_target_dependencies(joint_limiter_interface PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
+add_library(joint_limits_helpers SHARED
+  src/joint_limits_helpers.cpp
+)
+target_compile_features(joint_limits_helpers PUBLIC cxx_std_17)
+target_include_directories(joint_limits_helpers PUBLIC
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/joint_limits>
+)
+ament_target_dependencies(joint_limits_helpers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 add_library(joint_saturation_limiter SHARED
   src/joint_saturation_limiter.cpp
+  src/joint_range_limiter.cpp
+  src/joint_soft_limiter.cpp
 )
 target_compile_features(joint_saturation_limiter PUBLIC cxx_std_17)
 target_include_directories(joint_saturation_limiter PUBLIC
   $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include/joint_limits>
 )
+target_link_libraries(joint_saturation_limiter PUBLIC joint_limits_helpers)
+
 ament_target_dependencies(joint_saturation_limiter PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
 
 pluginlib_export_plugin_description_file(joint_limits joint_limiters.xml)
@@ -91,6 +105,24 @@ if(BUILD_TESTING)
     rclcpp
   )
 
+  ament_add_gmock(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_range_limiter
+    pluginlib
+    rclcpp
+  )
+
+  ament_add_gmock(test_joint_soft_limiter test/test_joint_soft_limiter.cpp)
+  target_include_directories(test_joint_soft_limiter PRIVATE include)
+  target_link_libraries(test_joint_soft_limiter joint_limiter_interface)
+  ament_target_dependencies(
+    test_joint_soft_limiter
+    pluginlib
+    rclcpp
+  )
+
 endif()
 
 install(
@@ -101,6 +133,7 @@ install(TARGETS
     joint_limits
     joint_limiter_interface
     joint_saturation_limiter
+    joint_limits_helpers
   EXPORT export_joint_limits
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

joint_limits/include/joint_limits/data_structures.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__DATA_STRUCTURES_HPP_
+#define JOINT_LIMITS__DATA_STRUCTURES_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__DATA_STRUCTURES_HPP_

joint_limits/include/joint_limits/joint_limiter_interface.hpp

@@ -20,6 +20,7 @@
 #include <string>
 #include <vector>
 
+#include "joint_limits/joint_limits.hpp"
 #include "joint_limits/joint_limits_rosparam.hpp"
 #include "rclcpp/node.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
@@ -28,9 +29,8 @@
 
 namespace joint_limits
 {
-using JointLimitsStateDataType = trajectory_msgs::msg::JointTrajectoryPoint;
 
-template <typename LimitsType>
+template <typename JointLimitsStateDataType>
 class JointLimiterInterface
 {
 public:
@@ -68,55 +68,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<joint_limits::JointLimits> 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)
     {
@@ -128,6 +131,34 @@ class JointLimiterInterface
     return result;
   }
 
+  /**
+   * 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_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;
+    soft_joint_limits_ = soft_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.
@@ -177,19 +208,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.
-   */
-  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.
    *
@@ -219,27 +237,32 @@ 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.
    */
-  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_;
-  std::vector<LimitsType> joint_limits_;
+  std::vector<joint_limits::JointLimits> joint_limits_;
+  std::vector<joint_limits::SoftJointLimits> soft_joint_limits_;
   rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_param_itf_;
   rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_itf_;
 
 private:
   rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_;
-  realtime_tools::RealtimeBuffer<std::vector<LimitsType>> updated_limits_;
+  realtime_tools::RealtimeBuffer<std::vector<joint_limits::JointLimits>> updated_limits_;
 };
 
 }  // namespace joint_limits

joint_limits/include/joint_limits/joint_limits.hpp

@@ -67,7 +67,7 @@ struct JointLimits
   bool has_effort_limits;
   bool angle_wraparound;
 
-  std::string to_string()
+  std::string to_string() const
   {
     std::stringstream ss_output;
 
@@ -124,7 +124,7 @@ struct SoftJointLimits
   double k_position;
   double k_velocity;
 
-  std::string to_string()
+  std::string to_string() const
   {
     std::stringstream ss_output;