Exit loop when position can't change.

moveit · Aug 16, 2023 · 9ef98e5 · 9ef98e5
1 parent a2f0183
commit 9ef98e5
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 0 deletions.
diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -566,6 +566,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
@@ -538,6 +538,29 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity)
   }
 }
 
+TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory)
+{
+  EXPECT_FALSE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }),
+                          Eigen::Vector2d(1, 1), Eigen::Vector2d(0, 1))
+                   .isValid());
+  EXPECT_FALSE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }),
+                          Eigen::Vector2d(1, 1), Eigen::Vector2d(1, 0))
+                   .isValid());
+  EXPECT_FALSE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }),
+                          Eigen::Vector2d(1, 1), Eigen::Vector2d(0, 0))
+                   .isValid());
+}
+
+TEST(time_optimal_trajectory_generation, testIrrelevantZeroMaxAccelerationsDontInvalidateTrajectory)
+{
+  EXPECT_TRUE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }),
+                         Eigen::Vector2d(1, 1), Eigen::Vector2d(0, 1))
+                  .isValid());
+  EXPECT_TRUE(Trajectory(Path(std::list<Eigen::VectorXd>{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }),
+                         Eigen::Vector2d(1, 1), Eigen::Vector2d(1, 0))
+                  .isValid());
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);