Use actual position when limiting desired position (#1988)

joint_limits/include/joint_limits/joint_limiter_interface.hpp

@@ -201,7 +201,7 @@ class JointLimiterInterface
    * \returns true if limits are enforced, otherwise false.
    */
   virtual bool enforce(
-    JointLimitsStateDataType & current_joint_states,
+    const JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt)
   {
     joint_limits_ = *(updated_limits_.readFromRT());
@@ -234,7 +234,7 @@ class JointLimiterInterface
    * \returns true if limits are enforced, otherwise false.
    */
   virtual bool on_enforce(
-    JointLimitsStateDataType & current_joint_states,
+    const JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0;
 
   /** \brief Checks if the logging interface is set.

joint_limits/include/joint_limits/joint_limits_helpers.hpp

@@ -49,7 +49,7 @@ bool is_limited(double value, double min, double max);
  */
 PositionLimits compute_position_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_vel,
-  const std::optional<double> & prev_command_pos, double dt);
+  const std::optional<double> & act_pos, const std::optional<double> & prev_command_pos, double dt);
 
 /**
  * @brief Computes the velocity limits based on the position and acceleration limits.

joint_limits/include/joint_limits/joint_saturation_limiter.hpp

@@ -67,7 +67,7 @@ class JointSaturationLimiter : public JointLimiterInterface<JointLimitsStateData
    * \returns true if limits are enforced, otherwise false.
    */
   bool on_enforce(
-    JointLimitsStateDataType & current_joint_states,
+    const JointLimitsStateDataType & current_joint_states,
     JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) override;
 
 protected:

joint_limits/include/joint_limits/joint_soft_limiter.hpp

@@ -46,7 +46,7 @@ class JointSoftLimiter : public JointSaturationLimiter<JointControlInterfacesDat
   }
 
   bool on_enforce(
-    JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+    const JointControlInterfacesData & actual, JointControlInterfacesData & desired,
     const rclcpp::Duration & dt) override;
 
   bool has_soft_position_limits(const joint_limits::SoftJointLimits & soft_joint_limits)

joint_limits/src/joint_limits_helpers.cpp

@@ -39,7 +39,7 @@ bool is_limited(double value, double min, double max) { return value < min || va
 
 PositionLimits compute_position_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_vel,
-  const std::optional<double> & prev_command_pos, double dt)
+  const std::optional<double> & act_pos, const std::optional<double> & prev_command_pos, double dt)
 {
   PositionLimits pos_limits(limits.min_position, limits.max_position);
   if (limits.has_velocity_limits)
@@ -50,8 +50,10 @@ PositionLimits compute_position_limits(
                                : limits.max_velocity;
     const double max_vel = std::min(limits.max_velocity, delta_vel);
     const double delta_pos = max_vel * dt;
-    pos_limits.lower_limit = std::max(prev_command_pos.value() - delta_pos, pos_limits.lower_limit);
-    pos_limits.upper_limit = std::min(prev_command_pos.value() + delta_pos, pos_limits.upper_limit);
+    const double position_reference =
+      act_pos.has_value() ? act_pos.value() : prev_command_pos.value();
+    pos_limits.lower_limit = std::max(position_reference - delta_pos, pos_limits.lower_limit);
+    pos_limits.upper_limit = std::min(position_reference + delta_pos, pos_limits.upper_limit);
   }
   internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit);
   return pos_limits;

joint_limits/src/joint_range_limiter.cpp

@@ -43,7 +43,7 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_init()
 
 template <>
 bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
-  JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+  const JointControlInterfacesData & actual, JointControlInterfacesData & desired,
   const rclcpp::Duration & dt)
 {
   bool limits_enforced = false;
@@ -112,8 +112,8 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
 
   if (desired.has_position())
   {
-    const auto limits =
-      compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds);
+    const auto limits = compute_position_limits(
+      joint_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds);
     limits_enforced = is_limited(desired.position.value(), limits.lower_limit, limits.upper_limit);
     desired.position = std::clamp(desired.position.value(), limits.lower_limit, limits.upper_limit);
   }

