From 9ef98e5ea3c4d1431b79a83b96622b58495015e4 Mon Sep 17 00:00:00 2001 From: Nacho Mellado Date: Thu, 17 Aug 2023 01:18:33 +0200 Subject: [PATCH] Exit loop when position can't change. --- .../time_optimal_trajectory_generation.cpp | 10 ++++++++ ...est_time_optimal_trajectory_generation.cpp | 23 +++++++++++++++++++ 2 files changed, 33 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 1c4da3dee2e..7ab97675a8b 100644 --- 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& 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 5d067c7c82f..02c30012203 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,29 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) } } +TEST(time_optimal_trajectory_generation, testRelevantZeroMaxAccelerationsInvalidateTrajectory) +{ + EXPECT_FALSE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }), + Eigen::Vector2d(1, 1), Eigen::Vector2d(0, 1)) + .isValid()); + EXPECT_FALSE(Trajectory(Path(std::list{ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }), + Eigen::Vector2d(1, 1), Eigen::Vector2d(1, 0)) + .isValid()); + EXPECT_FALSE(Trajectory(Path(std::list{ 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::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), + Eigen::Vector2d(1, 1), Eigen::Vector2d(0, 1)) + .isValid()); + EXPECT_TRUE(Trajectory(Path(std::list{ 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);