diff --git a/CHANGELOG.md b/CHANGELOG.md index 434a573cfe..9cec7f4752 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -14,6 +14,10 @@ * Fixed CollisionGroup bugs in Hand executors: [#299](https://github.com/personalrobotics/aikido/pull/299) +* Planner + + * Added parabolic timing for linear spline [#302](https://github.com/personalrobotics/aikido/pull/302) + ### 0.2.0 (2018-01-09) * State Space diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp index 8511bd5fb9..d9b9300f80 100644 --- a/include/aikido/planner/parabolic/ParabolicTimer.hpp +++ b/include/aikido/planner/parabolic/ParabolicTimer.hpp @@ -35,6 +35,31 @@ std::unique_ptr computeParabolicTiming( const Eigen::VectorXd& _maxVelocity, const Eigen::VectorXd& _maxAcceleration); +/// Computes the time-optimal timing of a trajectory consisting of a linear +/// spline between states under velocity and acceleration +/// bounds. The output is a parabolic spline, encoded in cubic polynomials, +/// that \b exactly follows the input path. +/// +/// The output trajectory consists of a sequence of trapezoidal velocity +/// profiles that implement bang-bang control. This trajectory must stop at +/// each waypoint that introduces a velocity discontinuity to satisfy finite +/// acceleration bounds. You should consider using a blending or smoothing +/// algorithm, which does \b not follow the exact input path, if this behavior +/// is undesirable. +/// +/// This function curently only supports \c RealVector, \c SO2, and compound +/// state spaces of those types. Additionally, this function requires that +/// \c _inputTrajectory to be interpolated using a \c GeodesicInterpolator. +/// +/// \param _inputTrajectory linear spline trajectory +/// \param _maxVelocity maximum velocity for each dimension +/// \param _maxAcceleration maximum acceleration for each dimension +/// \return time optimal trajectory that satisfies acceleration constraints +std::unique_ptr computeParabolicTiming( + const aikido::trajectory::Spline& _inputTrajectory, + const Eigen::VectorXd& _maxVelocity, + const Eigen::VectorXd& _maxAcceleration); + /// Convert an interpolated trajectory to a piecewise linear spline trajectory /// This function requires the \c _inputTrajectory to use a \c /// GeodesicInterpolator. diff --git a/src/planner/parabolic/ParabolicTimer.cpp b/src/planner/parabolic/ParabolicTimer.cpp index 522d84879c..690210d430 100644 --- a/src/planner/parabolic/ParabolicTimer.cpp +++ b/src/planner/parabolic/ParabolicTimer.cpp @@ -18,6 +18,7 @@ namespace aikido { namespace planner { namespace parabolic { +//============================================================================== std::unique_ptr convertToSpline( const aikido::trajectory::Interpolated& _inputTrajectory) { @@ -78,6 +79,7 @@ std::unique_ptr convertToSpline( return outputTrajectory; } +//============================================================================== std::unique_ptr computeParabolicTiming( const aikido::trajectory::Interpolated& _inputTrajectory, const Eigen::VectorXd& _maxVelocity, @@ -114,6 +116,43 @@ std::unique_ptr computeParabolicTiming( return outputTrajectory; } +//============================================================================== +std::unique_ptr computeParabolicTiming( + const aikido::trajectory::Spline& _inputTrajectory, + const Eigen::VectorXd& _maxVelocity, + const Eigen::VectorXd& _maxAcceleration) +{ + const auto stateSpace = _inputTrajectory.getStateSpace(); + const auto dimension = stateSpace->getDimension(); + + if (static_cast(_maxVelocity.size()) != dimension) + throw std::invalid_argument("Velocity limits have wrong dimension."); + + if (static_cast(_maxAcceleration.size()) != dimension) + throw std::invalid_argument("Acceleration limits have wrong dimension."); + + for (std::size_t i = 0; i < dimension; ++i) + { + if (_maxVelocity[i] <= 0.) + throw std::invalid_argument("Velocity limits must be positive."); + if (!std::isfinite(_maxVelocity[i])) + throw std::invalid_argument("Velocity limits must be finite."); + + if (_maxAcceleration[i] <= 0.) + throw std::invalid_argument("Acceleration limits must be positive."); + if (!std::isfinite(_maxAcceleration[i])) + throw std::invalid_argument("Acceleration limits must be finite."); + } + + double startTime = _inputTrajectory.getStartTime(); + auto dynamicPath = detail::convertToDynamicPath( + _inputTrajectory, _maxVelocity, _maxAcceleration, false); + + auto outputTrajectory + = detail::convertToSpline(*dynamicPath, startTime, stateSpace); + return outputTrajectory; +} + //============================================================================== ParabolicTimer::ParabolicTimer( const Eigen::VectorXd& _velocityLimits, diff --git a/src/planner/parabolic/ParabolicUtil.cpp b/src/planner/parabolic/ParabolicUtil.cpp index b1feab47c9..6007abf321 100644 --- a/src/planner/parabolic/ParabolicUtil.cpp +++ b/src/planner/parabolic/ParabolicUtil.cpp @@ -188,7 +188,8 @@ std::unique_ptr convertToSpline( std::unique_ptr convertToDynamicPath( const aikido::trajectory::Spline& _inputTrajectory, const Eigen::VectorXd& _maxVelocity, - const Eigen::VectorXd& _maxAcceleration) + const Eigen::VectorXd& _maxAcceleration, + bool _preserveWaypointVelocity) { const auto stateSpace = _inputTrajectory.getStateSpace(); const auto numWaypoints = _inputTrajectory.getNumWaypoints(); @@ -214,7 +215,14 @@ std::unique_ptr convertToDynamicPath( auto outputPath = make_unique(); outputPath->Init(toVector(_maxVelocity), toVector(_maxAcceleration)); - outputPath->SetMilestones(milestones, velocities); + if (_preserveWaypointVelocity) + { + outputPath->SetMilestones(milestones, velocities); + } + else + { + outputPath->SetMilestones(milestones); + } if (!outputPath->IsValid()) throw std::runtime_error("Converted DynamicPath is not valid"); return outputPath; diff --git a/src/planner/parabolic/ParabolicUtil.hpp b/src/planner/parabolic/ParabolicUtil.hpp index d466fcdeb1..178c777f69 100644 --- a/src/planner/parabolic/ParabolicUtil.hpp +++ b/src/planner/parabolic/ParabolicUtil.hpp @@ -55,16 +55,20 @@ std::unique_ptr convertToSpline( /// Convert a spline trajectory to a dynamic path /// \param _inputTrajectory a spline trajectory -/// \param[out] _startTime the start time of the spline trajectory +/// \param _maxVelocity maximum velocity in each dimension +/// \param _maxAcceleration maximum acceleration in each dimension +/// \param _preserveWaypointVelocity preserve waypoint velocity in conversion /// \return a dynamic path std::unique_ptr convertToDynamicPath(const aikido::trajectory::Spline& _inputTrajectory, const Eigen::VectorXd& _maxVelocity, - const Eigen::VectorXd& _maxAcceleration); + const Eigen::VectorXd& _maxAcceleration, + bool _preserveWaypointVelocity = true); /// Convert an interpolated trajectory to a dynamic path /// \param _inputTrajectory a spline trajectory -/// \param[out] _startTime the start time of the interploated trajectory +/// \param _maxVelocity maximum velocity in each dimension +/// \param _maxAcceleration maximum acceleration in each dimension /// \return a dynamic path std::unique_ptr convertToDynamicPath(const aikido::trajectory::Interpolated& _inputTrajectory, diff --git a/tests/planner/parabolic/test_ParabolicTimer.cpp b/tests/planner/parabolic/test_ParabolicTimer.cpp index 4b2f1b628b..9eeb88f0da 100644 --- a/tests/planner/parabolic/test_ParabolicTimer.cpp +++ b/tests/planner/parabolic/test_ParabolicTimer.cpp @@ -126,6 +126,41 @@ TEST_F(ParabolicTimerTests, StartsAtNonZeroTime) EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(state.getValue())); } +TEST_F(ParabolicTimerTests, InterploatedSplineEquivalence) +{ + Interpolated interpolated(mStateSpace, mInterpolator); + + auto state = mStateSpace->createState(); + auto state2 = mStateSpace->createState(); + + // This is the same test as StraightLine_TriangularProfile, except that the + // trajectory starts at a non-zero time. + state.setValue(Vector2d(1., 2.)); + interpolated.addWaypoint(1., state); + + state.setValue(Vector2d(2., 3.)); + interpolated.addWaypoint(3., state); + + auto spline = convertToSpline(interpolated); + + auto timedInterpolated = computeParabolicTiming( + interpolated, Vector2d::Constant(2.), Vector2d::Constant(1.)); + auto timedSpline = computeParabolicTiming( + *spline, Vector2d::Constant(2.), Vector2d::Constant(1.)); + + timedInterpolated->evaluate(1., state); + timedSpline->evaluate(1., state2); + EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + + timedInterpolated->evaluate(2., state); + timedSpline->evaluate(2., state2); + EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); + + timedInterpolated->evaluate(3., state); + timedSpline->evaluate(3., state2); + EXPECT_TRUE(state2.getValue().isApprox(state.getValue())); +} + TEST_F(ParabolicTimerTests, StraightLine_TriangularProfile) { Interpolated inputTrajectory(mStateSpace, mInterpolator);