Return strong type for joint_limits helpers (#1981)

joint_limits/include/joint_limits/data_structures.hpp

@@ -17,13 +17,30 @@
 #ifndef JOINT_LIMITS__DATA_STRUCTURES_HPP_
 #define JOINT_LIMITS__DATA_STRUCTURES_HPP_
 
+#include <limits>
 #include <memory>
 #include <optional>
 #include <string>
 
+#define DEFINE_LIMIT_STRUCT(LimitType)                             \
+  struct LimitType                                                 \
+  {                                                                \
+    LimitType(double minimum_value, double maximum_value)          \
+    : lower_limit(minimum_value), upper_limit(maximum_value)       \
+    {                                                              \
+    }                                                              \
+    double lower_limit = -std::numeric_limits<double>::infinity(); \
+    double upper_limit = std::numeric_limits<double>::infinity();  \
+  };
+
 namespace joint_limits
 {
 
+DEFINE_LIMIT_STRUCT(PositionLimits);
+DEFINE_LIMIT_STRUCT(VelocityLimits);
+DEFINE_LIMIT_STRUCT(EffortLimits);
+DEFINE_LIMIT_STRUCT(AccelerationLimits);
+
 struct JointControlInterfacesData
 {
   std::string joint_name;

joint_limits/include/joint_limits/joint_limits_helpers.hpp

@@ -20,6 +20,7 @@
 #include <optional>
 #include <string>
 #include <utility>
+#include "joint_limits/data_structures.hpp"
 #include "joint_limits/joint_limits.hpp"
 
 namespace joint_limits
@@ -46,7 +47,7 @@ bool is_limited(double value, double min, double max);
  * @param dt The time step.
  * @return The position limits, first is the lower limit and second is the upper limit.
  */
-std::pair<double, double> compute_position_limits(
+PositionLimits compute_position_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_vel,
   const std::optional<double> & prev_command_pos, double dt);
 
@@ -59,7 +60,7 @@ std::pair<double, double> compute_position_limits(
  * @param dt The time step.
  * @return The velocity limits, first is the lower limit and second is the upper limit.
  */
-std::pair<double, double> compute_velocity_limits(
+VelocityLimits compute_velocity_limits(
   const std::string & joint_name, const joint_limits::JointLimits & limits,
   const double & desired_vel, const std::optional<double> & act_pos,
   const std::optional<double> & prev_command_vel, double dt);
@@ -72,7 +73,7 @@ std::pair<double, double> compute_velocity_limits(
  * @param dt The time step.
  * @return The effort limits, first is the lower limit and second is the upper limit.
  */
-std::pair<double, double> compute_effort_limits(
+EffortLimits compute_effort_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_pos,
   const std::optional<double> & act_vel, double /*dt*/);
 
@@ -84,7 +85,7 @@ std::pair<double, double> compute_effort_limits(
  * @param actual_velocity The actual velocity of the joint.
  * @return The acceleration limits, first is the lower limit and second is the upper limit.
  */
-std::pair<double, double> compute_acceleration_limits(
+AccelerationLimits compute_acceleration_limits(
   const JointLimits & limits, double desired_acceleration, std::optional<double> actual_velocity);
 
 }  // namespace joint_limits

joint_limits/src/joint_limits_helpers.cpp

@@ -26,22 +26,22 @@ namespace internal
 /**
  * @brief Check if the limits are in the correct order and swap them if they are not.
  */
-void check_and_swap_limits(std::pair<double, double> & limits)
+void check_and_swap_limits(double & lower_limit, double & upper_limit)
 {
-  if (limits.first > limits.second)
+  if (lower_limit > upper_limit)
   {
-    std::swap(limits.first, limits.second);
+    std::swap(lower_limit, upper_limit);
   }
 }
 }  // namespace internal
 
 bool is_limited(double value, double min, double max) { return value < min || value > max; }
 
-std::pair<double, double> compute_position_limits(
+PositionLimits compute_position_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_vel,
   const std::optional<double> & prev_command_pos, double dt)
 {
-  std::pair<double, double> pos_limits({limits.min_position, limits.max_position});
+  PositionLimits pos_limits(limits.min_position, limits.max_position);
   if (limits.has_velocity_limits)
   {
     const double act_vel_abs = act_vel.has_value() ? std::fabs(act_vel.value()) : 0.0;
@@ -50,28 +50,28 @@ std::pair<double, double> 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.first = std::max(prev_command_pos.value() - delta_pos, pos_limits.first);
-    pos_limits.second = std::min(prev_command_pos.value() + delta_pos, pos_limits.second);
+    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);
   }
-  internal::check_and_swap_limits(pos_limits);
+  internal::check_and_swap_limits(pos_limits.lower_limit, pos_limits.upper_limit);
   return pos_limits;
 }
 
-std::pair<double, double> compute_velocity_limits(
+VelocityLimits compute_velocity_limits(
   const std::string & joint_name, const joint_limits::JointLimits & limits,
   const double & desired_vel, const std::optional<double> & act_pos,
   const std::optional<double> & prev_command_vel, double dt)
 {
   const double max_vel =
     limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits<double>::infinity();
-  std::pair<double, double> vel_limits({-max_vel, max_vel});
+  VelocityLimits vel_limits(-max_vel, max_vel);
   if (limits.has_position_limits && act_pos.has_value())
   {
     const double actual_pos = act_pos.value();
     const double max_vel_with_pos_limits = (limits.max_position - actual_pos) / dt;
     const double min_vel_with_pos_limits = (limits.min_position - actual_pos) / dt;
-    vel_limits.first = std::max(min_vel_with_pos_limits, vel_limits.first);
-    vel_limits.second = std::min(max_vel_with_pos_limits, vel_limits.second);
+    vel_limits.lower_limit = std::max(min_vel_with_pos_limits, vel_limits.lower_limit);
+    vel_limits.upper_limit = std::min(max_vel_with_pos_limits, vel_limits.upper_limit);
 
     if (actual_pos > limits.max_position || actual_pos < limits.min_position)
     {
@@ -88,7 +88,7 @@ std::pair<double, double> compute_velocity_limits(
           "further into bounds with vel %.5f: '%s'. Joint velocity limits will be "
           "restrictred to zero.",
           actual_pos, limits.min_position, limits.max_position, desired_vel, joint_name.c_str());
-        vel_limits = {0.0, 0.0};
+        vel_limits = VelocityLimits(0.0, 0.0);
       }
       // If the joint reports a position way out of bounds, then it would mean something is
       // extremely wrong, so no velocity command should be allowed as it might damage the robot
@@ -101,72 +101,73 @@ std::pair<double, double> compute_velocity_limits(
           "Joint position is out of bounds for the joint : '%s'. Joint velocity limits will be "
           "restricted to zero.",
           joint_name.c_str());
-        vel_limits = {0.0, 0.0};
+        vel_limits = VelocityLimits(0.0, 0.0);
       }
     }
   }
   if (limits.has_acceleration_limits && prev_command_vel.has_value())
   {
     const double delta_vel = limits.max_acceleration * dt;
-    vel_limits.first = std::max(prev_command_vel.value() - delta_vel, vel_limits.first);
-    vel_limits.second = std::min(prev_command_vel.value() + delta_vel, vel_limits.second);
+    vel_limits.lower_limit = std::max(prev_command_vel.value() - delta_vel, vel_limits.lower_limit);
+    vel_limits.upper_limit = std::min(prev_command_vel.value() + delta_vel, vel_limits.upper_limit);
   }
-  internal::check_and_swap_limits(vel_limits);
+  internal::check_and_swap_limits(vel_limits.lower_limit, vel_limits.upper_limit);
   return vel_limits;
 }
 
-std::pair<double, double> compute_effort_limits(
+EffortLimits compute_effort_limits(
   const joint_limits::JointLimits & limits, const std::optional<double> & act_pos,
   const std::optional<double> & act_vel, double /*dt*/)
 {
   const double max_effort =
     limits.has_effort_limits ? limits.max_effort : std::numeric_limits<double>::infinity();
-  std::pair<double, double> eff_limits({-max_effort, max_effort});
+  EffortLimits eff_limits(-max_effort, max_effort);
   if (limits.has_position_limits && act_pos.has_value() && act_vel.has_value())
   {
     if ((act_pos.value() <= limits.min_position) && (act_vel.value() <= 0.0))
     {
-      eff_limits.first = 0.0;
+      eff_limits.lower_limit = 0.0;
     }
     else if ((act_pos.value() >= limits.max_position) && (act_vel.value() >= 0.0))
     {
-      eff_limits.second = 0.0;
+      eff_limits.upper_limit = 0.0;
     }
   }
   if (limits.has_velocity_limits && act_vel.has_value())
   {
     if (act_vel.value() < -limits.max_velocity)
     {
-      eff_limits.first = 0.0;
+      eff_limits.lower_limit = 0.0;
     }
     else if (act_vel.value() > limits.max_velocity)
     {
-      eff_limits.second = 0.0;
+      eff_limits.upper_limit = 0.0;
     }
   }
-  internal::check_and_swap_limits(eff_limits);
+  internal::check_and_swap_limits(eff_limits.lower_limit, eff_limits.upper_limit);
   return eff_limits;
 }
 
-std::pair<double, double> compute_acceleration_limits(
+AccelerationLimits compute_acceleration_limits(
   const joint_limits::JointLimits & limits, double desired_acceleration,
   std::optional<double> actual_velocity)
 {
-  std::pair<double, double> acc_or_dec_limits(
+  AccelerationLimits acc_or_dec_limits(
     -std::numeric_limits<double>::infinity(), std::numeric_limits<double>::infinity());
   if (
     limits.has_deceleration_limits &&
     ((desired_acceleration < 0 && actual_velocity && actual_velocity.value() > 0) ||
      (desired_acceleration > 0 && actual_velocity && actual_velocity.value() < 0)))
   {
-    acc_or_dec_limits.first = -limits.max_deceleration;
-    acc_or_dec_limits.second = limits.max_deceleration;
+    acc_or_dec_limits.lower_limit = -limits.max_deceleration;
+    acc_or_dec_limits.upper_limit = limits.max_deceleration;
   }
   else if (limits.has_acceleration_limits)
   {
-    acc_or_dec_limits.first = -limits.max_acceleration;
-    acc_or_dec_limits.second = limits.max_acceleration;
+    acc_or_dec_limits.lower_limit = -limits.max_acceleration;
+    acc_or_dec_limits.upper_limit = limits.max_acceleration;
   }
+  internal::check_and_swap_limits(acc_or_dec_limits.lower_limit, acc_or_dec_limits.upper_limit);
   return acc_or_dec_limits;
 }
 

joint_limits/src/joint_range_limiter.cpp

@@ -114,8 +114,8 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
   {
     const auto limits =
       compute_position_limits(joint_limits, actual.velocity, prev_command_.position, dt_seconds);
-    limits_enforced = is_limited(desired.position.value(), limits.first, limits.second);
-    desired.position = std::clamp(desired.position.value(), limits.first, limits.second);
+    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);
   }
 
   if (desired.has_velocity())
@@ -124,26 +124,29 @@ bool JointSaturationLimiter<JointControlInterfacesData>::on_enforce(
       joint_name, joint_limits, desired.velocity.value(), actual.position, prev_command_.velocity,
       dt_seconds);
     limits_enforced =
-      is_limited(desired.velocity.value(), limits.first, limits.second) || limits_enforced;
-    desired.velocity = std::clamp(desired.velocity.value(), limits.first, limits.second);
+      is_limited(desired.velocity.value(), limits.lower_limit, limits.upper_limit) ||
+      limits_enforced;
+    desired.velocity = std::clamp(desired.velocity.value(), limits.lower_limit, limits.upper_limit);
   }
 
   if (desired.has_effort())
   {
     const auto limits =
       compute_effort_limits(joint_limits, actual.position, actual.velocity, dt_seconds);
     limits_enforced =
-      is_limited(desired.effort.value(), limits.first, limits.second) || limits_enforced;
-    desired.effort = std::clamp(desired.effort.value(), limits.first, limits.second);
+      is_limited(desired.effort.value(), limits.lower_limit, limits.upper_limit) || limits_enforced;
+    desired.effort = std::clamp(desired.effort.value(), limits.lower_limit, limits.upper_limit);
   }
 
   if (desired.has_acceleration())
   {
     const auto limits =
       compute_acceleration_limits(joint_limits, desired.acceleration.value(), actual.velocity);
     limits_enforced =
-      is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced;
-    desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second);
+      is_limited(desired.acceleration.value(), limits.lower_limit, limits.upper_limit) ||
+      limits_enforced;
+    desired.acceleration =
+      std::clamp(desired.acceleration.value(), limits.lower_limit, limits.upper_limit);
   }
 
   if (desired.has_jerk())

joint_limits/src/joint_saturation_limiter.cpp

@@ -15,7 +15,6 @@
 /// \authors Nathan Brooks, Dr. Denis Stogl, Guillaume Walck
 
 #include "joint_limits/joint_saturation_limiter.hpp"
-#include "joint_limits/joint_limits_helpers.hpp"
 
 #include <algorithm>
 

joint_limits/src/joint_soft_limiter.cpp

@@ -158,8 +158,8 @@ bool JointSoftLimiter::on_enforce(
       pos_low = std::clamp(position + soft_min_vel * dt_seconds, pos_low, pos_high);
       pos_high = std::clamp(position + soft_max_vel * dt_seconds, pos_low, pos_high);
     }
-    pos_low = std::max(pos_low, position_limits.first);
-    pos_high = std::min(pos_high, position_limits.second);
+    pos_low = std::max(pos_low, position_limits.lower_limit);
+    pos_high = std::min(pos_high, position_limits.upper_limit);
 
     limits_enforced = is_limited(desired.position.value(), pos_low, pos_high);
     desired.position = std::clamp(desired.position.value(), pos_low, pos_high);
@@ -179,8 +179,8 @@ bool JointSoftLimiter::on_enforce(
         std::min(actual.velocity.value() + hard_limits.max_acceleration * dt_seconds, soft_max_vel);
     }
 
-    soft_min_vel = std::max(soft_min_vel, velocity_limits.first);
-    soft_max_vel = std::min(soft_max_vel, velocity_limits.second);
+    soft_min_vel = std::max(soft_min_vel, velocity_limits.lower_limit);
+    soft_max_vel = std::min(soft_max_vel, velocity_limits.upper_limit);
 
     limits_enforced =
       is_limited(desired.velocity.value(), soft_min_vel, soft_max_vel) || limits_enforced;
@@ -192,8 +192,8 @@ bool JointSoftLimiter::on_enforce(
     const auto effort_limits =
       compute_effort_limits(hard_limits, actual.position, actual.velocity, dt_seconds);
 
-    double soft_min_eff = effort_limits.first;
-    double soft_max_eff = effort_limits.second;
+    double soft_min_eff = effort_limits.lower_limit;
+    double soft_max_eff = effort_limits.upper_limit;
 
     if (
       hard_limits.has_effort_limits && std::isfinite(soft_joint_limits.k_velocity) &&
@@ -207,8 +207,8 @@ bool JointSoftLimiter::on_enforce(
         -soft_joint_limits.k_velocity * (actual.velocity.value() - soft_max_vel),
         -hard_limits.max_effort, hard_limits.max_effort);
 
-      soft_min_eff = std::max(soft_min_eff, effort_limits.first);
-      soft_max_eff = std::min(soft_max_eff, effort_limits.second);
+      soft_min_eff = std::max(soft_min_eff, effort_limits.lower_limit);
+      soft_max_eff = std::min(soft_max_eff, effort_limits.upper_limit);
     }
 
     limits_enforced =
@@ -221,8 +221,10 @@ bool JointSoftLimiter::on_enforce(
     const auto limits =
       compute_acceleration_limits(hard_limits, desired.acceleration.value(), actual.velocity);
     limits_enforced =
-      is_limited(desired.acceleration.value(), limits.first, limits.second) || limits_enforced;
-    desired.acceleration = std::clamp(desired.acceleration.value(), limits.first, limits.second);
+      is_limited(desired.acceleration.value(), limits.lower_limit, limits.upper_limit) ||
+      limits_enforced;
+    desired.acceleration =
+      std::clamp(desired.acceleration.value(), limits.lower_limit, limits.upper_limit);
   }
 
   if (desired.has_jerk())