Allow custom velocity/acceleration limits for TOTG time-parameterization algorithm

moveit_core/robot_model/include/moveit/robot_model/joint_model.h

@@ -325,7 +325,7 @@ class JointModel
   /** \brief Force the specified velocities to be inside bounds. Return true if changes were made. */
   virtual bool enforceVelocityBounds(double* values, const Bounds& other_bounds) const;
 
-  /** \brief Get the bounds for a variable. Throw an exception if the variable was not found */
+  /** \brief Get the bounds for a variable. */
   const VariableBounds& getVariableBounds(const std::string& variable) const;
 
   /** \brief Get the variable bounds for this joint, in the same order as the names returned by getVariableNames() */

moveit_core/robot_model/include/moveit/robot_model/robot_model.h

@@ -428,7 +428,7 @@ class RobotModel
     return variable_names_;
   }
 
-  /** \brief Get the bounds for a specific variable. Throw an exception of variable is not found. */
+  /** \brief Get the bounds for a specific variable. */
   const VariableBounds& getVariableBounds(const std::string& variable) const
   {
     return getJointOfVariable(variable)->getVariableBounds(variable);

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h

@@ -169,7 +169,15 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
   bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, 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::map<std::string, double>& velocity_limits,
+                         const std::map<std::string, double>& acceleration_limits) const;
+
 private:
+  bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
+                                          const Eigen::VectorXd& max_velocity,
+                                          const Eigen::VectorXd& max_acceleration) const;
+
   const double path_tolerance_;
   const double resample_dt_;
   const double min_angle_change_;

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -911,17 +911,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
                 max_acceleration_scaling_factor, acceleration_scaling_factor);
   }
 
-  // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
-  trajectory.unwind();
-
-  // This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints
   const std::vector<std::string>& vars = group->getVariableNames();
-  const std::vector<int>& idx = group->getVariableIndexList();
   const moveit::core::RobotModel& rmodel = group->getParentModel();
   const unsigned num_joints = group->getVariableCount();
-  const unsigned num_points = trajectory.getWayPointCount();
 
-  // Get the limits (we do this at same time, unlike IterativeParabolicTimeParameterization)
+  // Get the vel/accel limits
   Eigen::VectorXd max_velocity(num_joints);
   Eigen::VectorXd max_acceleration(num_joints);
   for (size_t j = 0; j < num_joints; ++j)
@@ -969,6 +963,122 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
     }
   }
 
