diff --git a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h index dcb7bc4301f..eea4dad0a23 100644 --- a/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h +++ b/tesseract_task_composer/planning/include/tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h @@ -28,6 +28,7 @@ #include TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include #include TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -55,6 +56,12 @@ struct IterativeSplineParameterizationProfile : public Profile /** @brief max_velocity_scaling_factor The max acceleration scaling factor passed to the solver */ double max_acceleration_scaling_factor{ 1.0 }; + /** + * @brief minimum_time_delta The smallest-allowable difference between timestamps of + * consecutive trajectory points. Passed to the solver. + */ + double minimum_time_delta{ std::numeric_limits::epsilon() }; + protected: friend class boost::serialization::access; template diff --git a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp index ea7e9dca799..6c790dd56ec 100644 --- a/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp +++ b/tesseract_task_composer/planning/src/nodes/iterative_spline_parameterization_task.cpp @@ -171,6 +171,8 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, cur_composite_profile->max_acceleration_scaling_factor; Eigen::VectorXd jerk_scaling_factors = Eigen::VectorXd::Ones(static_cast(flattened.size())); + const auto minimum_time_delta = cur_composite_profile->minimum_time_delta; + // Loop over all MoveInstructions for (Eigen::Index idx = 0; idx < static_cast(flattened.size()); idx++) { @@ -196,7 +198,8 @@ IterativeSplineParameterizationTask::runImpl(TaskComposerContext& context, limits.jerk_limits, velocity_scaling_factors, acceleration_scaling_factors, - jerk_scaling_factors)) + jerk_scaling_factors, + minimum_time_delta)) { // If the output key is not the same as the input key the output data should be assigned the input data for error // branching diff --git a/tesseract_time_parameterization/core/include/tesseract_time_parameterization/core/time_parameterization.h b/tesseract_time_parameterization/core/include/tesseract_time_parameterization/core/time_parameterization.h index e2d4bfe1c43..876b3d2dada 100644 --- a/tesseract_time_parameterization/core/include/tesseract_time_parameterization/core/time_parameterization.h +++ b/tesseract_time_parameterization/core/include/tesseract_time_parameterization/core/time_parameterization.h @@ -43,6 +43,8 @@ class TimeParameterization * @param velocity_scaling_factor The velocity scaling factor. Size should be trajectory.size() * @param acceleration_scaling_factor The acceleration scaling factor. Size should be trajectory.size() * @param jerk_scaling_factor The jerk scaling factor. Size should be trajectory.size() + * @param minimum_time_delta The smallest-allowable difference in seconds between timestamps of consecutive trajectory + * points. * @return True if successful, otherwise false */ virtual bool compute(TrajectoryContainer& trajectory, @@ -51,7 +53,8 @@ class TimeParameterization const Eigen::Ref& jerk_limits, const Eigen::Ref& velocity_scaling_factors, const Eigen::Ref& acceleration_scaling_factors, - const Eigen::Ref& jerk_scaling_factors) const = 0; + const Eigen::Ref& jerk_scaling_factors, + const double& minimum_time_delta) const = 0; }; } // namespace tesseract_planning #endif // TESSERACT_TIME_PARAMETERIZATION_TIME_PARAMETERIZATION_H diff --git a/tesseract_time_parameterization/isp/include/tesseract_time_parameterization/isp/iterative_spline_parameterization.h b/tesseract_time_parameterization/isp/include/tesseract_time_parameterization/isp/iterative_spline_parameterization.h index 365d1c5d137..d34dc478d16 100644 --- a/tesseract_time_parameterization/isp/include/tesseract_time_parameterization/isp/iterative_spline_parameterization.h +++ b/tesseract_time_parameterization/isp/include/tesseract_time_parameterization/isp/iterative_spline_parameterization.h @@ -91,7 +91,8 @@ class IterativeSplineParameterization : public TimeParameterization const Eigen::Ref& jerk_limits, const Eigen::Ref& velocity_scaling_factors = Eigen::VectorXd::Ones(1), const Eigen::Ref& acceleration_scaling_factors = Eigen::VectorXd::Ones(1), - const Eigen::Ref& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override; + const Eigen::Ref& jerk_scaling_factors = Eigen::VectorXd::Ones(1), + const double& minimum_time_delta = std::numeric_limits::epsilon()) const override; private: /** diff --git a/tesseract_time_parameterization/isp/src/iterative_spline_parameterization.cpp b/tesseract_time_parameterization/isp/src/iterative_spline_parameterization.cpp index d9459cfc431..fa6dd2c2d76 100644 --- a/tesseract_time_parameterization/isp/src/iterative_spline_parameterization.cpp +++ b/tesseract_time_parameterization/isp/src/iterative_spline_parameterization.cpp @@ -56,8 +56,12 @@ static void adjust_two_positions(long n, double x2[], // NOLINT double x2_i, // NOLINT double x2_f); // NOLINT -static void -init_times(long n, double dt[], const double x[], const double max_velocity[], double min_velocity[]); // NOLINT +static void init_times(long n, + const double minimum_time_delta, + double dt[], + const double x[], + const double max_velocity[], + double min_velocity[]); // NOLINT // static int fit_spline_and_adjust_times(const int n, // double dt[], // const double x[], @@ -105,7 +109,8 @@ bool IterativeSplineParameterization::compute(TrajectoryContainer& trajectory, const Eigen::Ref& /*jerk_limits*/, const Eigen::Ref& velocity_scaling_factors, const Eigen::Ref& acceleration_scaling_factors, - const Eigen::Ref& /*jerk_scaling_factors*/) const + const Eigen::Ref& /*jerk_scaling_factors*/, + const double& minimum_time_delta) const { if (trajectory.empty()) return true; @@ -342,6 +347,7 @@ bool IterativeSplineParameterization::compute(TrajectoryContainer& trajectory, std::vector time_diff(static_cast(num_points - 1), std::numeric_limits::epsilon()); for (unsigned int j = 0; j < trajectory.dof(); j++) init_times(static_cast(num_points), + minimum_time_delta, time_diff.data(), t2[j].positions_.data(), t2[j].max_velocity_.data(), @@ -548,7 +554,12 @@ static void adjust_two_positions(const long n, Increase a segment's time interval if the current time isn't long enough. */ // NOLINTNEXTLINE -static void init_times(long n, double dt[], const double x[], const double max_velocity[], double min_velocity[]) +static void init_times(long n, + const double minimum_time_delta, + double dt[], + const double x[], + const double max_velocity[], + double min_velocity[]) { for (long i = 0; i < n - 1; i++) { @@ -560,6 +571,11 @@ static void init_times(long n, double dt[], const double x[], const double max_v time = (dx / min_velocity[i]); time += std::numeric_limits::epsilon(); // prevent divide-by-zero + if (time < minimum_time_delta) + { + time = minimum_time_delta; + } + if (dt[i] < time) dt[i] = time; } diff --git a/tesseract_time_parameterization/isp/test/iterative_spline_tests.cpp b/tesseract_time_parameterization/isp/test/iterative_spline_tests.cpp index f9e2c16fd3f..4e952075ffc 100644 --- a/tesseract_time_parameterization/isp/test/iterative_spline_tests.cpp +++ b/tesseract_time_parameterization/isp/test/iterative_spline_tests.cpp @@ -194,6 +194,44 @@ TEST(TestTimeParameterization, TestRepeatedPoint) // NOLINT ASSERT_LT(program.back().as().getWaypoint().as().getTime(), 0.001); } +TEST(TestTimeParameterization, TestEnforceMinimumDelta) +{ + // GIVEN a single-joint trajectory that is straight in joint-space + IterativeSplineParameterization time_parameterization(false); + CompositeInstruction program = createStraightTrajectory(); + TrajectoryContainer::Ptr trajectory = std::make_shared(program); + + // GIVEN valid limits for each joint + Eigen::MatrixX2d max_velocity(6, 2); + max_velocity.col(0) << -2.088, -2.082, -3.27, -3.6, -3.3, -3.078; + max_velocity.col(1) << 2.088, 2.082, 3.27, 3.6, 3.3, 3.078; + Eigen::MatrixX2d max_acceleration(6, 2); + max_acceleration.col(0) << -1, -1, -1, -1, -1, -1; + max_acceleration.col(1) << 1, 1, 1, 1, 1, 1; + Eigen::MatrixX2d max_jerk(6, 2); + max_jerk.col(0) << -1, -1, -1, -1, -1, -1; + max_jerk.col(1) << 1, 1, 1, 1, 1, 1; + + // WHEN we compute time parameterization while enforcing a minimum time delta that is much larger than the optimal + // time delta given the specified limits + constexpr double minimum_time_delta = 10.0; + // THEN time parameterization succeeds + EXPECT_TRUE(time_parameterization.compute(*trajectory, + max_velocity, + max_acceleration, + max_jerk, + Eigen::VectorXd::Ones(1), + Eigen::VectorXd::Ones(1), + Eigen::VectorXd::Ones(1), + minimum_time_delta)); + // THEN the timestamp of the first trajectory point is greater than or equal to the minimum time delta + ASSERT_GE(program[1].as().getWaypoint().as().getTime(), minimum_time_delta); + // THEN the timestamp of the last trajectory point shows that the minimum time delta has been enforced for each + // previous trajectory point + ASSERT_GE(program.back().as().getWaypoint().as().getTime(), + minimum_time_delta * static_cast(program.size() - 1)); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_time_parameterization/ruckig/include/tesseract_time_parameterization/ruckig/ruckig_trajectory_smoothing.h b/tesseract_time_parameterization/ruckig/include/tesseract_time_parameterization/ruckig/ruckig_trajectory_smoothing.h index 314f318f01a..882f6c9cc1b 100644 --- a/tesseract_time_parameterization/ruckig/include/tesseract_time_parameterization/ruckig/ruckig_trajectory_smoothing.h +++ b/tesseract_time_parameterization/ruckig/include/tesseract_time_parameterization/ruckig/ruckig_trajectory_smoothing.h @@ -57,7 +57,8 @@ class RuckigTrajectorySmoothing : public TimeParameterization const Eigen::Ref& jerk_limits, const Eigen::Ref& velocity_scaling_factors = Eigen::VectorXd::Ones(1), const Eigen::Ref& acceleration_scaling_factors = Eigen::VectorXd::Ones(1), - const Eigen::Ref& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override; + const Eigen::Ref& jerk_scaling_factors = Eigen::VectorXd::Ones(1), + const double& minimum_time_delta = std::numeric_limits::epsilon()) const override; protected: double duration_extension_fraction_; diff --git a/tesseract_time_parameterization/ruckig/src/ruckig_trajectory_smoothing.cpp b/tesseract_time_parameterization/ruckig/src/ruckig_trajectory_smoothing.cpp index 977e9bdb3a6..57d0502a0f5 100644 --- a/tesseract_time_parameterization/ruckig/src/ruckig_trajectory_smoothing.cpp +++ b/tesseract_time_parameterization/ruckig/src/ruckig_trajectory_smoothing.cpp @@ -73,7 +73,8 @@ bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, const Eigen::Ref& max_jerk, const Eigen::Ref& max_velocity_scaling_factors, const Eigen::Ref& max_acceleration_scaling_factors, - const Eigen::Ref& max_jerk_scaling_factors) const + const Eigen::Ref& max_jerk_scaling_factors, + const double& /*minimum_time_delta*/) const { if (trajectory.size() < 2) return true; @@ -208,7 +209,8 @@ bool RuckigTrajectorySmoothing::compute(TrajectoryContainer& trajectory, const Eigen::Ref& jerk_limits, const Eigen::Ref& /*velocity_scaling_factors*/, const Eigen::Ref& /*acceleration_scaling_factors*/, - const Eigen::Ref& /*jerk_scaling_factors*/) const + const Eigen::Ref& /*jerk_scaling_factors*/, + const double& /*minimum_time_delta*/) const { if (trajectory.size() < 2) return true; diff --git a/tesseract_time_parameterization/totg/include/tesseract_time_parameterization/totg/time_optimal_trajectory_generation.h b/tesseract_time_parameterization/totg/include/tesseract_time_parameterization/totg/time_optimal_trajectory_generation.h index 15b02e821c7..bc6299d77de 100644 --- a/tesseract_time_parameterization/totg/include/tesseract_time_parameterization/totg/time_optimal_trajectory_generation.h +++ b/tesseract_time_parameterization/totg/include/tesseract_time_parameterization/totg/time_optimal_trajectory_generation.h @@ -67,7 +67,8 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization const Eigen::Ref& jerk_limits, const Eigen::Ref& velocity_scaling_factors = Eigen::VectorXd::Ones(1), const Eigen::Ref& acceleration_scaling_factors = Eigen::VectorXd::Ones(1), - const Eigen::Ref& jerk_scaling_factors = Eigen::VectorXd::Ones(1)) const override; + const Eigen::Ref& jerk_scaling_factors = Eigen::VectorXd::Ones(1), + const double& minimum_time_delta = std::numeric_limits::epsilon()) const override; private: double path_tolerance_; diff --git a/tesseract_time_parameterization/totg/src/time_optimal_trajectory_generation.cpp b/tesseract_time_parameterization/totg/src/time_optimal_trajectory_generation.cpp index d80772d11e0..f18892ec583 100644 --- a/tesseract_time_parameterization/totg/src/time_optimal_trajectory_generation.cpp +++ b/tesseract_time_parameterization/totg/src/time_optimal_trajectory_generation.cpp @@ -67,7 +67,8 @@ bool TimeOptimalTrajectoryGeneration::compute(TrajectoryContainer& trajectory, const Eigen::Ref& /*jerk_limits*/, const Eigen::Ref& velocity_scaling_factors, const Eigen::Ref& acceleration_scaling_factors, - const Eigen::Ref& /*jerk_scaling_factors*/) const + const Eigen::Ref& /*jerk_scaling_factors*/, + const double& /*minimum_time_delta*/) const { if (trajectory.empty()) return true;