From ac1776bc1001535e94dfda5943369a3d1051fbe2 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Mon, 26 Dec 2022 11:50:02 -0500 Subject: [PATCH 1/7] add velocity and accelerations scaling when using custom limits for time parameterization --- .../ruckig_traj_smoothing.h | 4 +- .../time_optimal_trajectory_generation.h | 6 ++- .../time_parameterization.h | 4 +- .../src/ruckig_traj_smoothing.cpp | 10 ++-- .../time_optimal_trajectory_generation.cpp | 47 +++++++++++++++++-- 5 files changed, 58 insertions(+), 13 deletions(-) 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..a59be0a2b7 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 @@ -52,7 +52,9 @@ class RuckigSmoothing 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); 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..57549b3673 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 @@ -196,11 +196,15 @@ 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; private: bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, 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..e35b8f30c0 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 @@ -17,6 +17,8 @@ class TimeParameterization const double max_acceleration_scaling_factor = 1.0) const = 0; 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; }; } // 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..40d10c5db0 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]); 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..d86fcba198 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -969,7 +969,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT 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 +982,40 @@ 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 = 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); + } + const unsigned num_joints = group->getVariableCount(); const std::vector& vars = group->getVariableNames(); const moveit::core::RobotModel& rmodel = group->getParentModel(); @@ -997,7 +1032,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 +1045,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 +1065,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 +1078,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) From b073888b317cdd1013cbc8c82f321e98601c8a17 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Mon, 26 Dec 2022 18:16:26 -0500 Subject: [PATCH 2/7] add scaling when passing in vecotor of joint-limits --- .../ruckig_traj_smoothing.h | 5 ++++ .../time_optimal_trajectory_generation.h | 5 ++++ .../time_parameterization.h | 4 +++ .../src/ruckig_traj_smoothing.cpp | 27 +++++++++++++++++++ .../time_optimal_trajectory_generation.cpp | 22 +++++++++++++++ 5 files changed, 63 insertions(+) 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 a59be0a2b7..e0bb63bbb3 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 @@ -56,6 +56,11 @@ class RuckigSmoothing const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0); + 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: /** * \brief A utility function to check if the group is defined. 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 57549b3673..705e04b5cf 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 @@ -206,6 +206,11 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const override; + 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, const Eigen::VectorXd& max_velocity, 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 e35b8f30c0..9bc6dbbb2b 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 @@ -20,5 +20,9 @@ class TimeParameterization const std::unordered_map& acceleration_limits, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const = 0; + 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 40d10c5db0..82d7a70641 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -213,6 +213,33 @@ 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 (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 d86fcba198..fff78d05fb 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -967,6 +967,28 @@ 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 (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 double max_velocity_scaling_factor, From 515c63e6ab58a7110aabc5143c17b1d571404bfb Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Wed, 11 Jan 2023 14:29:15 -0500 Subject: [PATCH 3/7] add function descriptions --- .../ruckig_traj_smoothing.h | 23 ++++++++++++++++ .../time_optimal_trajectory_generation.h | 15 +++++++++++ .../time_parameterization.h | 27 +++++++++++++++++++ 3 files changed, 65 insertions(+) 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 e0bb63bbb3..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,10 +45,25 @@ 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, @@ -56,6 +71,14 @@ class RuckigSmoothing 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, 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 705e04b5cf..b612240b19 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 @@ -206,6 +206,21 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization 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, 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 9bc6dbbb2b..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,14 +12,41 @@ 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 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, From fe61d00d04a11f7d1ea7f9afb2ed38544492dee2 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Wed, 11 Jan 2023 16:22:50 -0500 Subject: [PATCH 4/7] add verifyScalingFactor helper function --- .../time_optimal_trajectory_generation.h | 18 +++- .../time_optimal_trajectory_generation.cpp | 87 +++++++------------ 2 files changed, 48 insertions(+), 57 deletions(-) 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 b612240b19..6694be4c71 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,13 @@ namespace trajectory_processing { +enum LimitType +{ + VELOCITY, + ACCELERATION +}; + +std::unordered_map LIMIT_TYPES = { { VELOCITY, "velocity" }, { ACCELERATION, "acceleration" } }; class PathSegment { public: @@ -216,7 +223,7 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization * 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 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. */ @@ -238,6 +245,15 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization */ bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const; + /** + * @brief Check if the max scaling factor is valid and if not, set it to the specified default value + * \param[in,out] scaling_factor The default scaling factor that should be applied + * if the `max_scaling_factor` is invalid + * \param max_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 + */ + void verifyScalingFactor(double& scaling_factor, const double max_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/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index fff78d05fb..3f58f766d0 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -881,36 +881,10 @@ 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); - } + verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); 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); - } + verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); const std::vector& vars = group->getVariableNames(); const moveit::core::RobotModel& rmodel = group->getParentModel(); @@ -1007,36 +981,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( // 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); - } + verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); 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); - } + verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); const unsigned num_joints = group->getVariableCount(); const std::vector& vars = group->getVariableNames(); @@ -1262,4 +1210,31 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi return have_prismatic && have_revolute; } + +void TimeOptimalTrajectoryGeneration::verifyScalingFactor(double& scaling_factor, const double max_scaling_factor, + const LimitType limit_type) const +{ + std::string limit_type_str; + auto limit_type_it = LIMIT_TYPES.find(limit_type); + + if (limit_type_it != LIMIT_TYPES.end()) + { + limit_type_str = limit_type_it->second + "_"; + } + + if (max_scaling_factor > 0.0 && max_scaling_factor <= 1.0) + { + scaling_factor = max_scaling_factor; + } + else if (max_scaling_factor == 0.0) + { + RCLCPP_DEBUG(LOGGER, "A max_%sscaling_factor of 0.0 was specified, defaulting to %f instead.", + limit_type_str.c_str(), scaling_factor); + } + else + { + RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(), + max_scaling_factor, scaling_factor); + } +} } // namespace trajectory_processing From 398e11c50d8fa87fda99ac3deb45a92f74864f3d Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Wed, 11 Jan 2023 18:12:26 -0500 Subject: [PATCH 5/7] make map const --- .../trajectory_processing/time_optimal_trajectory_generation.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 6694be4c71..d575e6d575 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 @@ -51,7 +51,8 @@ enum LimitType ACCELERATION }; -std::unordered_map LIMIT_TYPES = { { VELOCITY, "velocity" }, { ACCELERATION, "acceleration" } }; +const std::unordered_map LIMIT_TYPES = { { VELOCITY, "velocity" }, + { ACCELERATION, "acceleration" } }; class PathSegment { public: From 48d56f96428cd68980258d791eb7f16f0099b51d Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Tue, 17 Jan 2023 13:21:34 -0500 Subject: [PATCH 6/7] address feedback --- .../time_optimal_trajectory_generation.h | 9 +++-- .../time_optimal_trajectory_generation.cpp | 34 +++++++------------ 2 files changed, 17 insertions(+), 26 deletions(-) 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 d575e6d575..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 @@ -247,13 +247,12 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization bool hasMixedJointTypes(const moveit::core::JointModelGroup* group) const; /** - * @brief Check if the max scaling factor is valid and if not, set it to the specified default value - * \param[in,out] scaling_factor The default scaling factor that should be applied - * if the `max_scaling_factor` is invalid - * \param max_scaling_factor The desired maximum scaling factor to apply to the velocity or acceleration limits + * @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. */ - void verifyScalingFactor(double& scaling_factor, const double max_scaling_factor, const LimitType limit_type) const; + double verifyScalingFactor(const double requested_scaling_factor, const LimitType limit_type) const; const double path_tolerance_; const double resample_dt_; 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 3f58f766d0..939ec3fcfd 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,11 +881,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } // Validate scaling - double velocity_scaling_factor = 1.0; - verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); - - double acceleration_scaling_factor = 1.0; - verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); + 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(); @@ -980,11 +978,8 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( } // Validate scaling - double velocity_scaling_factor = 1.0; - verifyScalingFactor(velocity_scaling_factor, max_velocity_scaling_factor, VELOCITY); - - double acceleration_scaling_factor = 1.0; - verifyScalingFactor(acceleration_scaling_factor, max_acceleration_scaling_factor, ACCELERATION); + 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(); @@ -1211,30 +1206,27 @@ bool TimeOptimalTrajectoryGeneration::hasMixedJointTypes(const moveit::core::Joi return have_prismatic && have_revolute; } -void TimeOptimalTrajectoryGeneration::verifyScalingFactor(double& scaling_factor, const double max_scaling_factor, - const LimitType limit_type) const +double TimeOptimalTrajectoryGeneration::verifyScalingFactor(const double requested_scaling_factor, + const LimitType limit_type) const { std::string limit_type_str; - auto limit_type_it = LIMIT_TYPES.find(limit_type); + 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 (max_scaling_factor > 0.0 && max_scaling_factor <= 1.0) - { - scaling_factor = max_scaling_factor; - } - else if (max_scaling_factor == 0.0) + if (requested_scaling_factor > 0.0 && requested_scaling_factor <= 1.0) { - RCLCPP_DEBUG(LOGGER, "A max_%sscaling_factor of 0.0 was specified, defaulting to %f instead.", - limit_type_str.c_str(), scaling_factor); + scaling_factor = requested_scaling_factor; } else { RCLCPP_WARN(LOGGER, "Invalid max_%sscaling_factor %f specified, defaulting to %f instead.", limit_type_str.c_str(), - max_scaling_factor, scaling_factor); + requested_scaling_factor, scaling_factor); } + return scaling_factor; } } // namespace trajectory_processing From b5d774e9e8a6790a7d32c67fde4fc0967f7e5530 Mon Sep 17 00:00:00 2001 From: Michael Wiznitzer Date: Tue, 17 Jan 2023 13:27:20 -0500 Subject: [PATCH 7/7] add comment --- moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp | 1 + .../src/time_optimal_trajectory_generation.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 82d7a70641..aa22a46e11 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -223,6 +223,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto 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; 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 939ec3fcfd..67b78ff4c4 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -948,6 +948,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT 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;