Repository: moveit/moveit2 (C++)

Issue:
TOTG not respecting acceleration limits
### Description

TOTG seems to not care about acceleration limits other than at the beginning and end of a path.
I created an example test here to verify that a trajectory timed with TOTG satisfies the acceleration limits:
https://github.com/marioprats/moveit2/commit/0993a9a44b1c809cfbd332fe90103129837e97c9

The test is failing with accelerations up to 200 rad/s2, when the limit should be 0.2 rad/s2.

The input path consists of 10 waypoints (one single variable), in a u-shape:

![image](https://github.com/ros-planning/moveit2/assets/1014179/0704f623-3470-41fd-9ac9-f52b96a2b3d3)

Acceleration spikes to 200 rad/s2 at the inflection point:

![image](https://github.com/ros-planning/moveit2/assets/1014179/c7265552-b2c7-425c-9ee2-9df2b6e75b14)

But it behaves as expected at the beginning of the path for instance (maximum acceleration until maximum velocity is reached).

![image](https://github.com/ros-planning/moveit2/assets/1014179/26aa02d2-ae66-4609-b229-071161b258e2)

### Your environment
* ROS Distro: Rolling
* OS Version: e.g. Ubuntu 22.04
* Source, main

### Steps to reproduce
(https://github.com/marioprats/moveit2/commit/0993a9a44b1c809cfbd332fe90103129837e97c9)

### Expected behaviour
I would expect the output trajectory to satisfy velocity and acceleration limits at all times.



Fix (patch):
diff --git a/MIGRATION.md b/MIGRATION.md
index 8a851b09d2..9af12e14c4 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -3,6 +3,8 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
+- [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse.
+The constructors have been replaced by builder methods so that errors can be communicated. Paths and trajectories now need to be created with `Path::Create()` and `Trajectory::Create()`. These methods now return an `std::optional` that needs to be checked for a valid value. `Trajectory` no longer has the `isValid()` method. If it's invalid, `Trajectory::Create()` will return `std::nullopt`. Finally, `Path` now takes the list of input waypoints as `std::vector`, instead of `std::list`.
 - [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK
 - [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this:
 ```diff
@@ -10,7 +12,6 @@ API changes in MoveIt releases
 + planning_plugins:
 +   - ompl_interface/OMPLPlanner
 ```
-
 - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this:
 ```diff
 - request_adapters: >-
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 0a80bff316..8f7b90ec03 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,12 @@
 
 namespace trajectory_processing
 {
+
+// The intermediate waypoints of the input path need to be blended so that the entire path is diffentiable.
+// This constant defines the maximum deviation allowed at those intermediate waypoints, in radians for revolute joints,
+// or meters for prismatic joints.
+constexpr double DEFAULT_PATH_TOLERANCE = 0.1;
+
 enum LimitType
 {
   VELOCITY,
@@ -81,8 +87,14 @@ class PathSegment
 class Path
 {
 public:
-  Path(const std::list<Eigen::VectorXd>& path, double max_deviation = 0.0);
+  // Create a Path from a vector of waypoints and a maximum deviation to tolerate at the intermediate waypoints.
+  // The algorithm needs max_deviation to be greater than zero so that the path is differentiable.
+  static std::optional<Path> create(const std::vector<Eigen::VectorXd>& waypoint,
+                                    double max_deviation = DEFAULT_PATH_TOLERANCE);
+
+  // Copy constructor.
   Path(const Path& path);
+
   double getLength() const;
   Eigen::VectorXd getConfig(double s) const;
   Eigen::VectorXd getTangent(double s) const;
@@ -99,8 +111,12 @@ class Path
   std::list<std::pair<double, bool>> getSwitchingPoints() const;
 
 private:
+  // Default constructor private to prevent misuse. Use `create` instead to create a Path object.
+  Path() = default;
+
   PathSegment* getPathSegment(double& s) const;
-  double length_;
+
+  double length_ = 0.0;
   std::list<std::pair<double, bool>> switching_points_;
   std::list<std::unique_ptr<PathSegment>> path_segments_;
 };
@@ -108,16 +124,10 @@ class Path
 class Trajectory
 {
 public:
-  /// @brief Generates a time-optimal trajectory
-  Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
-             double time_step = 0.001);
-
-  ~Trajectory();
-
-  /** @brief Call this method after constructing the object to make sure the
-     trajectory generation succeeded without errors. If this method returns
-     false, all other methods have undefined behavior. **/
-  bool isValid() const;
+  /// @brief Generates a time-optimal trajectory.
+  /// @returns std::nullopt if the trajectory couldn't be parameterized.
+  static std::optional<Trajectory> create(const Path& path, const Eigen::VectorXd& max_velocity,
+                                          const Eigen::VectorXd& max_acceleration, double time_step = 0.001);
 
   /// @brief Returns the optimal duration of the trajectory
   double getDuration() const;
@@ -131,6 +141,9 @@ class Trajectory
   Eigen::VectorXd getAcceleration(double time) const;
 
 private:
+  Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
+             double time_step);
+
   struct TrajectoryStep
   {
     TrajectoryStep()
@@ -165,14 +178,14 @@ class Trajectory
   Path path_;
   Eigen::VectorXd max_velocity_;
   Eigen::VectorXd max_acceleration_;
-  unsigned int joint_num_;
-  bool valid_;
+  unsigned int joint_num_ = 0.0;
+  bool valid_ = true;
   std::list<TrajectoryStep> trajectory_;
   std::list<TrajectoryStep> end_trajectory_;  // non-empty only if the trajectory generation failed.
 
-  const double time_step_;
+  double time_step_ = 0.0;
 
-  mutable double cached_time_;
+  mutable double cached_time_ = std::numeric_limits<double>::max();
   mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
 };
 
@@ -180,7 +193,7 @@ MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration);
 class TimeOptimalTrajectoryGeneration : public TimeParameterization
 {
 public:
-  TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1,
+  TimeOptimalTrajectoryGeneration(const double path_tolerance = DEFAULT_PATH_TOLERANCE, const double resample_dt = 0.1,
                                   const double min_angle_change = 0.001);
 
   // clang-format off
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 5f7a66c894..d38eae1784 100644
--- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -198,59 +198,95 @@ class CircularPathSegment : public PathSegment
   Eigen::VectorXd y_;
 };
 
-Path::Path(const std::list<Eigen::VectorXd>& path, double max_deviation) : length_(0.0)
+std::optional<Path> Path::create(const std::vector<Eigen::VectorXd>& waypoints, double max_deviation)
 {
-  if (path.size() < 2)
-    return;
-  std::list<Eigen::VectorXd>::const_iterator path_iterator1 = path.begin();
-  std::list<Eigen::VectorXd>::const_iterator path_iterator2 = path_iterator1;
-  ++path_iterator2;
-  std::list<Eigen::VectorXd>::const_iterator path_iterator3;
-  Eigen::VectorXd start_config = *path_iterator1;
-  while (path_iterator2 != path.end())
+  if (waypoints.size() < 2)
   {
-    path_iterator3 = path_iterator2;
-    ++path_iterator3;
-    if (max_deviation > 0.0 && path_iterator3 != path.end())
+    RCLCPP_ERROR(getLogger(), "A path needs at least 2 waypoints.");
+    return std::nullopt;
+  }
+  if (max_deviation <= 0.0)
+  {
+    RCLCPP_ERROR(getLogger(), "Path max_deviation must be greater than 0.0.");
+    return std::nullopt;
+  }
+
+  // waypoints_iterator1, waypoints_iterator2 and waypoints_iterator3 point to three consecutive waypoints of the input
+  // path. The algorithm creates a LinearPathSegment starting at waypoints_iterator1, connected to CircularPathSegment
+  // at waypoints_iterator2, connected to another LinearPathSegment towards waypoints_iterator3.
+  // It does this iteratively for each three consecutive waypoints, therefore applying a blending of 'max_deviation' at
+  // the intermediate waypoints.
+  Path path;
+  std::vector<Eigen::VectorXd>::const_iterator waypoints_iterator1 = waypoints.begin();
+  std::vector<Eigen::VectorXd>::const_iterator waypoints_iterator2 = waypoints_iterator1;
+  ++waypoints_iterator2;
+  std::vector<Eigen::VectorXd>::const_iterator waypoints_iterator3;
+  Eigen::VectorXd start_config = *waypoints_iterator1;
+  while (waypoints_iterator2 != waypoints.end())
+  {
+    waypoints_iterator3 = waypoints_iterator2;
+    ++waypoints_iterator3;
+    if (waypoints_iterator3 != waypoints.end())
+    {
+      // Check that the path is not making a 180 deg. turn, which is not supported by the current implementation.
+      Eigen::VectorXd incoming_vector = *waypoints_iterator2 - *waypoints_iterator1;
+      Eigen::VectorXd outcoming_vector = *waypoints_iterator3 - *waypoints_iterator2;
+      double incoming_vector_norm = incoming_vector.norm();
+      double outcoming_vector_norm = outcoming_vector.norm();
+      if (incoming_vector_norm > std::numeric_limits<double>::epsilon() &&
+          outcoming_vector_norm > std::numeric_limits<double>::epsilon())
+      {
+        double cos_angle = incoming_vector.dot(outcoming_vector) / (incoming_vector_norm * outcoming_vector_norm);
+        constexpr double angle_tolerance = 1e-05;
+        if (cos_angle <= -1.0 + angle_tolerance)
+        {
+          RCLCPP_ERROR(getLogger(),
+                       "The path requires a 180 deg. turn, which is not supported by the current implementation.");
+          return std::nullopt;
+        }
+      }
+    }
+    if (max_deviation > 0.0 && waypoints_iterator3 != waypoints.end())
     {
       CircularPathSegment* blend_segment =
-          new CircularPathSegment(0.5 * (*path_iterator1 + *path_iterator2), *path_iterator2,
-                                  0.5 * (*path_iterator2 + *path_iterator3), max_deviation);
+          new CircularPathSegment(0.5 * (*waypoints_iterator1 + *waypoints_iterator2), *waypoints_iterator2,
+                                  0.5 * (*waypoints_iterator2 + *waypoints_iterator3), max_deviation);
       Eigen::VectorXd end_config = blend_segment->getConfig(0.0);
       if ((end_config - start_config).norm() > 0.000001)
       {
-        path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, end_config));
+        path.path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, end_config));
       }
-      path_segments_.emplace_back(blend_segment);
+      path.path_segments_.emplace_back(blend_segment);
 
       start_config = blend_segment->getConfig(blend_segment->getLength());
     }
     else
     {
-      path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, *path_iterator2));
-      start_config = *path_iterator2;
+      path.path_segments_.push_back(std::make_unique<LinearPathSegment>(start_config, *waypoints_iterator2));
+      start_config = *waypoints_iterator2;
     }
-    path_iterator1 = path_iterator2;
-    ++path_iterator2;
+    waypoints_iterator1 = waypoints_iterator2;
+    ++waypoints_iterator2;
   }
 
   // Create list of switching point candidates, calculate total path length and
   // absolute positions of path segments
-  for (std::unique_ptr<PathSegment>& path_segment : path_segments_)
+  for (std::unique_ptr<PathSegment>& path_segment : path.path_segments_)
   {
-    path_segment->position_ = length_;
+    path_segment->position_ = path.length_;
     std::list<double> local_switching_points = path_segment->getSwitchingPoints();
     for (std::list<double>::const_iterator point = local_switching_points.begin();
          point != local_switching_points.end(); ++point)
     {
-      switching_points_.push_back(std::make_pair(length_ + *point, false));
+      path.switching_points_.push_back(std::make_pair(path.length_ + *point, false));
     }
-    length_ += path_segment->getLength();
-    while (!switching_points_.empty() && switching_points_.back().first >= length_)
-      switching_points_.pop_back();
-    switching_points_.push_back(std::make_pair(length_, true));
+    path.length_ += path_segment->getLength();
+    while (!path.switching_points_.empty() && path.switching_points_.back().first >= path.length_)
+      path.switching_points_.pop_back();
+    path.switching_points_.push_back(std::make_pair(path.length_, true));
   }
-  switching_points_.pop_back();
+  path.switching_points_.pop_back();
+  return path;
 }
 
 Path::Path(const Path& path) : length_(path.length_), switching_points_(path.switching_points_)
@@ -319,60 +355,66 @@ std::list<std::pair<double, bool>> Path::getSwitchingPoints() const
   return switching_points_;
 }
 
-Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
-                       double time_step)
-  : path_(path)
-  , max_velocity_(max_velocity)
-  , max_acceleration_(max_acceleration)
-  , joint_num_(max_velocity.size())
-  , valid_(true)
-  , time_step_(time_step)
-  , cached_time_(std::numeric_limits<double>::max())
+std::optional<Trajectory> Trajectory::create(const Path& path, const Eigen::VectorXd& max_velocity,
+                                             const Eigen::VectorXd& max_acceleration, double time_step)
 {
-  if (time_step_ == 0)
+  if (time_step <= 0)
   {
-    valid_ = false;
-    RCLCPP_ERROR(getLogger(), "The trajectory is invalid because the time step is 0.");
-    return;
+    RCLCPP_ERROR(getLogger(), "The trajectory is invalid because the time step is <= 0.0.");
+    return std::nullopt;
   }
-  trajectory_.push_back(TrajectoryStep(0.0, 0.0));
-  double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true);
-  while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_)
+
+  Trajectory output(path, max_velocity, max_acceleration, time_step);
+  output.trajectory_.push_back(TrajectoryStep(0.0, 0.0));
+  double after_acceleration = output.getMinMaxPathAcceleration(0.0, 0.0, true);
+  while (output.valid_ && !output.integrateForward(output.trajectory_, after_acceleration) && output.valid_)
   {
     double before_acceleration;
     TrajectoryStep switching_point;
-    if (getNextSwitchingPoint(trajectory_.back().path_pos_, switching_point, before_acceleration, after_acceleration))
+    if (output.getNextSwitchingPoint(output.trajectory_.back().path_pos_, switching_point, before_acceleration,
+                                     after_acceleration))
     {
       break;
     }
-    integrateBackward(trajectory_, switching_point.path_pos_, switching_point.path_vel_, before_acceleration);
+    output.integrateBackward(output.trajectory_, switching_point.path_pos_, switching_point.path_vel_,
+                             before_acceleration);
+  }
+
+  if (!output.valid_)
+  {
+    RCLCPP_ERROR(getLogger(), "Trajectory not valid after integrateForward and integrateBackward.");
+    return std::nullopt;
   }
 
-  if (valid_)
+  double before_acceleration = output.getMinMaxPathAcceleration(output.path_.getLength(), 0.0, false);
+  output.integrateBackward(output.trajectory_, output.path_.getLength(), 0.0, before_acceleration);
+
+  if (!output.valid_)
   {
-    double before_acceleration = getMinMaxPathAcceleration(path_.getLength(), 0.0, false);
-    integrateBackward(trajectory_, path_.getLength(), 0.0, before_acceleration);
+    RCLCPP_ERROR(getLogger(), "Trajectory not valid after the second integrateBackward pass.");
+    return std::nullopt;
   }
 
-  if (valid_)
+  // Calculate timing.
+  std::list<TrajectoryStep>::iterator previous = output.trajectory_.begin();
+  std::list<TrajectoryStep>::iterator it = previous;
+  it->time_ = 0.0;
+  ++it;
+  while (it != output.trajectory_.end())
   {
-    // Calculate timing
-    std::list<TrajectoryStep>::iterator previous = trajectory_.begin();
-    std::list<TrajectoryStep>::iterator it = previous;
-    it->time_ = 0.0;
+    it->time_ = previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0);
+    previous = it;
     ++it;
-    while (it != trajectory_.end())
-    {
-      it->time_ =
-          previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0);
-      previous = it;
-      ++it;
-    }
   }
+
+  return output;
 }
 
-Trajectory::~Trajectory()
+Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration,
+                       double time_step)
+  : path_(path), max_velocity_(max_velocity), max_acceleration_(max_acceleration), time_step_(time_step)
 {
+  joint_num_ = max_velocity.size();
 }
 
 // Returns true if end of path is reached.
@@ -791,11 +833,6 @@ double Trajectory::getVelocityMaxPathVelocityDeriv(double path_pos)
          (tangent[active_constraint] * std::abs(tangent[active_constraint]));
 }
 
-bool Trajectory::isValid() const
-{
-  return valid_;
-}
-
 double Trajectory::getDuration() const
 {
   return trajectory_.back().time_;
@@ -933,9 +970,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
     }
     else
     {
-      RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint "
-                                           << vars[idx].c_str()
-                                           << "! You have to define velocity limits in the URDF or joint_limits.yaml");
+      RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " << vars[idx].c_str()
+                                                                                  << "! You have to define velocity "
+                                                                                     "limits "
+                                                                                     "in the URDF or "
+                                                                                     "joint_limits.yaml");
       return false;
     }
 
@@ -952,10 +991,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
     }
     else
     {
-      RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint "
-                                           << vars[idx].c_str()
-                                           << "! You have to define acceleration limits in the URDF or "
-                                              "joint_limits.yaml");
+      RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " << vars[idx].c_str()
+                                                                                      << "! You have to define "
+                                                                                         "acceleration "
+                                                                                         "limits in the URDF or "
+                                                                                         "joint_limits.yaml");
       return false;
     }
   }
@@ -1049,10 +1089,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
 
     if (!set_velocity_limit)
     {
-      RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint "
-                                           << vars[idx].c_str()
-                                           << "! You have to define velocity limits in the URDF or "
-                                              "joint_limits.yaml");
+      RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " << vars[idx].c_str()
+                                                                                  << "! You have to define velocity "
+                                                                                     "limits "
+                                                                                     "in the URDF or "
+                                                                                     "joint_limits.yaml");
       return false;
     }
 
@@ -1081,10 +1122,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
     }
     if (!set_acceleration_limit)
     {
-      RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint "
-                                           << vars[idx].c_str()
-                                           << "! You have to define acceleration limits in the URDF or "
-                                              "joint_limits.yaml");
+      RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " << vars[idx].c_str()
+                                                                                      << "! You have to define "
+                                                                                         "acceleration "
+                                                                                         "limits in the URDF or "
+                                                                                         "joint_limits.yaml");
       return false;
     }
   }
@@ -1143,7 +1185,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t
 
   // Have to convert into Eigen data structs and remove repeated points
   //  (https://github.com/tobiaskunz/trajectories/issues/3)
-  std::list<Eigen::VectorXd> points;
+  std::vector<Eigen::VectorXd> points;
   for (size_t p = 0; p < num_points; ++p)
   {
     moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p);
@@ -1185,15 +1227,22 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t
   }
 
   // Now actually call the algorithm
-  Trajectory parameterized(Path(points, path_tolerance_), max_velocity, max_acceleration, DEFAULT_TIMESTEP);
-  if (!parameterized.isValid())
+  std::optional<Path> path = Path::create(points, path_tolerance_);
+  if (!path)
+  {
+    RCLCPP_ERROR(getLogger(), "Invalid path.");
+    return false;
+  }
+
+  std::optional<Trajectory> parameterized = Trajectory::create(*path, max_velocity, max_acceleration, DEFAULT_TIMESTEP);
+  if (!parameterized)
   {
-    RCLCPP_ERROR(getLogger(), "Unable to parameterize trajectory.");
+    RCLCPP_ERROR(getLogger(), "Couldn't create trajectory");
     return false;
   }
 
   // Compute sample count
-  size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt_);
+  const size_t sample_count = std::ceil(parameterized->getDuration() / resample_dt_);
 
   // Resample and fill in trajectory
   moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0));
@@ -1202,10 +1251,10 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t
   for (size_t sample = 0; sample <= sample_count; ++sample)
   {
     // always sample the end of the trajectory as well
-    double t = std::min(parameterized.getDuration(), sample * resample_dt_);
-    Eigen::VectorXd position = parameterized.getPosition(t);
-    Eigen::VectorXd velocity = parameterized.getVelocity(t);
-    Eigen::VectorXd acceleration = parameterized.getAcceleration(t);
+    double t = std::min(parameterized->getDuration(), sample * resample_dt_);
+    Eigen::VectorXd position = parameterized->getPosition(t);
+    Eigen::VectorXd velocity = parameterized->getVelocity(t);
+    Eigen::VectorXd acceleration = parameterized->getAcceleration(t);
 
     for (size_t j = 0; j < num_joints; ++j)
     {