From 7c95367a2bd6fbd7a58ad64d3c24228136d8a13f Mon Sep 17 00:00:00 2001
From: Nacho Mellado <ignacio.mellado@gmail.com>
Date: Fri, 18 Aug 2023 06:13:35 -0700
Subject: [PATCH] [TOTG] Exit loop when position can't change (#2307)

---
 .../time_optimal_trajectory_generation.cpp    | 16 ++++++++++
 ...est_time_optimal_trajectory_generation.cpp | 30 +++++++++++++++++++
 2 files changed, 46 insertions(+)

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 1c4da3dee2..70a6272df1 100644
--- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -325,6 +325,12 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co
   , time_step_(time_step)
   , cached_time_(std::numeric_limits<double>::max())
 {
+  if (time_step_ == 0)
+  {
+    valid_ = false;
+    RCLCPP_ERROR(LOGGER, "The trajectory is invalid because the time step is 0.");
+    return;
+  }
   trajectory_.push_back(TrajectoryStep(0.0, 0.0));
   double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true);
   while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_)
@@ -566,6 +572,16 @@ bool Trajectory::integrateForward(std::list<TrajectoryStep>& trajectory, double
     trajectory.push_back(TrajectoryStep(path_pos, path_vel));
     acceleration = getMinMaxPathAcceleration(path_pos, path_vel, true);
 
+    if (path_vel == 0 && acceleration == 0)
+    {
+      // The position will never change if velocity and acceleration are zero.
+      // The loop will spin indefinitely as no exit condition is met.
+      valid_ = false;
+      RCLCPP_ERROR(LOGGER, "Error while integrating forward: zero acceleration and velocity. Are any relevant "
+                           "acceleration components limited to zero?");
+      return true;
+    }
+
     if (path_vel > getAccelerationMaxPathVelocity(path_pos) || path_vel > getVelocityMaxPathVelocity(path_pos))
     {
       // Find more accurate intersection with max-velocity curve using bisection
diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
index 5d067c7c82..71a6b9c147 100644
--- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
+++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
@@ -538,6 +538,36 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
   }
 }
 
+TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory)
+{
+  const Eigen::Vector2d max_velocity(1, 1);
+  const Path path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) });
+
+  EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 1)).isValid());
+  EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(1, 0)).isValid());
+  EXPECT_FALSE(Trajectory(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 0)).isValid());
+}
+
+TEST(time_optimal_trajectory_generation, testIrrelevantZeroMaxAccelerationsDontInvalidateTrajectory)
+{
+  const Eigen::Vector2d max_velocity(1, 1);
+
+  EXPECT_TRUE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity,
+                         /*max_acceleration=*/Eigen::Vector2d(0, 1))
+                  .isValid());
+  EXPECT_TRUE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }), max_velocity,
+                         /*max_acceleration=*/Eigen::Vector2d(1, 0))
+                  .isValid());
+}
+
+TEST(time_optimal_trajectory_generation, testTimeStepZeroMakesTrajectoryInvalid)
+{
+  EXPECT_FALSE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }),
+                          /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/Eigen::Vector2d(1, 1),
+                          /*time_step=*/0)
+                   .isValid());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);