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::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& 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::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::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(0, 1)) + .isValid()); + EXPECT_TRUE(Trajectory(Path(std::list{ 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::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);