joint_limits/src/joint_saturation_limiter.cpp

@@ -27,7 +27,7 @@ namespace joint_limits
 {
 template <>
 bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enforce(
-  trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
+  const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states,
   trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_states, const rclcpp::Duration & dt)
 {
   bool limits_enforced = false;
@@ -44,23 +44,21 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
   // velocity max is implicitly already violated due to max_acc * dt > 2.0
 
   // check for required inputs combination
-  bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
-  bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_);
-  bool has_desired_acceleration = (desired_joint_states.accelerations.size() == number_of_joints_);
-  bool has_current_position = (current_joint_states.positions.size() == number_of_joints_);
-  bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_);
+  const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_);
+  const bool has_desired_velocity = (desired_joint_states.velocities.size() == number_of_joints_);
+  const bool has_desired_acceleration =
+    (desired_joint_states.accelerations.size() == number_of_joints_);
+  const bool has_current_position = (current_joint_states.positions.size() == number_of_joints_);
+  const bool has_current_velocity = (current_joint_states.velocities.size() == number_of_joints_);
 
   // pos state and vel or pos cmd is required, vel state is optional
   if (!(has_current_position && (has_desired_position || has_desired_velocity)))
   {
     return false;
   }
-
-  if (!has_current_velocity)
-  {
-    // First update() after activating does not have velocity available, assume 0
-    current_joint_states.velocities.resize(number_of_joints_, 0.0);
-  }
+  const std::vector<double> & current_joint_velocities =
+    has_current_velocity ? current_joint_states.velocities
+                         : std::vector<double>(number_of_joints_, 0.0);
 
   // TODO(destogl): please check if we get too much malloc from this initialization,
   // if so then we should use members instead local variables and initialize them in other method
@@ -140,8 +138,7 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
             current_joint_states.positions[index] + desired_vel[index] * dt_seconds;
         }
 
-        desired_acc[index] =
-          (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+        desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds;
       }
     }
 
