From cdd8d14cd9b471e8c1aee1baee9ceb58eb2da46f Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 3 Jan 2024 01:45:53 -0800 Subject: [PATCH] make time_optimal_trajectory_generation harder to misuse (#2624) * make time_optimal_trajectory_generation harder to misuse * style fixes * more style fixes * more style fixes * address PR comments --- MIGRATION.md | 3 +- .../time_optimal_trajectory_generation.h | 47 ++-- .../time_optimal_trajectory_generation.cpp | 231 +++++++++++------- ...est_time_optimal_trajectory_generation.cpp | 137 ++++++++--- 4 files changed, 275 insertions(+), 143 deletions(-) diff --git a/MIGRATION.md b/MIGRATION.md index 8a851b09d2..9af12e14c4 100644 --- a/MIGRATION.md +++ b/MIGRATION.md @@ -3,6 +3,8 @@ API changes in MoveIt releases ## ROS Rolling +- [12/2023] `trajectory_processing::Path` and `trajectory_processing::Trajectory` APIs have been updated to prevent misuse. +The constructors have been replaced by builder methods so that errors can be communicated. Paths and trajectories now need to be created with `Path::Create()` and `Trajectory::Create()`. These methods now return an `std::optional` that needs to be checked for a valid value. `Trajectory` no longer has the `isValid()` method. If it's invalid, `Trajectory::Create()` will return `std::nullopt`. Finally, `Path` now takes the list of input waypoints as `std::vector`, instead of `std::list`. - [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK - [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this: ```diff @@ -10,7 +12,6 @@ API changes in MoveIt releases + planning_plugins: + - ompl_interface/OMPLPlanner ``` - - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this: ```diff - request_adapters: >- diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index 0a80bff316..8f7b90ec03 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -45,6 +45,12 @@ namespace trajectory_processing { + +// The intermediate waypoints of the input path need to be blended so that the entire path is diffentiable. +// This constant defines the maximum deviation allowed at those intermediate waypoints, in radians for revolute joints, +// or meters for prismatic joints. +constexpr double DEFAULT_PATH_TOLERANCE = 0.1; + enum LimitType { VELOCITY, @@ -81,8 +87,14 @@ class PathSegment class Path { public: - Path(const std::list& path, double max_deviation = 0.0); + // Create a Path from a vector of waypoints and a maximum deviation to tolerate at the intermediate waypoints. + // The algorithm needs max_deviation to be greater than zero so that the path is differentiable. + static std::optional create(const std::vector& waypoint, + double max_deviation = DEFAULT_PATH_TOLERANCE); + + // Copy constructor. Path(const Path& path); + double getLength() const; Eigen::VectorXd getConfig(double s) const; Eigen::VectorXd getTangent(double s) const; @@ -99,8 +111,12 @@ class Path std::list> getSwitchingPoints() const; private: + // Default constructor private to prevent misuse. Use `create` instead to create a Path object. + Path() = default; + PathSegment* getPathSegment(double& s) const; - double length_; + + double length_ = 0.0; std::list> switching_points_; std::list> path_segments_; }; @@ -108,16 +124,10 @@ class Path class Trajectory { public: - /// @brief Generates a time-optimal trajectory - Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, - double time_step = 0.001); - - ~Trajectory(); - - /** @brief Call this method after constructing the object to make sure the - trajectory generation succeeded without errors. If this method returns - false, all other methods have undefined behavior. **/ - bool isValid() const; + /// @brief Generates a time-optimal trajectory. + /// @returns std::nullopt if the trajectory couldn't be parameterized. + static std::optional create(const Path& path, const Eigen::VectorXd& max_velocity, + const Eigen::VectorXd& max_acceleration, double time_step = 0.001); /// @brief Returns the optimal duration of the trajectory double getDuration() const; @@ -131,6 +141,9 @@ class Trajectory Eigen::VectorXd getAcceleration(double time) const; private: + Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, + double time_step); + struct TrajectoryStep { TrajectoryStep() @@ -165,14 +178,14 @@ class Trajectory Path path_; Eigen::VectorXd max_velocity_; Eigen::VectorXd max_acceleration_; - unsigned int joint_num_; - bool valid_; + unsigned int joint_num_ = 0.0; + bool valid_ = true; std::list trajectory_; std::list end_trajectory_; // non-empty only if the trajectory generation failed. - const double time_step_; + double time_step_ = 0.0; - mutable double cached_time_; + mutable double cached_time_ = std::numeric_limits::max(); mutable std::list::const_iterator cached_trajectory_segment_; }; @@ -180,7 +193,7 @@ MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration); class TimeOptimalTrajectoryGeneration : public TimeParameterization { public: - TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, const double resample_dt = 0.1, + TimeOptimalTrajectoryGeneration(const double path_tolerance = DEFAULT_PATH_TOLERANCE, const double resample_dt = 0.1, const double min_angle_change = 0.001); // clang-format off 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 5f7a66c894..d38eae1784 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -198,59 +198,95 @@ class CircularPathSegment : public PathSegment Eigen::VectorXd y_; }; -Path::Path(const std::list& path, double max_deviation) : length_(0.0) +std::optional Path::create(const std::vector& waypoints, double max_deviation) { - if (path.size() < 2) - return; - std::list::const_iterator path_iterator1 = path.begin(); - std::list::const_iterator path_iterator2 = path_iterator1; - ++path_iterator2; - std::list::const_iterator path_iterator3; - Eigen::VectorXd start_config = *path_iterator1; - while (path_iterator2 != path.end()) + if (waypoints.size() < 2) { - path_iterator3 = path_iterator2; - ++path_iterator3; - if (max_deviation > 0.0 && path_iterator3 != path.end()) + RCLCPP_ERROR(getLogger(), "A path needs at least 2 waypoints."); + return std::nullopt; + } + if (max_deviation <= 0.0) + { + RCLCPP_ERROR(getLogger(), "Path max_deviation must be greater than 0.0."); + return std::nullopt; + } + + // waypoints_iterator1, waypoints_iterator2 and waypoints_iterator3 point to three consecutive waypoints of the input + // path. The algorithm creates a LinearPathSegment starting at waypoints_iterator1, connected to CircularPathSegment + // at waypoints_iterator2, connected to another LinearPathSegment towards waypoints_iterator3. + // It does this iteratively for each three consecutive waypoints, therefore applying a blending of 'max_deviation' at + // the intermediate waypoints. + Path path; + std::vector::const_iterator waypoints_iterator1 = waypoints.begin(); + std::vector::const_iterator waypoints_iterator2 = waypoints_iterator1; + ++waypoints_iterator2; + std::vector::const_iterator waypoints_iterator3; + Eigen::VectorXd start_config = *waypoints_iterator1; + while (waypoints_iterator2 != waypoints.end()) + { + waypoints_iterator3 = waypoints_iterator2; + ++waypoints_iterator3; + if (waypoints_iterator3 != waypoints.end()) + { + // Check that the path is not making a 180 deg. turn, which is not supported by the current implementation. + Eigen::VectorXd incoming_vector = *waypoints_iterator2 - *waypoints_iterator1; + Eigen::VectorXd outcoming_vector = *waypoints_iterator3 - *waypoints_iterator2; + double incoming_vector_norm = incoming_vector.norm(); + double outcoming_vector_norm = outcoming_vector.norm(); + if (incoming_vector_norm > std::numeric_limits::epsilon() && + outcoming_vector_norm > std::numeric_limits::epsilon()) + { + double cos_angle = incoming_vector.dot(outcoming_vector) / (incoming_vector_norm * outcoming_vector_norm); + constexpr double angle_tolerance = 1e-05; + if (cos_angle <= -1.0 + angle_tolerance) + { + RCLCPP_ERROR(getLogger(), + "The path requires a 180 deg. turn, which is not supported by the current implementation."); + return std::nullopt; + } + } + } + if (max_deviation > 0.0 && waypoints_iterator3 != waypoints.end()) { CircularPathSegment* blend_segment = - new CircularPathSegment(0.5 * (*path_iterator1 + *path_iterator2), *path_iterator2, - 0.5 * (*path_iterator2 + *path_iterator3), max_deviation); + new CircularPathSegment(0.5 * (*waypoints_iterator1 + *waypoints_iterator2), *waypoints_iterator2, + 0.5 * (*waypoints_iterator2 + *waypoints_iterator3), max_deviation); Eigen::VectorXd end_config = blend_segment->getConfig(0.0); if ((end_config - start_config).norm() > 0.000001) { - path_segments_.push_back(std::make_unique(start_config, end_config)); + path.path_segments_.push_back(std::make_unique(start_config, end_config)); } - path_segments_.emplace_back(blend_segment); + path.path_segments_.emplace_back(blend_segment); start_config = blend_segment->getConfig(blend_segment->getLength()); } else { - path_segments_.push_back(std::make_unique(start_config, *path_iterator2)); - start_config = *path_iterator2; + path.path_segments_.push_back(std::make_unique(start_config, *waypoints_iterator2)); + start_config = *waypoints_iterator2; } - path_iterator1 = path_iterator2; - ++path_iterator2; + waypoints_iterator1 = waypoints_iterator2; + ++waypoints_iterator2; } // Create list of switching point candidates, calculate total path length and // absolute positions of path segments - for (std::unique_ptr& path_segment : path_segments_) + for (std::unique_ptr& path_segment : path.path_segments_) { - path_segment->position_ = length_; + path_segment->position_ = path.length_; std::list local_switching_points = path_segment->getSwitchingPoints(); for (std::list::const_iterator point = local_switching_points.begin(); point != local_switching_points.end(); ++point) { - switching_points_.push_back(std::make_pair(length_ + *point, false)); + path.switching_points_.push_back(std::make_pair(path.length_ + *point, false)); } - length_ += path_segment->getLength(); - while (!switching_points_.empty() && switching_points_.back().first >= length_) - switching_points_.pop_back(); - switching_points_.push_back(std::make_pair(length_, true)); + path.length_ += path_segment->getLength(); + while (!path.switching_points_.empty() && path.switching_points_.back().first >= path.length_) + path.switching_points_.pop_back(); + path.switching_points_.push_back(std::make_pair(path.length_, true)); } - switching_points_.pop_back(); + path.switching_points_.pop_back(); + return path; } Path::Path(const Path& path) : length_(path.length_), switching_points_(path.switching_points_) @@ -319,60 +355,66 @@ std::list> Path::getSwitchingPoints() const return switching_points_; } -Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, - double time_step) - : path_(path) - , max_velocity_(max_velocity) - , max_acceleration_(max_acceleration) - , joint_num_(max_velocity.size()) - , valid_(true) - , time_step_(time_step) - , cached_time_(std::numeric_limits::max()) +std::optional Trajectory::create(const Path& path, const Eigen::VectorXd& max_velocity, + const Eigen::VectorXd& max_acceleration, double time_step) { - if (time_step_ == 0) + if (time_step <= 0) { - valid_ = false; - RCLCPP_ERROR(getLogger(), "The trajectory is invalid because the time step is 0."); - return; + RCLCPP_ERROR(getLogger(), "The trajectory is invalid because the time step is <= 0.0."); + return std::nullopt; } - trajectory_.push_back(TrajectoryStep(0.0, 0.0)); - double after_acceleration = getMinMaxPathAcceleration(0.0, 0.0, true); - while (valid_ && !integrateForward(trajectory_, after_acceleration) && valid_) + + Trajectory output(path, max_velocity, max_acceleration, time_step); + output.trajectory_.push_back(TrajectoryStep(0.0, 0.0)); + double after_acceleration = output.getMinMaxPathAcceleration(0.0, 0.0, true); + while (output.valid_ && !output.integrateForward(output.trajectory_, after_acceleration) && output.valid_) { double before_acceleration; TrajectoryStep switching_point; - if (getNextSwitchingPoint(trajectory_.back().path_pos_, switching_point, before_acceleration, after_acceleration)) + if (output.getNextSwitchingPoint(output.trajectory_.back().path_pos_, switching_point, before_acceleration, + after_acceleration)) { break; } - integrateBackward(trajectory_, switching_point.path_pos_, switching_point.path_vel_, before_acceleration); + output.integrateBackward(output.trajectory_, switching_point.path_pos_, switching_point.path_vel_, + before_acceleration); + } + + if (!output.valid_) + { + RCLCPP_ERROR(getLogger(), "Trajectory not valid after integrateForward and integrateBackward."); + return std::nullopt; } - if (valid_) + double before_acceleration = output.getMinMaxPathAcceleration(output.path_.getLength(), 0.0, false); + output.integrateBackward(output.trajectory_, output.path_.getLength(), 0.0, before_acceleration); + + if (!output.valid_) { - double before_acceleration = getMinMaxPathAcceleration(path_.getLength(), 0.0, false); - integrateBackward(trajectory_, path_.getLength(), 0.0, before_acceleration); + RCLCPP_ERROR(getLogger(), "Trajectory not valid after the second integrateBackward pass."); + return std::nullopt; } - if (valid_) + // Calculate timing. + std::list::iterator previous = output.trajectory_.begin(); + std::list::iterator it = previous; + it->time_ = 0.0; + ++it; + while (it != output.trajectory_.end()) { - // Calculate timing - std::list::iterator previous = trajectory_.begin(); - std::list::iterator it = previous; - it->time_ = 0.0; + it->time_ = previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0); + previous = it; ++it; - while (it != trajectory_.end()) - { - it->time_ = - previous->time_ + (it->path_pos_ - previous->path_pos_) / ((it->path_vel_ + previous->path_vel_) / 2.0); - previous = it; - ++it; - } } + + return output; } -Trajectory::~Trajectory() +Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, const Eigen::VectorXd& max_acceleration, + double time_step) + : path_(path), max_velocity_(max_velocity), max_acceleration_(max_acceleration), time_step_(time_step) { + joint_num_ = max_velocity.size(); } // Returns true if end of path is reached. @@ -791,11 +833,6 @@ double Trajectory::getVelocityMaxPathVelocityDeriv(double path_pos) (tangent[active_constraint] * std::abs(tangent[active_constraint])); } -bool Trajectory::isValid() const -{ - return valid_; -} - double Trajectory::getDuration() const { return trajectory_.back().time_; @@ -933,9 +970,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } else { - RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " - << vars[idx].c_str() - << "! You have to define velocity limits in the URDF or joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " << vars[idx].c_str() + << "! You have to define velocity " + "limits " + "in the URDF or " + "joint_limits.yaml"); return false; } @@ -952,10 +991,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } else { - RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " - << vars[idx].c_str() - << "! You have to define acceleration limits in the URDF or " - "joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " << vars[idx].c_str() + << "! You have to define " + "acceleration " + "limits in the URDF or " + "joint_limits.yaml"); return false; } } @@ -1049,10 +1089,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( if (!set_velocity_limit) { - RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " - << vars[idx].c_str() - << "! You have to define velocity limits in the URDF or " - "joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No velocity limit was defined for joint " << vars[idx].c_str() + << "! You have to define velocity " + "limits " + "in the URDF or " + "joint_limits.yaml"); return false; } @@ -1081,10 +1122,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( } if (!set_acceleration_limit) { - RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " - << vars[idx].c_str() - << "! You have to define acceleration limits in the URDF or " - "joint_limits.yaml"); + RCLCPP_ERROR_STREAM(getLogger(), "No acceleration limit was defined for joint " << vars[idx].c_str() + << "! You have to define " + "acceleration " + "limits in the URDF or " + "joint_limits.yaml"); return false; } } @@ -1143,7 +1185,7 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t // Have to convert into Eigen data structs and remove repeated points // (https://github.com/tobiaskunz/trajectories/issues/3) - std::list points; + std::vector points; for (size_t p = 0; p < num_points; ++p) { moveit::core::RobotStatePtr waypoint = trajectory.getWayPointPtr(p); @@ -1185,15 +1227,22 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t } // Now actually call the algorithm - Trajectory parameterized(Path(points, path_tolerance_), max_velocity, max_acceleration, DEFAULT_TIMESTEP); - if (!parameterized.isValid()) + std::optional path = Path::create(points, path_tolerance_); + if (!path) + { + RCLCPP_ERROR(getLogger(), "Invalid path."); + return false; + } + + std::optional parameterized = Trajectory::create(*path, max_velocity, max_acceleration, DEFAULT_TIMESTEP); + if (!parameterized) { - RCLCPP_ERROR(getLogger(), "Unable to parameterize trajectory."); + RCLCPP_ERROR(getLogger(), "Couldn't create trajectory"); return false; } // Compute sample count - size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt_); + const size_t sample_count = std::ceil(parameterized->getDuration() / resample_dt_); // Resample and fill in trajectory moveit::core::RobotState waypoint = moveit::core::RobotState(trajectory.getWayPoint(0)); @@ -1202,10 +1251,10 @@ bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_t for (size_t sample = 0; sample <= sample_count; ++sample) { // always sample the end of the trajectory as well - double t = std::min(parameterized.getDuration(), sample * resample_dt_); - Eigen::VectorXd position = parameterized.getPosition(t); - Eigen::VectorXd velocity = parameterized.getVelocity(t); - Eigen::VectorXd acceleration = parameterized.getAcceleration(t); + double t = std::min(parameterized->getDuration(), sample * resample_dt_); + Eigen::VectorXd position = parameterized->getPosition(t); + Eigen::VectorXd velocity = parameterized->getVelocity(t); + Eigen::VectorXd acceleration = parameterized->getAcceleration(t); for (size_t j = 0; j < num_joints; ++j) { 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 b83f7ffadb..ba3f9681cb 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 @@ -70,7 +70,7 @@ void setAccelerationLimits(const moveit::core::RobotModelPtr& robot_model) TEST(time_optimal_trajectory_generation, test1) { Eigen::VectorXd waypoint(4); - std::list waypoints; + std::vector waypoints; waypoint << 1424.0, 984.999694824219, 2126.0, 0.0; waypoints.push_back(waypoint); @@ -82,8 +82,13 @@ TEST(time_optimal_trajectory_generation, test1) Eigen::VectorXd max_accelerations(4); max_accelerations << 0.00249, 0.00249, 0.00249, 0.00249; - Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations, 10.0); - EXPECT_TRUE(trajectory.isValid()); + auto path_maybe = Path::create(waypoints, 100.0); + ASSERT_TRUE(path_maybe.has_value()); + + auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 10.0); + ASSERT_TRUE(trajectory_maybe.has_value()); + const Trajectory& trajectory = trajectory_maybe.value(); + EXPECT_DOUBLE_EQ(40.080256821829849, trajectory.getDuration()); // Test start matches @@ -107,7 +112,7 @@ TEST(time_optimal_trajectory_generation, test1) TEST(time_optimal_trajectory_generation, test2) { Eigen::VectorXd waypoint(4); - std::list waypoints; + std::vector waypoints; waypoint << 1427.0, 368.0, 690.0, 90.0; waypoints.push_back(waypoint); @@ -125,8 +130,13 @@ TEST(time_optimal_trajectory_generation, test2) Eigen::VectorXd max_accelerations(4); max_accelerations << 0.002, 0.002, 0.002, 0.002; - Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations, 10.0); - EXPECT_TRUE(trajectory.isValid()); + auto path_maybe = Path::create(waypoints, 100.0); + ASSERT_TRUE(path_maybe.has_value()); + + auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 10.0); + ASSERT_TRUE(trajectory_maybe.has_value()); + const Trajectory& trajectory = trajectory_maybe.value(); + EXPECT_DOUBLE_EQ(1922.1418427445944, trajectory.getDuration()); // Test start matches @@ -150,7 +160,7 @@ TEST(time_optimal_trajectory_generation, test2) TEST(time_optimal_trajectory_generation, test3) { Eigen::VectorXd waypoint(4); - std::list waypoints; + std::vector waypoints; waypoint << 1427.0, 368.0, 690.0, 90.0; waypoints.push_back(waypoint); @@ -168,8 +178,13 @@ TEST(time_optimal_trajectory_generation, test3) Eigen::VectorXd max_accelerations(4); max_accelerations << 0.002, 0.002, 0.002, 0.002; - Trajectory trajectory(Path(waypoints, 100.0), max_velocities, max_accelerations); - EXPECT_TRUE(trajectory.isValid()); + auto path_maybe = Path::create(waypoints, 100.0); + ASSERT_TRUE(path_maybe.has_value()); + + auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations); + ASSERT_TRUE(trajectory_maybe.has_value()); + const Trajectory& trajectory = trajectory_maybe.value(); + EXPECT_DOUBLE_EQ(1919.5597888812974, trajectory.getDuration()); // Test start matches @@ -230,7 +245,7 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) double path_tolerance = 0.1; double resample_dt = 0.1; Eigen::VectorXd waypoint(6); - std::list waypoints; + std::vector waypoints; Eigen::VectorXd max_velocities(6); Eigen::VectorXd max_accelerations(6); @@ -284,9 +299,12 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) 4.7292792634680003; // clang-format on - Trajectory parameterized(Path(waypoints, path_tolerance), max_velocities, max_accelerations, 0.001); + auto path_maybe = Path::create(waypoints, path_tolerance); + ASSERT_TRUE(path_maybe.has_value()); - ASSERT_TRUE(parameterized.isValid()); + auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 0.001); + ASSERT_TRUE(trajectory_maybe.has_value()); + const Trajectory& parameterized = trajectory_maybe.value(); size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt); for (size_t sample = 0; sample <= sample_count; ++sample) @@ -301,6 +319,57 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) } } +// This test differentiates the trajectory velocities and verifies that the acceleration limit is actually respected. +// A path tolerance of 0.0 makes this test fail. Output trajectory accelerations are all within the limits, but there +// are jumps in velocity, i.e. the derivative of the velocity is not consistent with the acceleration. +// The test is here mostly to prevent us from making a path tolerance of 0.0 a valid default again in the future. +TEST(time_optimal_trajectory_generation, AccelerationLimitIsRespected) +{ + double path_tolerance = 0.001; + double resample_dt = 0.01; + + // Waypoints. + std::vector waypoints; + waypoints.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); + waypoints.push_back(Eigen::Vector3d(1.0, 0.0, 0.0)); + waypoints.push_back(Eigen::Vector3d(1.0, 1.0, 0.0)); + + Eigen::VectorXd max_velocities = Eigen::VectorXd::Constant(3, 0.1); + Eigen::VectorXd max_accelerations = Eigen::VectorXd::Constant(3, 0.5); + + auto path_maybe = Path::create(waypoints, path_tolerance); + ASSERT_TRUE(path_maybe.has_value()); + + auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 0.001); + ASSERT_TRUE(trajectory_maybe.has_value()); + const Trajectory& parameterized = trajectory_maybe.value(); + + size_t sample_count = std::ceil(parameterized.getDuration() / resample_dt); + Eigen::Vector3d previous_velocity = Eigen::Vector3d::Zero(); + for (size_t sample = 0; sample <= sample_count; ++sample) + { + double t = std::min(parameterized.getDuration(), sample * resample_dt); + Eigen::Vector3d velocity = parameterized.getVelocity(t); + + double acceleration = (velocity - previous_velocity).norm() / resample_dt; + EXPECT_LT(acceleration, max_accelerations.norm() + 1e-3); + previous_velocity = velocity; + } +} + +// A path that requires a full 180 degree turn at any point is not supported by the current implementation. +// Path::create() should fail. +TEST(time_optimal_trajectory_generation, PathMakes180DegreeTurn) +{ + // Waypoints. + std::vector waypoints; + waypoints.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); + waypoints.push_back(Eigen::Vector3d(1.0, 0.0, 0.0)); + waypoints.push_back(Eigen::Vector3d(0.0, 0.0, 0.0)); + + EXPECT_FALSE(Path::create(waypoints, /*path_tolerance=*/0.01)); +} + // Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end // waypoint TEST(time_optimal_trajectory_generation, testLastWaypoint) @@ -362,9 +431,9 @@ TEST(time_optimal_trajectory_generation, testPluginAPI) trajectory.addSuffixWayPoint(waypoint_state, 0.1); waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 }); trajectory.addSuffixWayPoint(waypoint_state, 0.1); - waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.00, 1.35, -2.51, -0.88, 0.63, 0.0 }); trajectory.addSuffixWayPoint(waypoint_state, 0.1); - waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.0, -0.2, 0.0 }); + waypoint_state.setJointGroupPositions(group, std::vector{ 0.0, -3.5, 1.4, -1.2, -1.5, -0.2, 0.0 }); trajectory.addSuffixWayPoint(waypoint_state, 0.1); waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); trajectory.addSuffixWayPoint(waypoint_state, 0.1); @@ -485,8 +554,8 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints) waypoint_state.setJointGroupPositions(group, std::vector{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 }); trajectory.addSuffixWayPoint(waypoint_state, delta_t); - ASSERT_TRUE(trajectory_processing::totgComputeTimeStamps(desired_num_waypoints, trajectory)) - << "Failed to compute time stamps"; + ASSERT_TRUE(trajectory_processing::totgComputeTimeStamps(desired_num_waypoints, trajectory)) << "Failed to compute " + "time stamps"; // Allow +/-1 waypoint due to floating point error EXPECT_NEAR(trajectory.getWayPointCount(), desired_num_waypoints, 1); } @@ -495,7 +564,7 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) { // Test a (prior) specific failure case Eigen::VectorXd waypoint(1); - std::list waypoints; + std::vector waypoints; const double start_position = 1.881943; waypoint << start_position; @@ -508,9 +577,12 @@ TEST(time_optimal_trajectory_generation, testSingleDofDiscontinuity) Eigen::VectorXd max_accelerations(1); max_accelerations << 28.0; - Trajectory trajectory(Path(waypoints, 0.1 /* path tolerance */), max_velocities, max_accelerations, - 0.001 /* timestep */); - EXPECT_TRUE(trajectory.isValid()); + auto path_maybe = Path::create(waypoints, 0.1 /* path tolerance */); + ASSERT_TRUE(path_maybe.has_value()); + + auto trajectory_maybe = Trajectory::create(*path_maybe, max_velocities, max_accelerations, 0.001 /* timestep */); + ASSERT_TRUE(trajectory_maybe.has_value()); + const Trajectory& trajectory = trajectory_maybe.value(); EXPECT_GT(trajectory.getDuration(), 0.0); const double traj_duration = trajectory.getDuration(); @@ -541,31 +613,28 @@ 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) }); + const Path path = *Path::create({ 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()); + EXPECT_FALSE(Trajectory::create(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 1))); + EXPECT_FALSE(Trajectory::create(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(1, 0))); + EXPECT_FALSE(Trajectory::create(path, max_velocity, /*max_acceleration=*/Eigen::Vector2d(0, 0))); } 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()); + EXPECT_TRUE(Trajectory::create(*Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(0, 1) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(0, 1))); + EXPECT_TRUE(Trajectory::create(*Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 0) }), max_velocity, + /*max_acceleration=*/Eigen::Vector2d(1, 0))); } 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()); + EXPECT_FALSE(Trajectory::create(*Path::create({ Eigen::Vector2d(0, 0), Eigen::Vector2d(1, 1) }), + /*max_velocity=*/Eigen::Vector2d(1, 1), /*max_acceleration=*/Eigen::Vector2d(1, 1), + /*time_step=*/0)); } int main(int argc, char** argv)