+  return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
+}
+
+bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
+                                                        const std::map<std::string, double>& velocity_limits,
+                                                        const std::map<std::string, double>& acceleration_limits) const
+{
+  if (trajectory.empty())
+    return true;
+
+  // Get the default joint limits from the robot model, then overwrite any that are provided as arguments
+  const moveit::core::JointModelGroup* group = trajectory.getGroup();
+  if (!group)
+  {
+    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
+    return false;
+  }
+  const unsigned num_joints = group->getVariableCount();
+  const std::vector<std::string>& vars = group->getVariableNames();
+  const moveit::core::RobotModel& rmodel = group->getParentModel();
+
+  Eigen::VectorXd max_velocity(num_joints);
+  Eigen::VectorXd max_acceleration(num_joints);
+  for (size_t j = 0; j < num_joints; ++j)
+  {
+    const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]);
+
+    // VELOCITY LIMIT
+    // Check if a custom limit was specified as an argument
+    bool set_velocity_limit = false;
+    for (const auto& [joint_name, velocity_limit] : velocity_limits)
+    {
+      if (joint_name.compare(vars[j]))
+      {
+        max_velocity[j] = velocity_limit;
+        set_velocity_limit = true;
+        break;
+      }
+    }
+
+    if (bounds.velocity_bounded_ && !set_velocity_limit)
+    {
+      // Set the default velocity limit, from robot model
+      if (bounds.max_velocity_ < std::numeric_limits<double>::epsilon())
+      {
+        RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0",
+                     bounds.max_velocity_, vars[j].c_str());
+        return false;
+      }
+      max_velocity[j] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_));
+      set_velocity_limit = true;
+    }
+
+    if (!set_velocity_limit)
+    {
+      max_velocity[j] = 1.0;
+      RCLCPP_WARN_STREAM_ONCE(
+          LOGGER, "Joint velocity limits are not defined. Using the default "
+                      << max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml.");
+    }
+
+    // ACCELERATION LIMIT
+    // Check if a custom limit was specified as an argument
+    bool set_acceleration_limit = false;
+    for (const auto& [joint_name, acceleration_limit] : acceleration_limits)
+    {
+      if (joint_name.compare(vars[j]))
+      {
+        max_acceleration[j] = acceleration_limit;
+        set_acceleration_limit = true;
+        break;
+      }
+    }
+
+    if (bounds.acceleration_bounded_ && !set_acceleration_limit)
+    {
+      if (bounds.max_acceleration_ < std::numeric_limits<double>::epsilon())
+      {
+        RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0",
+                     bounds.max_acceleration_, vars[j].c_str());
+        return false;
+      }
+      max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_));
+      set_acceleration_limit = true;
+    }
+    if (!set_acceleration_limit)
+    {
+      max_acceleration[j] = 1.0;
+      RCLCPP_WARN_STREAM_ONCE(LOGGER,
+                              "Joint acceleration limits are not defined. Using the default "
+                                  << max_acceleration[j]
+                                  << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.");
+    }
+  }
+
+  return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration);
+}
+
+bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,
+                                                                         const Eigen::VectorXd& max_velocity,
+                                                                         const Eigen::VectorXd& max_acceleration) const
+{
+  // This lib does not actually work properly when angles wrap around, so we need to unwind the path first
+  trajectory.unwind();
+
+  const moveit::core::JointModelGroup* group = trajectory.getGroup();
+  if (!group)
+  {
+    RCLCPP_ERROR(LOGGER, "It looks like the planner did not set the group the plan was computed for");
+    return false;
+  }
+
+  const unsigned num_points = trajectory.getWayPointCount();
+  const std::vector<int>& idx = group->getVariableIndexList();
+  const unsigned num_joints = group->getVariableCount();
+
   // Have to convert into Eigen data structs and remove repeated points
   //  (https://github.com/tobiaskunz/trajectories/issues/3)
   std::list<Eigen::VectorXd> points;

moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp

@@ -152,6 +152,33 @@ TEST(time_optimal_trajectory_generation, test3)
   EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]);
 }
 
+// Test the version of computeTimeStamps that takes custom velocity/acceleration limits
+TEST(time_optimal_trajectory_generation, test_custom_limits)
+{
+  constexpr auto robot_name{ "panda" };
+  constexpr auto group_name{ "panda_arm" };
+
+  auto robot_model = moveit::core::loadTestingRobotModel(robot_name);
+  ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name;
+  auto group = robot_model->getJointModelGroup(group_name);
+  ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name;
+  moveit::core::RobotState waypoint_state(robot_model);
+  waypoint_state.setToDefaultValues();
+
+  const double delta_t = 0.1;
+  robot_trajectory::RobotTrajectory trajectory(robot_model, group);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, delta_t);
+  waypoint_state.setJointGroupPositions(group, std::vector<double>{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 });
+  trajectory.addSuffixWayPoint(waypoint_state, delta_t);
+
+  TimeOptimalTrajectoryGeneration totg;
+  // Custom velocity & acceleration limits for some joints
+  std::map<std::string, double> vel_limits{ { "panda_joint1", 1.3 } };
+  std::map<std::string, double> accel_limits{ { "panda_joint2", 2.3 }, { "panda_joint3", 3.3 } };
+  ASSERT_TRUE(totg.computeTimeStamps(trajectory, vel_limits, accel_limits)) << "Failed to compute time stamps";
+}
+
 // Test that totg algorithm doesn't give large acceleration
 TEST(time_optimal_trajectory_generation, testLargeAccel)
 {