Enhance JointSaturationLimiter with Jerk limits

joint_limits/include/joint_limits/data_structures.hpp

@@ -42,6 +42,7 @@ DEFINE_LIMIT_STRUCT(PositionLimits)
 DEFINE_LIMIT_STRUCT(VelocityLimits)
 DEFINE_LIMIT_STRUCT(EffortLimits)
 DEFINE_LIMIT_STRUCT(AccelerationLimits)
+DEFINE_LIMIT_STRUCT(JerkLimits)
 
 struct JointControlInterfacesData
 {

joint_limits/include/joint_limits/joint_saturation_limiter.hpp

@@ -50,6 +50,19 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
   bool on_configure(const JointLimitsStateDataType & current_joint_states) override
   {
     prev_command_ = current_joint_states;
+    const size_t num_joints = this->number_of_joints_;
+
+    desired_pos_.assign(num_joints, 0.0);
+    desired_vel_.assign(num_joints, 0.0);
+    desired_acc_.assign(num_joints, 0.0);
+    expected_pos_.assign(num_joints, 0.0);
+    expected_vel_.assign(num_joints, 0.0);
+
+    pos_limit_hit_.assign(num_joints, false);
+    vel_limit_hit_.assign(num_joints, false);
+    acc_limit_hit_.assign(num_joints, false);
+    dec_limit_hit_.assign(num_joints, false);
+    jerk_limit_hit_.assign(num_joints, false);
     return true;
   }
 
@@ -91,6 +104,42 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
   rclcpp::Clock::SharedPtr clock_;
   JointLimitsStateDataType prev_command_;
   std::mutex mutex_;
+
+private:
+  // Cached vectors to eliminate dynamic memory allocation (malloc) in the real-time execution loop
+  std::vector<double> desired_pos_;
+  std::vector<double> desired_vel_;
+  std::vector<double> desired_acc_;
+  std::vector<double> expected_vel_;
+  std::vector<double> expected_pos_;
+
+  // Pre-allocation boolean flags for tracking limits
+  std::vector<bool> pos_limit_hit_;
+  std::vector<bool> vel_limit_hit_;
+  std::vector<bool> acc_limit_hit_;
+  std::vector<bool> dec_limit_hit_;
+  std::vector<bool> jerk_limit_hit_;
+
+  /**
+   * @brief
+   * Clamps the joint limits
+   */
+  void clamp_joint_limits(
+    const bool has_desired_position, const bool has_desired_velocity,
+    const bool has_desired_acceleration, const bool has_current_velocity,
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+    trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, bool & limits_enforced,
+    const std::vector<double> & current_joint_velocities,
+    bool & braking_near_position_limit_triggered, const double dt_seconds);
+
+  /**
+   * @brief
+   * Handles the braking near position limit
+   */
+  void handle_braking_near_position_limit(
+    const std::vector<double> & current_joint_velocities, double dt_seconds,
+    bool has_desired_position, bool has_desired_velocity,
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states);
 };
 
 template <typename JointLimitsStateDataType>
@@ -114,6 +163,22 @@ bool JointSaturationLimiter<JointLimitsStateDataType>::on_init()
 template <>
 bool JointSaturationLimiter<JointControlInterfacesData>::on_init();
 
+template <>
+void JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::clamp_joint_limits(
+  const bool has_desired_position, const bool has_desired_velocity,
+  const bool has_desired_acceleration, const bool has_current_velocity,
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+  trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, bool & limits_enforced,
+  const std::vector<double> & current_joint_velocities,
+  bool & braking_near_position_limit_triggered, const double dt_seconds);
+
+template <>
+void JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::
+  handle_braking_near_position_limit(
+    const std::vector<double> & current_joint_velocities, double dt_seconds,
+    bool has_desired_position, bool has_desired_velocity,
+    const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states);
+
 }  // namespace joint_limits
 
 #endif  // JOINT_LIMITS__JOINT_SATURATION_LIMITER_HPP_