Add ParabolicTiming for linear spline (#302)

* add timing for spline trajectories

* fix code style

* add test case

* add docstring

* update change log

* fix CHANGELOG

* address Brian's comments

* fix bug

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

include/aikido/planner/parabolic/ParabolicTimer.hpp

@@ -35,6 +35,31 @@ std::unique_ptr<aikido::trajectory::Spline> 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<aikido::trajectory::Spline> 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.

src/planner/parabolic/ParabolicTimer.cpp

@@ -18,6 +18,7 @@ namespace aikido {
 namespace planner {
 namespace parabolic {
 
+//==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
     const aikido::trajectory::Interpolated& _inputTrajectory)
 {
@@ -78,6 +79,7 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
   return outputTrajectory;
 }
 
+//==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> computeParabolicTiming(
     const aikido::trajectory::Interpolated& _inputTrajectory,
     const Eigen::VectorXd& _maxVelocity,
@@ -114,6 +116,43 @@ std::unique_ptr<aikido::trajectory::Spline> computeParabolicTiming(
   return outputTrajectory;
 }
 
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline> 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<std::size_t>(_maxVelocity.size()) != dimension)
+    throw std::invalid_argument("Velocity limits have wrong dimension.");
+
+  if (static_cast<std::size_t>(_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,

src/planner/parabolic/ParabolicUtil.cpp

@@ -188,7 +188,8 @@ std::unique_ptr<aikido::trajectory::Spline> convertToSpline(
 std::unique_ptr<ParabolicRamp::DynamicPath> 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<ParabolicRamp::DynamicPath> convertToDynamicPath(
 
   auto outputPath = make_unique<ParabolicRamp::DynamicPath>();
   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;

src/planner/parabolic/ParabolicUtil.hpp

@@ -55,16 +55,20 @@ std::unique_ptr<aikido::trajectory::Spline> 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<ParabolicRamp::DynamicPath>
     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<ParabolicRamp::DynamicPath>
     convertToDynamicPath(const aikido::trajectory::Interpolated& _inputTrajectory,

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);