From 04b4f35ad559bb3c9b64aec6941b33b79653a310 Mon Sep 17 00:00:00 2001 From: Nacho Mellado Date: Thu, 17 Aug 2023 17:32:30 +0200 Subject: [PATCH] Deal with time_step==0 --- .../src/time_optimal_trajectory_generation.cpp | 6 ++++++ ...test_time_optimal_trajectory_generation.cpp | 18 +++++++++++++----- 2 files changed, 19 insertions(+), 5 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 index 7ab97675a8b..70a6272df16 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_) 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 02c30012203..d6a42e96b72 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 @@ -541,26 +541,34 @@ 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)) + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/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)) + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/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)) + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/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)) + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/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)) + /*max_velocity=*/Eigen::Vector2d(1, 1), /*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);