diff --git a/joint_limits/include/joint_limits/joint_limits_helpers.hpp b/joint_limits/include/joint_limits/joint_limits_helpers.hpp index 23414f4d7b..32b9afbd76 100644 --- a/joint_limits/include/joint_limits/joint_limits_helpers.hpp +++ b/joint_limits/include/joint_limits/joint_limits_helpers.hpp @@ -25,20 +25,60 @@ namespace joint_limits { +/** + * @brief Checks if a value is limited by the given limits. + * @param value The value to check. + * @param min The minimum limit. + * @param max The maximum limit. + * @return True if the value is limited, false otherwise. + */ bool is_limited(double value, double min, double max); +/** + * @brief Computes the position limits based on the velocity and acceleration limits. + * @param limits The joint limits. + * @param act_vel The actual velocity of the joint. + * @param prev_command_pos The previous commanded position of the joint. + * @param dt The time step. + * @return The position limits. + */ std::pair compute_position_limits( const joint_limits::JointLimits & limits, const std::optional & act_vel, const std::optional & prev_command_pos, double dt); +/** + * @brief Computes the velocity limits based on the position and acceleration limits. + * @param joint_name The name of the joint. + * @param limits The joint limits. + * @param act_pos The actual position of the joint. + * @param prev_command_vel The previous commanded velocity of the joint. + * @param dt The time step. + * @return The velocity limits. + */ std::pair compute_velocity_limits( const std::string & joint_name, const joint_limits::JointLimits & limits, const std::optional & act_pos, const std::optional & prev_command_vel, double dt); +/** + * @brief Computes the effort limits based on the position and velocity limits. + * @param limits The joint limits. + * @param act_pos The actual position of the joint. + * @param act_vel The actual velocity of the joint. + * @param dt The time step. + * @return The effort limits. + */ std::pair compute_effort_limits( const joint_limits::JointLimits & limits, const std::optional & act_pos, const std::optional & act_vel, double /*dt*/); +/** + * @brief Computes the acceleration limits based on the change in velocity and acceleration and + * deceleration limits. + * @param limits The joint limits. + * @param desired_acceleration The desired acceleration. + * @param actual_velocity The actual velocity of the joint. + * @return The acceleration limits. + */ std::pair compute_acceleration_limits( const JointLimits & limits, double desired_acceleration, std::optional actual_velocity); diff --git a/joint_limits/src/joint_limits_helpers.cpp b/joint_limits/src/joint_limits_helpers.cpp index 6d8a77b646..597d23598e 100644 --- a/joint_limits/src/joint_limits_helpers.cpp +++ b/joint_limits/src/joint_limits_helpers.cpp @@ -139,8 +139,8 @@ std::pair compute_acceleration_limits( -std::numeric_limits::infinity(), std::numeric_limits::infinity()); if ( limits.has_deceleration_limits && - ((desired_acceleration < 0 && actual_velocity && actual_velocity > 0) || - (desired_acceleration > 0 && actual_velocity && actual_velocity < 0))) + ((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;