Refactor JointSaturationLimiter to use member variables

Move temporary calculation buffers and limit status vectors from local
scope to class members to avoid repeated heap allocations and simplify
data access across refactored methods. Split logic into dedicated
methods
for clamping and braking.

Signed-off-by: Sachin Kumar <sachinkum123567@gmail.com>

Author: sachinkum0009

joint_limits/include/joint_limits/joint_saturation_limiter.hpp

@@ -50,6 +50,18 @@ 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);
     return true;
   }
 
@@ -91,6 +103,41 @@ 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_;
+
+  /**
+   * @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 +161,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_