@@ -174,16 +171,15 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
         if (
           std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index]))
         {
-          desired_acc[index] =
-            (desired_vel[index] - current_joint_states.velocities[index]) / dt_seconds;
+          desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds;
         }
 
         // check if decelerating - if velocity is changing toward 0
         bool deceleration_limit_applied = false;
         bool limit_applied = false;
         if (
-          (desired_acc[index] < 0 && current_joint_states.velocities[index] > 0) ||
-          (desired_acc[index] > 0 && current_joint_states.velocities[index] < 0))
+          (desired_acc[index] < 0 && current_joint_velocities[index] > 0) ||
+          (desired_acc[index] > 0 && current_joint_velocities[index] < 0))
         {
           // limit deceleration
           if (joint_limits_[index].has_deceleration_limits)
@@ -204,13 +200,12 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
         if (limit_applied)
         {
           // vel_cmd from integration of desired_acc, needed even if no vel output
-          desired_vel[index] =
-            current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
+          desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds;
           if (has_desired_position)
           {
             // pos_cmd from from double integration of desired_acc
             desired_pos[index] = current_joint_states.positions[index] +
-                                 current_joint_states.velocities[index] * dt_seconds +
+                                 current_joint_velocities[index] * dt_seconds +
                                  0.5 * desired_acc[index] * dt_seconds * dt_seconds;
           }
         }
@@ -315,7 +310,7 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       // Compute accel to stop
       // Here we aren't explicitly maximally decelerating, but for joints near their limits this
       // should still result in max decel being used
-      desired_acc[index] = -current_joint_states.velocities[index] / dt_seconds;
+      desired_acc[index] = -current_joint_velocities[index] / dt_seconds;
       if (joint_limits_[index].has_deceleration_limits)
       {
         desired_acc[index] = std::copysign(
@@ -332,13 +327,12 @@ bool JointSaturationLimiter<trajectory_msgs::msg::JointTrajectoryPoint>::on_enfo
       // Recompute velocity and position
       if (has_desired_velocity)
       {
-        desired_vel[index] =
-          current_joint_states.velocities[index] + desired_acc[index] * dt_seconds;
+        desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds;
       }
       if (has_desired_position)
       {
         desired_pos[index] = current_joint_states.positions[index] +
-                             current_joint_states.velocities[index] * dt_seconds +
+                             current_joint_velocities[index] * dt_seconds +
                              0.5 * desired_acc[index] * dt_seconds * dt_seconds;
       }
     }

joint_limits/src/joint_soft_limiter.cpp

@@ -19,7 +19,7 @@ namespace joint_limits
 {
 
 bool JointSoftLimiter::on_enforce(
-  JointControlInterfacesData & actual, JointControlInterfacesData & desired,
+  const JointControlInterfacesData & actual, JointControlInterfacesData & desired,
   const rclcpp::Duration & dt)
 {
   bool limits_enforced = false;
@@ -141,8 +141,8 @@ bool JointSoftLimiter::on_enforce(
 
   if (desired.has_position())
   {
-    const auto position_limits =
-      compute_position_limits(hard_limits, actual.velocity, prev_command_.position, dt_seconds);
+    const auto position_limits = compute_position_limits(
+      hard_limits, actual.velocity, actual.position, prev_command_.position, dt_seconds);
 
     double pos_low = -std::numeric_limits<double>::infinity();
     double pos_high = std::numeric_limits<double>::infinity();

joint_limits/test/test_joint_range_limiter.cpp

@@ -142,6 +142,32 @@ TEST_F(JointSaturationLimiterTest, check_desired_position_only_cases)
   EXPECT_FALSE(desired_state_.has_effort());
   EXPECT_FALSE(desired_state_.has_jerk());
 
+  desired_state_ = {};
+  actual_state_ = {};
+  actual_state_.position = 0.0;
+  desired_state_.position = 5.0 * M_PI;
+  const rclcpp::Duration test_period(0, 100);  // 0.1 second
+  for (size_t i = 0; i < 2000; i++)
+  {
+    desired_state_.position = 5.0 * M_PI;
+    SCOPED_TRACE(
+      "Testing for actual position: " + std::to_string(actual_state_.position.value()) +
+      ", desired position: " + std::to_string(desired_state_.position.value()) +
+      " for the joint limits : " + limits.to_string() + " Iteration : " + std::to_string(i));
+    ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, test_period));
+    EXPECT_NEAR(
+      desired_state_.position.value(),
+      std::min(
+        actual_state_.position.value() + (limits.max_velocity * test_period.seconds()), M_PI),
+      COMMON_THRESHOLD);
+    EXPECT_TRUE(desired_state_.has_position());
+    EXPECT_FALSE(desired_state_.has_velocity());
+    EXPECT_FALSE(desired_state_.has_acceleration());
+    EXPECT_FALSE(desired_state_.has_effort());
+    EXPECT_FALSE(desired_state_.has_jerk());
+    actual_state_.position = desired_state_.position.value() / 2.0;
+  }
+
   // Now test when there are no position limits, then the desired position is not saturated
   limits = joint_limits::JointLimits();
   ASSERT_TRUE(Init(limits));
@@ -652,19 +678,40 @@ TEST_F(JointSaturationLimiterTest, check_all_desired_references_limiting)
   test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(std::nullopt, std::nullopt, 3.0, 1.0, 0.5, 0.5, 3.0, 1.0, 0.5, 0.5, false);
 
+  ASSERT_TRUE(Init(limits));
+  ASSERT_TRUE(joint_limiter_->configure(last_commanded_state_));
+
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 1.5, 0.5, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 2.5, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 3.5, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 4.5, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(std::nullopt, std::nullopt, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
+
   // Now enforce the limits with actual position and velocity
   ASSERT_TRUE(Init(limits));
   // Desired position and velocity affected due to the acceleration limits
   test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
   test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true);
+  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true);
   test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true);
-  test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
+  test_limit_enforcing(4.0, 1.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(4.8, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.5, 0.5, 0.5, true);
   test_limit_enforcing(5.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true);
+  test_limit_enforcing(5.0, 1.0, 6.0, 3.0, 1.0, 0.5, 5.0, 0.0, 0.5, 0.5, true);
 }
 
 int main(int argc, char ** argv)

joint_limits/test/test_joint_soft_limiter.cpp

@@ -162,6 +162,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
   soft_limits.max_position = 1.5;
   soft_limits.min_position = -1.5;
   ASSERT_TRUE(Init(limits, soft_limits));
+  actual_state_.position = 0.0;
   desired_state_.position = 2.0;
   ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
   EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD);
@@ -170,6 +171,7 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
   soft_limits.min_position = -0.75;
   ASSERT_TRUE(Init(limits, soft_limits));
   desired_state_.position = 2.0;
+  actual_state_ = {};
   ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
   EXPECT_NEAR(desired_state_.position.value(), soft_limits.max_position, COMMON_THRESHOLD);
   desired_state_.position = -3.0;
@@ -286,6 +288,32 @@ TEST_F(JointSoftLimiterTest, check_desired_position_only_cases)
     actual_state_.position = expected_pos;
   }
 
