diff --git a/joint_limits/include/joint_limits/data_structures.hpp b/joint_limits/include/joint_limits/data_structures.hpp index cce9dbdbce..9275b8e9a5 100644 --- a/joint_limits/include/joint_limits/data_structures.hpp +++ b/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 { diff --git a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp index 6dc1a279cc..823a39c06b 100644 --- a/joint_limits/include/joint_limits/joint_saturation_limiter.hpp +++ b/joint_limits/include/joint_limits/joint_saturation_limiter.hpp @@ -50,6 +50,19 @@ class JointSaturationLimiter : public JointLimiterInterfacenumber_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 desired_pos_; + std::vector desired_vel_; + std::vector desired_acc_; + std::vector expected_vel_; + std::vector expected_pos_; + + // Pre-allocation boolean flags for tracking limits + std::vector pos_limit_hit_; + std::vector vel_limit_hit_; + std::vector acc_limit_hit_; + std::vector dec_limit_hit_; + std::vector 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 & 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 & current_joint_velocities, double dt_seconds, + bool has_desired_position, bool has_desired_velocity, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states); }; template @@ -114,6 +163,22 @@ bool JointSaturationLimiter::on_init() template <> bool JointSaturationLimiter::on_init(); +template <> +void JointSaturationLimiter::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 & current_joint_velocities, + bool & braking_near_position_limit_triggered, const double dt_seconds); + +template <> +void JointSaturationLimiter:: + handle_braking_near_position_limit( + const std::vector & 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_ diff --git a/joint_limits/src/joint_saturation_limiter.cpp b/joint_limits/src/joint_saturation_limiter.cpp index 53828deaf6..318f61a4fd 100644 --- a/joint_limits/src/joint_saturation_limiter.cpp +++ b/joint_limits/src/joint_saturation_limiter.cpp @@ -40,9 +40,24 @@ bool JointSaturationLimiter::on_enfo return false; } - // TODO(gwalck) compute if the max are not implicitly violated with the given dt - // e.g. for max vel 2.0 and max acc 5.0, with dt >0.4 - // velocity max is implicitly already violated due to max_acc * dt > 2.0 + /** WARN users if implicit vel exceeds max velocity */ + // TODO(Sachin): Replace this code from the other PR to scale down the implicit + for (size_t i = 0; i < number_of_joints_; ++i) + { + if (joint_limits_[i].has_velocity_limits && joint_limits_[i].has_acceleration_limits) + { + const double implicit_vel = joint_limits_[i].max_acceleration * dt_seconds; + if (implicit_vel > joint_limits_[i].max_velocity) + { + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint '" << joint_names_[i] << "': dt (" << dt_seconds + << ") is too large for max_acceleration (" << joint_limits_[i].max_acceleration + << ") and max_velocity (" << joint_limits_[i].max_velocity + << "); max_acc * dt = " << implicit_vel << " exceeds max_velocity"); + } + } + } // check for required inputs combination const bool has_desired_position = (desired_joint_states.positions.size() == number_of_joints_); @@ -61,85 +76,174 @@ bool JointSaturationLimiter::on_enfo has_current_velocity ? current_joint_states.velocities : std::vector(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 - std::vector desired_pos(number_of_joints_); - std::vector desired_vel(number_of_joints_); - std::vector desired_acc(number_of_joints_); - std::vector expected_vel(number_of_joints_); - std::vector expected_pos(number_of_joints_); + // reset values of vectors + std::fill(desired_pos_.begin(), desired_pos_.end(), 0.0); + std::fill(desired_vel_.begin(), desired_vel_.end(), 0.0); + std::fill(desired_acc_.begin(), desired_acc_.end(), 0.0); + std::fill(expected_pos_.begin(), expected_pos_.end(), 0.0); + std::fill(expected_vel_.begin(), expected_vel_.end(), 0.0); - // limits triggered - std::vector limited_jnts_pos, limited_jnts_vel, limited_jnts_acc, limited_jnts_dec; + std::fill(pos_limit_hit_.begin(), pos_limit_hit_.end(), false); + std::fill(vel_limit_hit_.begin(), vel_limit_hit_.end(), false); + std::fill(acc_limit_hit_.begin(), acc_limit_hit_.end(), false); + std::fill(dec_limit_hit_.begin(), dec_limit_hit_.end(), false); + std::fill(jerk_limit_hit_.begin(), jerk_limit_hit_.end(), false); bool braking_near_position_limit_triggered = false; - for (size_t index = 0; index < number_of_joints_; ++index) + clamp_joint_limits( + has_desired_position, has_desired_velocity, has_desired_acceleration, has_current_velocity, + current_joint_states, desired_joint_states, limits_enforced, current_joint_velocities, + braking_near_position_limit_triggered, dt_seconds); + + if (braking_near_position_limit_triggered) { - if (has_desired_position) + handle_braking_near_position_limit( + current_joint_velocities, dt_seconds, has_desired_position, has_desired_velocity, + current_joint_states); + } + + // display limitations + auto log_limits = [&](const std::vector & hits, const std::string & msg) + { + std::string out_str = ""; + for (size_t i = 0; i < number_of_joints_; ++i) + { + if (hits[i]) + { + out_str += joint_names_[i] + " "; + } + } + if (!out_str.empty()) + { + out_str.pop_back(); // remove trailing space + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << out_str << "] " << msg); + } + }; + + log_limits(pos_limit_hit_, "would exceed position limits, limiting"); + log_limits(vel_limit_hit_, "would exceed velocity limits, limiting"); + log_limits(acc_limit_hit_, "would exceed acceleration limits, limiting"); + log_limits(dec_limit_hit_, "would exceed deceleration limits, limiting"); + log_limits(jerk_limit_hit_, "would exceed jerk limits, limiting"); + + if (has_desired_position) + { + desired_joint_states.positions = desired_pos_; + } + if (has_desired_velocity) + { + desired_joint_states.velocities = desired_vel_; + } + if (has_desired_acceleration) + { + desired_joint_states.accelerations = desired_acc_; + } + + return limits_enforced; +} + +template <> +void JointSaturationLimiter::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 & current_joint_velocities, + bool & braking_near_position_limit_triggered, const double dt_seconds) +{ + if (has_desired_position) + { + for (size_t index = 0; index < number_of_joints_; ++index) { - desired_pos[index] = desired_joint_states.positions[index]; + desired_pos_[index] = desired_joint_states.positions[index]; } - if (has_desired_velocity) + } + if (has_desired_velocity) + { + for (size_t index = 0; index < number_of_joints_; ++index) { - desired_vel[index] = desired_joint_states.velocities[index]; + desired_vel_[index] = desired_joint_states.velocities[index]; } - if (has_desired_acceleration) + } + if (has_desired_acceleration) + { + for (size_t index = 0; index < number_of_joints_; ++index) { - desired_acc[index] = desired_joint_states.accelerations[index]; + desired_acc_[index] = desired_joint_states.accelerations[index]; } + } + + for (size_t index = 0; index < number_of_joints_; ++index) + { + auto clamp_pos_limit = [&]() + { + auto pos = std::clamp( + desired_pos_[index], joint_limits_[index].min_position, joint_limits_[index].max_position); + if (pos != desired_pos_[index]) + { + desired_pos_[index] = pos; + pos_limit_hit_[index] = true; + limits_enforced = true; + } + }; if (has_desired_position) { // limit position if (joint_limits_[index].has_position_limits) { - // clamp input pos_cmd - auto pos = std::clamp( - desired_pos[index], joint_limits_[index].min_position, joint_limits_[index].max_position); - if (pos != desired_pos[index]) - { - desired_pos[index] = pos; - limited_jnts_pos.emplace_back(joint_names_[index]); - limits_enforced = true; - } + clamp_pos_limit(); } // priority to pos_cmd derivative over cmd_vel when not defined. If done always then we might // get jumps in the velocity based on the system's dynamics. Position limit clamping is done // below once again. - const double position_difference = desired_pos[index] - current_joint_states.positions[index]; + const double position_difference = + desired_pos_[index] - current_joint_states.positions[index]; if ( std::fabs(position_difference) > VALUE_CONSIDERED_ZERO && - std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO) + std::fabs(desired_vel_[index]) <= VALUE_CONSIDERED_ZERO) { - desired_vel[index] = position_difference / dt_seconds; + desired_vel_[index] = position_difference / dt_seconds; } } + auto clamp_vel_limit = [&]() + { + desired_vel_[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel_[index]); + vel_limit_hit_[index] = true; + limits_enforced = true; + + // recompute pos_cmd if needed + if (has_desired_position) + { + desired_pos_[index] = + current_joint_states.positions[index] + desired_vel_[index] * dt_seconds; + if (joint_limits_[index].has_position_limits) + { + clamp_pos_limit(); + } + } + + desired_acc_[index] = (desired_vel_[index] - current_joint_velocities[index]) / dt_seconds; + }; + // limit velocity if (joint_limits_[index].has_velocity_limits) { // if desired velocity is not defined calculate it from positions - if (std::fabs(desired_vel[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel[index])) + if ( + std::fabs(desired_vel_[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_vel_[index])) { - desired_vel[index] = - (desired_pos[index] - current_joint_states.positions[index]) / dt_seconds; + desired_vel_[index] = + (desired_pos_[index] - current_joint_states.positions[index]) / dt_seconds; } // clamp input vel_cmd - if (std::fabs(desired_vel[index]) > joint_limits_[index].max_velocity) + if (std::fabs(desired_vel_[index]) > joint_limits_[index].max_velocity) { - desired_vel[index] = std::copysign(joint_limits_[index].max_velocity, desired_vel[index]); - limited_jnts_vel.emplace_back(joint_names_[index]); - limits_enforced = true; - - // recompute pos_cmd if needed - if (has_desired_position) - { - desired_pos[index] = - current_joint_states.positions[index] + desired_vel[index] * dt_seconds; - } - - desired_acc[index] = (desired_vel[index] - current_joint_velocities[index]) / dt_seconds; + clamp_vel_limit(); } } @@ -147,18 +251,17 @@ bool JointSaturationLimiter::on_enfo if ( joint_limits_[index].has_acceleration_limits || joint_limits_[index].has_deceleration_limits) { - // if(has_current_velocity) - if (1) // for now use a zero velocity if not provided + if (has_current_velocity) { // limiting acc or dec function auto apply_acc_or_dec_limit = [&]( const double max_acc_or_dec, std::vector & acc, - std::vector & limited_jnts) -> bool + std::vector & limit_hit) -> bool { if (std::fabs(acc[index]) > max_acc_or_dec) { acc[index] = std::copysign(max_acc_or_dec, acc[index]); - limited_jnts.emplace_back(joint_names_[index]); + limit_hit[index] = true; limits_enforced = true; return true; } @@ -168,25 +271,27 @@ bool JointSaturationLimiter::on_enfo } }; - // if desired acceleration if not provided compute it from desired_vel and vel_state + // if desired acceleration if not provided compute it from desired_vel_ and vel_state if ( - std::fabs(desired_acc[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc[index])) + std::fabs(desired_acc_[index]) <= VALUE_CONSIDERED_ZERO || + std::isnan(desired_acc_[index])) { - desired_acc[index] = (desired_vel[index] - current_joint_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_velocities[index] > 0) || - (desired_acc[index] > 0 && current_joint_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) { limit_applied = apply_acc_or_dec_limit( - joint_limits_[index].max_deceleration, desired_acc, limited_jnts_dec); + joint_limits_[index].max_deceleration, desired_acc_, dec_limit_hit_); deceleration_limit_applied = true; } } @@ -195,23 +300,158 @@ bool JointSaturationLimiter::on_enfo if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) { limit_applied = apply_acc_or_dec_limit( - joint_limits_[index].max_acceleration, desired_acc, limited_jnts_acc); + joint_limits_[index].max_acceleration, desired_acc_, acc_limit_hit_); } if (limit_applied) { - // vel_cmd from integration of desired_acc, needed even if no vel output - desired_vel[index] = current_joint_velocities[index] + desired_acc[index] * dt_seconds; + // vel_cmd from integration of desired_acc_, needed even if no vel output + 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_velocities[index] * dt_seconds + - 0.5 * desired_acc[index] * dt_seconds * dt_seconds; + // pos_cmd from from double integration of desired_acc_ + desired_pos_[index] = current_joint_states.positions[index] + + current_joint_velocities[index] * dt_seconds + + 0.5 * desired_acc_[index] * dt_seconds * dt_seconds; + } + } + } + else // else we cannot compute acc, so not limiting it + { + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint '" << joint_names_[index] + << "': Acceleration limits configured but no current velocity provided. " + << "Acceleration limiting will be skipped."); + } + } + + // Check if joint velocity exceeds max velocity + if ( + joint_limits_[index].has_velocity_limits && + std::fabs(desired_vel_[index]) > joint_limits_[index].max_velocity) + { + clamp_vel_limit(); + } + + // Limit jerk + if (joint_limits_[index].has_jerk_limits) + { + // check if desired acceleration is zero or corrupted + if ( + std::fabs(desired_acc_[index]) <= VALUE_CONSIDERED_ZERO || std::isnan(desired_acc_[index])) + { + desired_acc_[index] = (desired_vel_[index] - current_joint_velocities[index]) / dt_seconds; + } + const double current_acceleration = + (current_joint_states.accelerations.size() == number_of_joints_) + ? current_joint_states.accelerations[index] + : 0.0f; + + // Calc desired jerk over this time + double desired_jerk = (desired_acc_[index] - current_acceleration) / dt_seconds; + + // Limit Jerk + if (std::fabs(desired_jerk) > joint_limits_[index].max_jerk) + { + desired_jerk = std::copysign(joint_limits_[index].max_jerk, desired_jerk); + jerk_limit_hit_[index] = true; + limits_enforced = true; + + // Backward recalculation: Update acceleration based on limited jerk + desired_acc_[index] = current_acceleration + (desired_jerk * dt_seconds); + + // Backward recalculation: Recompute velocity and position + 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_velocities[index] * dt_seconds) + + (0.5 * desired_acc_[index] * dt_seconds * dt_seconds); + } + + // Re-check velocity exceeds max velocity and clamp it + if ( + joint_limits_[index].has_velocity_limits && + std::fabs(desired_vel_[index]) > joint_limits_[index].max_velocity) + { + clamp_vel_limit(); + } + + // Re-check acceleration and deceleration + if ( + joint_limits_[index].has_acceleration_limits || + joint_limits_[index].has_deceleration_limits) + { + if ( + std::fabs(desired_acc_[index]) <= VALUE_CONSIDERED_ZERO || + std::isnan(desired_acc_[index])) + { + desired_acc_[index] = + (desired_vel_[index] - current_joint_velocities[index]) / dt_seconds; + } + + bool deceleration_limit_applied = false; + bool limit_applied = false; + auto apply_recheck_limit = + [&](const double limit, std::vector & vec, std::vector & hit_flag) -> bool + { + if (std::fabs(vec[index]) > limit) + { + vec[index] = std::copysign(limit, vec[index]); + hit_flag[index] = true; + limits_enforced = true; + return true; + } + return false; + }; + + // Check deceleration (velocity changing toward zero) + if ( + (desired_acc_[index] < 0 && current_joint_velocities[index] > 0) || + (desired_acc_[index] > 0 && current_joint_velocities[index] < 0)) + { + if (joint_limits_[index].has_deceleration_limits) + { + limit_applied = apply_recheck_limit( + joint_limits_[index].max_deceleration, desired_acc_, dec_limit_hit_); + deceleration_limit_applied = true; + } + } + + // Check acceleration (fallback to acceleration if no decel check applied) + if (joint_limits_[index].has_acceleration_limits && !deceleration_limit_applied) + { + limit_applied = apply_recheck_limit( + joint_limits_[index].max_acceleration, desired_acc_, acc_limit_hit_); + } + + if (limit_applied) + { + 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_velocities[index] * dt_seconds + + 0.5 * desired_acc_[index] * dt_seconds * dt_seconds; + } } } } - // else we cannot compute acc, so not limiting it + } + + // Re-clamp desired velocity after acceleration/jerk may have recomputed it + if ( + has_desired_velocity && joint_limits_[index].has_velocity_limits && + std::fabs(desired_vel_[index]) > joint_limits_[index].max_velocity) + { + clamp_vel_limit(); + } + // Re-clamp desired position after acceleration/jerk may have recomputed it + if (has_desired_position && joint_limits_[index].has_position_limits) + { + clamp_pos_limit(); } // plan ahead for position limits @@ -221,23 +461,47 @@ bool JointSaturationLimiter::on_enfo { // Check immediate next step when using vel_cmd only, other cases already handled // integrate pos - expected_pos[index] = - current_joint_states.positions[index] + desired_vel[index] * dt_seconds; - // if expected_pos over limit + expected_pos_[index] = + current_joint_states.positions[index] + desired_vel_[index] * dt_seconds; + // if expected_pos_ over limit auto pos = std::clamp( - expected_pos[index], joint_limits_[index].min_position, + expected_pos_[index], joint_limits_[index].min_position, joint_limits_[index].max_position); - if (pos != expected_pos[index]) + if (pos != expected_pos_[index]) { - // TODO(gwalck) compute vel_cmd that would permit to slow down in time at full - // deceleration in any case limit pos to max - expected_pos[index] = pos; - // and recompute vel_cmd that would lead to pos_max (not ideal as velocity would not be - // zero) - desired_vel[index] = - (expected_pos[index] - current_joint_states.positions[index]) / dt_seconds; - limited_jnts_pos.emplace_back(joint_names_[index]); + pos_limit_hit_[index] = true; limits_enforced = true; + + double decel; + if (joint_limits_[index].has_deceleration_limits) + { + decel = joint_limits_[index].max_deceleration; + } + else if (joint_limits_[index].has_acceleration_limits) + { + decel = joint_limits_[index].max_acceleration; + } + else + { + decel = std::fabs(desired_vel_[index] / dt_seconds); + } + + const double distance_to_limit = + (desired_vel_[index] < 0) + ? current_joint_states.positions[index] - joint_limits_[index].min_position + : joint_limits_[index].max_position - current_joint_states.positions[index]; + + const double vel_to_stop = std::sqrt(2.0 * decel * std::max(distance_to_limit, 0.0)); + const double vel_to_not_exceed = std::max(distance_to_limit, 0.0) / dt_seconds; + const double safe_vel = std::min(vel_to_stop, vel_to_not_exceed); + + if (std::fabs(desired_vel_[index]) > safe_vel) + { + desired_vel_[index] = std::copysign(safe_vel, desired_vel_[index]); + } + + expected_pos_[index] = + current_joint_states.positions[index] + desired_vel_[index] * dt_seconds; } } @@ -250,7 +514,7 @@ bool JointSaturationLimiter::on_enfo // Here we assume we will not trigger velocity limits while maximally decelerating. // This is a valid assumption if we are not currently at a velocity limit since we are just // coming to a rest. - double stopping_deccel = std::fabs(desired_vel[index] / dt_seconds); + double stopping_deccel = std::fabs(desired_vel_[index] / dt_seconds); if (joint_limits_[index].has_deceleration_limits) { stopping_deccel = joint_limits_[index].max_deceleration; @@ -261,21 +525,21 @@ bool JointSaturationLimiter::on_enfo } double stopping_distance = - std::fabs((-desired_vel[index] * desired_vel[index]) / (2 * stopping_deccel)); + std::fabs((-desired_vel_[index] * desired_vel_[index]) / (2 * stopping_deccel)); // compute stopping duration at stopping_deccel - double stopping_duration = std::fabs((desired_vel[index]) / (stopping_deccel)); + const double stopping_duration = std::fabs((desired_vel_[index]) / (stopping_deccel)); // Check that joint limits are beyond stopping_distance and desired_velocity is towards // that limit if ( - (desired_vel[index] < 0 && + (desired_vel_[index] < 0 && (current_joint_states.positions[index] - joint_limits_[index].min_position < stopping_distance)) || - (desired_vel[index] > 0 && + (desired_vel_[index] > 0 && (joint_limits_[index].max_position - current_joint_states.positions[index] < stopping_distance))) { - limited_jnts_pos.emplace_back(joint_names_[index]); + pos_limit_hit_[index] = true; braking_near_position_limit_triggered = true; limits_enforced = true; } @@ -283,17 +547,17 @@ bool JointSaturationLimiter::on_enfo { // compute the travel_distance at new desired velocity, with best case duration // stopping_duration - double motion_after_stopping_duration = desired_vel[index] * stopping_duration; + double motion_after_stopping_duration = desired_vel_[index] * stopping_duration; // re-check what happens if we don't slow down if ( - (desired_vel[index] < 0 && + (desired_vel_[index] < 0 && (current_joint_states.positions[index] - joint_limits_[index].min_position < motion_after_stopping_duration)) || - (desired_vel[index] > 0 && + (desired_vel_[index] > 0 && (joint_limits_[index].max_position - current_joint_states.positions[index] < motion_after_stopping_duration))) { - limited_jnts_pos.emplace_back(joint_names_[index]); + pos_limit_hit_[index] = true; braking_near_position_limit_triggered = true; limits_enforced = true; } @@ -301,124 +565,75 @@ bool JointSaturationLimiter::on_enfo } } } +} - // update variables according to triggers - if (braking_near_position_limit_triggered) +template <> +void JointSaturationLimiter:: + handle_braking_near_position_limit( + const std::vector & current_joint_velocities, double dt_seconds, + bool has_desired_position, bool has_desired_velocity, + const trajectory_msgs::msg::JointTrajectoryPoint & current_joint_states) +{ + for (size_t index = 0; index < number_of_joints_; ++index) { - // this limit applies to all joints even if a single one is triggered - for (size_t index = 0; index < number_of_joints_; ++index) + desired_acc_[index] = -current_joint_velocities[index] / dt_seconds; + if (joint_limits_[index].has_deceleration_limits) { - // 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_velocities[index] / dt_seconds; - if (joint_limits_[index].has_deceleration_limits) - { - desired_acc[index] = std::copysign( - std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_deceleration), - desired_acc[index]); - } - else if (joint_limits_[index].has_acceleration_limits) - { - desired_acc[index] = std::copysign( - std::min(std::fabs(desired_acc[index]), joint_limits_[index].max_acceleration), - desired_acc[index]); - } - - // Recompute velocity and position - if (has_desired_velocity) - { - 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_velocities[index] * dt_seconds + - 0.5 * desired_acc[index] * dt_seconds * dt_seconds; - } + desired_acc_[index] = std::copysign( + std::min(std::fabs(desired_acc_[index]), joint_limits_[index].max_deceleration), + desired_acc_[index]); } - std::ostringstream ostr; - for (auto jnt : limited_jnts_pos) + else if (joint_limits_[index].has_acceleration_limits) { - ostr << jnt << " "; + desired_acc_[index] = std::copysign( + std::min(std::fabs(desired_acc_[index]), joint_limits_[index].max_acceleration), + desired_acc_[index]); } - ostr << "\b \b"; // erase last character - RCLCPP_WARN_STREAM_THROTTLE( - node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, - "Joint(s) [" << ostr.str().c_str() - << "] would exceed position limits" - " if continuing at current state, limiting all joints"); } - // display limitations - - // if position limiting - if (limited_jnts_pos.size() > 0) + if (has_desired_velocity) { - std::ostringstream ostr; - for (auto jnt : limited_jnts_pos) + for (size_t index = 0; index < number_of_joints_; ++index) { - ostr << jnt << " "; + desired_vel_[index] = current_joint_velocities[index] + desired_acc_[index] * dt_seconds; } - ostr << "\b \b"; // erase last character - RCLCPP_WARN_STREAM_THROTTLE( - node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, - "Joint(s) [" << ostr.str().c_str() << "] would exceed position limits, limiting"); } - - if (limited_jnts_vel.size() > 0) + if (has_desired_position) { - std::ostringstream ostr; - for (auto jnt : limited_jnts_vel) + for (size_t index = 0; index < number_of_joints_; ++index) { - ostr << jnt << " "; + desired_pos_[index] = current_joint_states.positions[index] + + current_joint_velocities[index] * dt_seconds + + 0.5 * desired_acc_[index] * dt_seconds * dt_seconds; + // Final clamp for position + desired_pos_[index] = std::clamp( + desired_pos_[index], joint_limits_[index].min_position, joint_limits_[index].max_position); } - ostr << "\b \b"; // erase last character - RCLCPP_WARN_STREAM_THROTTLE( - node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, - "Joint(s) [" << ostr.str().c_str() << "] would exceed velocity limits, limiting"); } - if (limited_jnts_acc.size() > 0) + auto log_limits = [&](const std::vector & hits, const std::string & msg) { - std::ostringstream ostr; - for (auto jnt : limited_jnts_acc) + std::string out_str = ""; + for (size_t i = 0; i < number_of_joints_; ++i) { - ostr << jnt << " "; + if (hits[i]) + { + out_str += joint_names_[i] + " "; + } } - ostr << "\b \b"; // erase last character - RCLCPP_WARN_STREAM_THROTTLE( - node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, - "Joint(s) [" << ostr.str().c_str() << "] would exceed acceleration limits, limiting"); - } - - if (limited_jnts_dec.size() > 0) - { - std::ostringstream ostr; - for (auto jnt : limited_jnts_dec) + if (!out_str.empty()) { - ostr << jnt << " "; + out_str.pop_back(); // remove trailing space + RCLCPP_WARN_STREAM_THROTTLE( + node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, + "Joint(s) [" << out_str << "] " << msg); } - ostr << "\b \b"; // erase last character - RCLCPP_WARN_STREAM_THROTTLE( - node_logging_itf_->get_logger(), *clock_, ROS_LOG_THROTTLE_PERIOD, - "Joint(s) [" << ostr.str().c_str() << "] would exceed deceleration limits, limiting"); - } + }; - if (has_desired_position) - { - desired_joint_states.positions = desired_pos; - } - if (has_desired_velocity) - { - desired_joint_states.velocities = desired_vel; - } - if (has_desired_acceleration) - { - desired_joint_states.accelerations = desired_acc; - } - - return limits_enforced; + log_limits( + pos_limit_hit_, + "would exceed position limits" + " if continuing at current state, limiting all joints"); } } // namespace joint_limits diff --git a/joint_limits/test/test_joint_saturation_limiter.cpp b/joint_limits/test/test_joint_saturation_limiter.cpp index e2b296eca9..6f7662dceb 100644 --- a/joint_limits/test/test_joint_saturation_limiter.cpp +++ b/joint_limits/test/test_joint_saturation_limiter.cpp @@ -53,7 +53,7 @@ TEST_F(JointSaturationLimiterTest, when_invalid_dt_expect_enforce_fail) } } -TEST_F(JointSaturationLimiterTest, when_neigher_poscmd_nor_velcmd_expect_enforce_fail) +TEST_F(JointSaturationLimiterTest, when_neither_poscmd_nor_velcmd_expect_enforce_fail) { SetupNode("joint_saturation_limiter"); Load(); @@ -348,8 +348,9 @@ TEST_F(JointSaturationLimiterTest, when_position_close_to_pos_limit_expect_decel desired_joint_states_.velocities[0] = 1.5; // this setup requires 0.15 distance to stop, and 0.2 seconds (so 4 cycles at 0.05) - std::vector expected_ret = {true, true, true, false}; - for (auto i = 0u; i < 4; ++i) + // cycles 0-3 apply limits (braking + jerk), cycles 4-5 settle at stop + std::vector expected_ret = {true, true, true, true, false, false}; + for (auto i = 0u; i < 6; ++i) { auto previous_vel_request = desired_joint_states_.velocities[0]; // expect limits applied until the end stop @@ -481,9 +482,9 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_expect_dec_enforce // check if vel and acc limits applied CHECK_STATE_SINGLE_JOINT( desired_joint_states_, 0, - 0.315625, // pos = double integration from max dec with current state - 0.125, // vel limited by vel - max dec * dt - -7.5 // acc limited by -max dec + 0.31875, // pos = double integration from jerk-limited acc with current state + 0.25, // vel limited by jerk-limited acc + -5.0 // acc limited by jerk-limited max dec (jerk from 0 to -7.5 exceeds max_jerk) ); } } @@ -517,6 +518,236 @@ TEST_F(JointSaturationLimiterTest, when_deceleration_exceeded_with_no_maxdec_exp } } +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_pos_only_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions[0] = 0.075; + desired_joint_states_.velocities.clear(); + desired_joint_states_.accelerations.clear(); + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_NEAR(desired_joint_states_.positions[0], 0.05375, COMMON_THRESHOLD); + } +} + +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_vel_only_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions.clear(); + desired_joint_states_.velocities[0] = 1.5; + desired_joint_states_.accelerations.clear(); + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_NEAR(desired_joint_states_.velocities[0], 1.15, COMMON_THRESHOLD); + } +} + +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_acc_only_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions[0] = 0.075; + desired_joint_states_.velocities.clear(); + desired_joint_states_.accelerations[0] = 4.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_NEAR(desired_joint_states_.positions[0], 0.05375, COMMON_THRESHOLD); + ASSERT_NEAR(desired_joint_states_.accelerations[0], 3.0, COMMON_THRESHOLD); + } +} + +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_pos_and_acc_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions[0] = 0.075; + desired_joint_states_.velocities.clear(); + desired_joint_states_.accelerations[0] = 4.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_NEAR(desired_joint_states_.positions[0], 0.05375, COMMON_THRESHOLD); + ASSERT_NEAR(desired_joint_states_.accelerations[0], 3.0, COMMON_THRESHOLD); + } +} + +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_pos_and_vel_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions[0] = 0.075; + desired_joint_states_.velocities[0] = 1.5; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + CHECK_STATE_SINGLE_JOINT(desired_joint_states_, 0, 0.05375, 1.15, 3.0); + } +} + +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_vel_and_acc_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions.clear(); + desired_joint_states_.velocities[0] = 1.5; + desired_joint_states_.accelerations[0] = 4.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + ASSERT_NEAR(desired_joint_states_.velocities[0], 1.15, COMMON_THRESHOLD); + ASSERT_NEAR(desired_joint_states_.accelerations[0], 3.0, COMMON_THRESHOLD); + } +} + +TEST_F(JointSaturationLimiterTest, when_jerk_exceeded_with_pos_vel_and_acc_expect_limits_enforced) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + rclcpp::Duration period(0, 50000000); + + current_joint_states_.velocities[0] = 1.0; + current_joint_states_.accelerations[0] = -2.0; + + desired_joint_states_.positions[0] = 0.075; + desired_joint_states_.velocities[0] = 1.5; + desired_joint_states_.accelerations[0] = 4.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + CHECK_STATE_SINGLE_JOINT(desired_joint_states_, 0, 0.05375, 1.15, 3.0); + } +} + +TEST_F(JointSaturationLimiterTest, when_acc_limited_vel_overshoots_expect_vel_reclamped) +{ + SetupNode("joint_saturation_limiter"); + Load(); + + if (joint_limiter_) + { + Init(); + Configure(); + + // dt such that max_acc * dt > max_velocity: 5.0 * 0.5 = 2.5 > 2.0 + // Acceleration limiting recomputes velocity which then overshoots max_vel. + // The second-pass velocity clamp should catch it. + rclcpp::Duration period(0, 500000000); // 0.5 second + + // forward direction + { + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 0.0; + desired_joint_states_.positions[0] = 1.0; + desired_joint_states_.velocities[0] = 2.0; + desired_joint_states_.accelerations[0] = 10.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + 1.0, // pos = 0 + max_vel * dt + 2.0, // vel re-clamped to max_vel + 4.0 // acc = (2.0 - 0) / dt + ); + } + + // reverse direction + { + current_joint_states_.positions[0] = 0.0; + current_joint_states_.velocities[0] = 0.0; + desired_joint_states_.positions[0] = -1.0; + desired_joint_states_.velocities[0] = -2.0; + desired_joint_states_.accelerations[0] = -10.0; + + ASSERT_TRUE(joint_limiter_->enforce(current_joint_states_, desired_joint_states_, period)); + + CHECK_STATE_SINGLE_JOINT( + desired_joint_states_, 0, + -1.0, // pos = 0 - max_vel * dt + -2.0, // vel re-clamped to -max_vel + -4.0 // acc = (-2.0 - 0) / dt + ); + } + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv);