diff --git a/joint_limits/include/joint_limits/data_structures.hpp b/joint_limits/include/joint_limits/data_structures.hpp index b673908e67..acc5a90cc2 100644 --- a/joint_limits/include/joint_limits/data_structures.hpp +++ b/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 #include #include #include +#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::infinity(); \ + double upper_limit = std::numeric_limits::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; diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 368f09002a..8a17896139 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -20,6 +20,7 @@ #include #include #include +#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 compute_position_limits( +PositionLimits compute_position_limits( const joint_limits::JointLimits & limits, const std::optional & act_vel, const std::optional & prev_command_pos, double dt); @@ -59,7 +60,7 @@ std::pair 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 compute_velocity_limits( +VelocityLimits compute_velocity_limits( const std::string & joint_name, const joint_limits::JointLimits & limits, const double & desired_vel, const std::optional & act_pos, const std::optional & prev_command_vel, double dt); @@ -72,7 +73,7 @@ std::pair 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 compute_effort_limits( +EffortLimits compute_effort_limits( const joint_limits::JointLimits & limits, const std::optional & act_pos, const std::optional & act_vel, double /*dt*/); @@ -84,7 +85,7 @@ std::pair 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 compute_acceleration_limits( +AccelerationLimits compute_acceleration_limits( const JointLimits & limits, double desired_acceleration, std::optional actual_velocity); } // namespace joint_limits diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 4fee92948c..11ccaf7738 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/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 & 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 compute_position_limits( +PositionLimits compute_position_limits( const joint_limits::JointLimits & limits, const std::optional & act_vel, const std::optional & prev_command_pos, double dt) { - std::pair 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 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 compute_velocity_limits( +VelocityLimits compute_velocity_limits( const std::string & joint_name, const joint_limits::JointLimits & limits, const double & desired_vel, const std::optional & act_pos, const std::optional & prev_command_vel, double dt) { const double max_vel = limits.has_velocity_limits ? limits.max_velocity : std::numeric_limits::infinity(); - std::pair 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 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 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 compute_effort_limits( +EffortLimits compute_effort_limits( const joint_limits::JointLimits & limits, const std::optional & act_pos, const std::optional & act_vel, double /*dt*/) { const double max_effort = limits.has_effort_limits ? limits.max_effort : std::numeric_limits::infinity(); - std::pair 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 compute_acceleration_limits( +AccelerationLimits compute_acceleration_limits( const joint_limits::JointLimits & limits, double desired_acceleration, std::optional actual_velocity) { - std::pair acc_or_dec_limits( + AccelerationLimits acc_or_dec_limits( -std::numeric_limits::infinity(), std::numeric_limits::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; } diff --git a/joint_limits/src/joint_range_limiter.cpp b/joint_limits/src/joint_range_limiter.cpp index 1db7b3ef9a..2279b910ae 100644 --- a/joint_limits/src/joint_range_limiter.cpp +++ b/joint_limits/src/joint_range_limiter.cpp @@ -114,8 +114,8 @@ bool JointSaturationLimiter::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,8 +124,9 @@ bool JointSaturationLimiter::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()) @@ -133,8 +134,8 @@ bool JointSaturationLimiter::on_enforce( 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()) @@ -142,8 +143,10 @@ bool JointSaturationLimiter::on_enforce( 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()) diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index a9987c4634..47a88e658d 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/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 diff --git a/joint_limits/src/joint_soft_limiter.cpp b/joint_limits/src/joint_soft_limiter.cpp index a3d0d81ee9..a2292cb033 100644 --- a/joint_limits/src/joint_soft_limiter.cpp +++ b/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())