diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index 4671db898e..4f296ea438 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h @@ -45,14 +45,44 @@ namespace trajectory_processing class RuckigSmoothing { public: + /** + * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. + * \param[in,out] trajectory A path which needs smoothing. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0); + /** + * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. + * \param[in,out] trajectory A path which needs smoothing. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + * \param jerk_limits Joint names and jerk limits in rad/s^3 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, const std::unordered_map& acceleration_limits, - const std::unordered_map& jerk_limits); + const std::unordered_map& jerk_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0); + + /** + * \brief Apply smoothing to a time-parameterized trajectory so that jerk limits are not violated. + * \param[in,out] trajectory A path which needs smoothing. + * \param joint_limits Joint names and corresponding velocity limits in rad/s, acceleration limits in rad/s^2, + * and jerk limits in rad/s^3 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0); private: /** diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 4860362a63..527b0c6c8d 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -45,6 +45,14 @@ namespace trajectory_processing { +enum LimitType +{ + VELOCITY, + ACCELERATION +}; + +const std::unordered_map LIMIT_TYPES = { { VELOCITY, "velocity" }, + { ACCELERATION, "acceleration" } }; class PathSegment { public: @@ -196,11 +204,35 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization * time-parameterized; this function will re-time-parameterize it. * \param velocity_limits Joint names and velocity limits in rad/s * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. */ // clang-format on bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, - const std::unordered_map& acceleration_limits) const override; + const std::unordered_map& acceleration_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const override; + + // clang-format off +/** + * \brief Compute a trajectory with waypoints spaced equally in time (according to resample_dt_). + * Resampling the trajectory doesn't change the start and goal point, + * and all re-sampled waypoints will be on the path of the original trajectory (within path_tolerance_). + * However, controller execution is separate from MoveIt and may deviate from the intended path between waypoints. + * path_tolerance_ is defined in configuration space, so the unit is rad for revolute joints, + * meters for prismatic joints. + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + // clang-format on + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const override; private: bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, @@ -214,6 +246,14 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization */ bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const; + /** + * @brief Check if the requested scaling factor is valid and if not, return 1.0. + * \param requested_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits + * \param limit_type Whether the velocity or acceleration scaling factor is being verified + * \return The user requested scaling factor, if it is valid. Otherwise, return 1.0. + */ + double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const; + const double path_tolerance_; const double resample_dt_; const double min_angle_change_; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h index 4746e11967..940bb2c5da 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -12,11 +12,44 @@ class TimeParameterization { public: virtual ~TimeParameterization() = default; + + /** + * \brief Compute a trajectory with waypoints spaced equally in time + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const = 0; + + /** + * \brief Compute a trajectory with waypoints spaced equally in time + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param velocity_limits Joint names and velocity limits in rad/s + * \param acceleration_limits Joint names and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, - const std::unordered_map& acceleration_limits) const = 0; + const std::unordered_map& acceleration_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; + + /** + * \brief Compute a trajectory with waypoints spaced equally in time + * \param[in,out] trajectory A path which needs time-parameterization. It's OK if this path has already been + * time-parameterized; this function will re-time-parameterize it. + * \param joint_limits Joint names and corresponding velocity limits in rad/s and acceleration limits in rad/s^2 + * \param max_velocity_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + * \param max_acceleration_scaling_factor A factor in the range [0,1] which can slow down the trajectory. + */ + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const = 0; }; } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index ee436b0b1b..aa22a46e11 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -92,7 +92,9 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, const std::unordered_map& acceleration_limits, - const std::unordered_map& jerk_limits) + const std::unordered_map& jerk_limits, + const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) { if (!validateGroup(trajectory)) { @@ -111,8 +113,6 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto moveit::core::JointModelGroup const* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::InputParameter ruckig_input{ num_dof }; - double max_velocity_scaling_factor = 1.0; - double max_acceleration_scaling_factor = 1.0; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) { RCLCPP_ERROR(LOGGER, "Error while retrieving kinematic limits (vel/accel/jerk) from RobotModel."); @@ -128,13 +128,13 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto auto it = velocity_limits.find(vars[j]); if (it != velocity_limits.end()) { - ruckig_input.max_velocity.at(j) = it->second; + ruckig_input.max_velocity.at(j) = it->second * max_velocity_scaling_factor; } // Acceleration it = acceleration_limits.find(vars[j]); if (it != acceleration_limits.end()) { - ruckig_input.max_acceleration.at(j) = it->second; + ruckig_input.max_acceleration.at(j) = it->second * max_acceleration_scaling_factor; } // Jerk it = jerk_limits.find(vars[j]); @@ -213,6 +213,34 @@ RuckigSmoothing::runRuckigInBatches(const size_t num_waypoints, const robot_traj return std::make_optional(output_trajectory, true /* deep copy */); } +bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) +{ + std::unordered_map velocity_limits; + std::unordered_map acceleration_limits; + std::unordered_map jerk_limits; + for (const auto& limit : joint_limits) + { + // If custom limits are not defined here, they will be supplied from getRobotModelBounds() later + if (limit.has_velocity_limits) + { + velocity_limits[limit.joint_name] = limit.max_velocity; + } + if (limit.has_acceleration_limits) + { + acceleration_limits[limit.joint_name] = limit.max_acceleration; + } + if (limit.has_jerk_limits) + { + jerk_limits[limit.joint_name] = limit.max_jerk; + } + } + return applySmoothing(trajectory, velocity_limits, acceleration_limits, jerk_limits, max_velocity_scaling_factor, + max_acceleration_scaling_factor); +} + bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory) { moveit::core::JointModelGroup const* const group = trajectory.getGroup(); diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index ec54d6dd39..67b78ff4c4 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -53,6 +53,7 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.time_optimal_trajectory_generation"); constexpr double DEFAULT_TIMESTEP = 1e-3; constexpr double EPS = 1e-6; +constexpr double DEFAULT_SCALING_FACTOR = 1.0; } // namespace class LinearPathSegment : public PathSegment @@ -880,37 +881,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } // Validate scaling - double velocity_scaling_factor = 1.0; - if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0) - { - velocity_scaling_factor = max_velocity_scaling_factor; - } - else if (max_velocity_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", - velocity_scaling_factor); - } - else - { - RCLCPP_WARN(LOGGER, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.", - max_velocity_scaling_factor, velocity_scaling_factor); - } - - double acceleration_scaling_factor = 1.0; - if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0) - { - acceleration_scaling_factor = max_acceleration_scaling_factor; - } - else if (max_acceleration_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.", - acceleration_scaling_factor); - } - else - { - RCLCPP_WARN(LOGGER, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.", - max_acceleration_scaling_factor, acceleration_scaling_factor); - } + double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY); + double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION); const std::vector& vars = group->getVariableNames(); const moveit::core::RobotModel& rmodel = group->getParentModel(); @@ -967,9 +939,33 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration); } +bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::vector& joint_limits, + const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) const +{ + std::unordered_map velocity_limits; + std::unordered_map acceleration_limits; + for (const auto& limit : joint_limits) + { + // If custom limits are not defined here, they will be supplied from getRobotModelBounds() later + if (limit.has_velocity_limits) + { + velocity_limits[limit.joint_name] = limit.max_velocity; + } + if (limit.has_acceleration_limits) + { + acceleration_limits[limit.joint_name] = limit.max_acceleration; + } + } + return computeTimeStamps(trajectory, velocity_limits, acceleration_limits, max_velocity_scaling_factor, + max_acceleration_scaling_factor); +} + bool TimeOptimalTrajectoryGeneration::computeTimeStamps( robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, - const std::unordered_map& acceleration_limits) const + const std::unordered_map& acceleration_limits, const double max_velocity_scaling_factor, + const double max_acceleration_scaling_factor) const { if (trajectory.empty()) return true; @@ -981,6 +977,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for"); return false; } + + // Validate scaling + double velocity_scaling_factor = verifyScalingFactor(max_velocity_scaling_factor, VELOCITY); + double acceleration_scaling_factor = verifyScalingFactor(max_acceleration_scaling_factor, ACCELERATION); + const unsigned num_joints = group->getVariableCount(); const std::vector& vars = group->getVariableNames(); const moveit::core::RobotModel& rmodel = group->getParentModel(); @@ -997,7 +998,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( auto it = velocity_limits.find(vars[j]); if (it != velocity_limits.end()) { - max_velocity[j] = it->second; + max_velocity[j] = it->second * velocity_scaling_factor; set_velocity_limit = true; } @@ -1010,7 +1011,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( bounds.max_velocity_, vars[j].c_str()); return false; } - max_velocity[j] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)); + max_velocity[j] = + std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)) * velocity_scaling_factor; set_velocity_limit = true; } @@ -1029,7 +1031,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( it = acceleration_limits.find(vars[j]); if (it != acceleration_limits.end()) { - max_acceleration[j] = it->second; + max_acceleration[j] = it->second * acceleration_scaling_factor; set_acceleration_limit = true; } @@ -1042,7 +1044,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( bounds.max_acceleration_, vars[j].c_str()); return false; } - max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)); + max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)) * + acceleration_scaling_factor; set_acceleration_limit = true; } if (!set_acceleration_limit) @@ -1203,4 +1206,28 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi return have_prismatic && have_revolute; } + +double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor, + const LimitType limit_type) const +{ + std::string limit_type_str; + double scaling_factor = DEFAULT_SCALING_FACTOR; + + const auto limit_type_it = LIMIT_TYPES.find(limit_type); + if (limit_type_it != LIMIT_TYPES.end()) + { + limit_type_str = limit_type_it->second + "_"; + } + + if (requested_scaling_factor > 0.0 && requested_scaling_factor <= 1.0) + { + scaling_factor = requested_scaling_factor; + } + else + { + RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(), + requested_scaling_factor, scaling_factor); + } + return scaling_factor; +} } // namespace trajectory_processing