+  // More generic test case to mock a slow system
+  soft_limits.k_position = 0.5;
+  soft_limits.max_position = 2.0;
+  soft_limits.min_position = -2.0;
+  ASSERT_TRUE(Init(limits, soft_limits));
+  desired_state_.position = 2.0;
+  ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
+  EXPECT_NEAR(desired_state_.position.value(), 1.0, COMMON_THRESHOLD);
+  actual_state_.position = -0.1;
+  for (auto i = 0; i < 10000; i++)
+  {
+    SCOPED_TRACE(
+      "Testing for iteration: " + std::to_string(i) +
+      " for desired position: " + std::to_string(desired_state_.position.value()) +
+      " and actual position : " + std::to_string(actual_state_.position.value()) +
+      " for the joint limits : " + limits.to_string());
+    desired_state_.position = 4.0;
+    const double delta_pos = std::min(
+      (soft_limits.max_position - actual_state_.position.value()) * soft_limits.k_position,
+      limits.max_velocity * period.seconds());
+    const double expected_pos = actual_state_.position.value() + delta_pos;
+    ASSERT_TRUE(joint_limiter_->enforce(actual_state_, desired_state_, period));
+    EXPECT_NEAR(desired_state_.position.value(), expected_pos, COMMON_THRESHOLD);
+    actual_state_.position = expected_pos / 1.27;
+  }
+
   // Now test when there are no position limits and soft limits, then the desired position is not
   // saturated
   limits = joint_limits::JointLimits();
@@ -1021,9 +1049,13 @@ TEST_F(JointSoftLimiterTest, check_all_desired_references_limiting)
   // Desired position and velocity affected due to the acceleration limits
   test_limit_enforcing(0.5, 0.0, 6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
   test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(1.0, 0.0, -6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
-  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.0, 0.5, 0.5, 0.5, true);
+  // If the actual position doesn't change, the desired position should not change
+  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.0, 0.0, 6.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.2, 0.0, 6.0, 0.0, 0.0, 0.0, 1.7, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, 6.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(1.5, 0.0, -6.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, true);
+  test_limit_enforcing(2.0, 0.0, 6.0, 2.0, 1.0, 0.5, 2.5, 0.5, 0.5, 0.5, true);
   test_limit_enforcing(2.0, 0.5, 6.0, 2.0, 1.0, 0.5, 3.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(3.0, 0.5, 6.0, 2.0, 1.0, 0.5, 4.0, 1.0, 0.5, 0.5, true);
   test_limit_enforcing(4.0, 0.5, 6.0, 2.0, 1.0, 0.5, 5.0, 1.0, 0.5, 0.5, true);