Add velocity and acceleration scaling when using custom limits in Time Parameterization

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<std::string, double>& velocity_limits,
                              const std::unordered_map<std::string, double>& acceleration_limits,
-                             const std::unordered_map<std::string, double>& jerk_limits);
+                             const std::unordered_map<std::string, double>& 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<moveit_msgs::msg::JointLimits>& joint_limits,
+                             const double max_velocity_scaling_factor = 1.0,
+                             const double max_acceleration_scaling_factor = 1.0);
 
 private:
   /**

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<LimitType, std::string> 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<std::string, double>& velocity_limits,
-                         const std::unordered_map<std::string, double>& acceleration_limits) const override;
+                         const std::unordered_map<std::string, double>& 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<moveit_msgs::msg::JointLimits>& 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_;

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<std::string, double>& velocity_limits,
-                                 const std::unordered_map<std::string, double>& acceleration_limits) const = 0;
+                                 const std::unordered_map<std::string, double>& 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<moveit_msgs::msg::JointLimits>& joint_limits,
+                                 const double max_velocity_scaling_factor = 1.0,
+                                 const double max_acceleration_scaling_factor = 1.0) const = 0;
 };
 }  // namespace trajectory_processing

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<std::string, double>& velocity_limits,
                                      const std::unordered_map<std::string, double>& acceleration_limits,
-                                     const std::unordered_map<std::string, double>& jerk_limits)
+                                     const std::unordered_map<std::string, double>& 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::DynamicDOFs> 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<robot_trajectory::RobotTrajectory>(output_trajectory, true /* deep copy */);
 }
 
+bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
+                                     const std::vector<moveit_msgs::msg::JointLimits>& joint_limits,
+                                     const double max_velocity_scaling_factor,
+                                     const double max_acceleration_scaling_factor)
+{
+  std::unordered_map<std::string, double> velocity_limits;
+  std::unordered_map<std::string, double> acceleration_limits;
+  std::unordered_map<std::string, double> 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();

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<std::string>& 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<moveit_msgs::msg::JointLimits>& joint_limits,
+                                                        const double max_velocity_scaling_factor,
+                                                        const double max_acceleration_scaling_factor) const
+{
+  std::unordered_map<std::string, double> velocity_limits;
+  std::unordered_map<std::string, double> 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<std::string, double>& velocity_limits,
-    const std::unordered_map<std::string, double>& acceleration_limits) const
+    const std::unordered_map<std::string, double>& 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<std::string>& 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