Update ParabolicSmoother/ParabolicTimer to support Splines (#324)

Magi requires that we have post processors support both Interpolated and Spline trajectories. This is a small PR that makes the parabolic post processor classes support the new methods added in #302.
personalrobotics · Feb 8, 2018 · d99e774 · d99e774
1 parent 77d7012
commit d99e774
Show file tree

Hide file tree

Showing 8 changed files with 213 additions and 35 deletions.
diff --git a/include/aikido/planner/TrajectoryPostProcessor.hpp b/include/aikido/planner/TrajectoryPostProcessor.hpp
@@ -14,8 +14,14 @@ class TrajectoryPostProcessor
   /// \param _inputTraj The untimed trajectory for the arm to process.
   /// \param _rng Random number generator.
   virtual std::unique_ptr<aikido::trajectory::Spline> postprocess(
-      const aikido::trajectory::InterpolatedPtr& _inputTraj,
-      const aikido::common::RNG* _rng)
+      const aikido::trajectory::Interpolated& _inputTraj,
+      const aikido::common::RNG& _rng)
+      = 0;
+
+  /// \param _inputTraj The untimed *spline* trajectory for the arm to process.
+  /// \param _rng Random number generator.
+  virtual std::unique_ptr<aikido::trajectory::Spline> postprocess(
+      const trajectory::Spline& _inputTraj, const aikido::common::RNG& _rng)
       = 0;
 };
 

diff --git a/include/aikido/planner/parabolic/ParabolicSmoother.hpp b/include/aikido/planner/parabolic/ParabolicSmoother.hpp
@@ -167,10 +167,22 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor
   /// Performs parabolic smoothing on an input trajectory.
   /// \copydoc TrajectoryPostProcessor::postprocess
   std::unique_ptr<aikido::trajectory::Spline> postprocess(
-      const aikido::trajectory::InterpolatedPtr& _inputTraj,
-      const aikido::common::RNG* _rng) override;
+      const aikido::trajectory::Interpolated& _inputTraj,
+      const aikido::common::RNG& _rng) override;
+
+  /// Performs parabolic smoothing on an input *spline* trajectory.
+  /// \copydoc TrajectoryPostProcessor::postprocess
+  std::unique_ptr<aikido::trajectory::Spline> postprocess(
+      const aikido::trajectory::Spline& _inputTraj,
+      const aikido::common::RNG& _rng) override;
 
 private:
+  /// Common logic to do shortcutting and/or blending on the input trajectory
+  /// as dictated by mEnableShortcut and mEnableBlend.
+  std::unique_ptr<aikido::trajectory::Spline> handleShortcutOrBlend(
+      const aikido::trajectory::Spline& _inputTraj,
+      const aikido::common::RNG& _rng);
+
   /// Set to the value of \c _feasibilityCheckResolution.
   double mFeasibilityCheckResolution;
 

diff --git a/include/aikido/planner/parabolic/ParabolicTimer.hpp b/include/aikido/planner/parabolic/ParabolicTimer.hpp
@@ -82,8 +82,14 @@ class ParabolicTimer : public aikido::planner::TrajectoryPostProcessor
   /// Performs parabolic retiming on an input trajectory.
   /// \copydoc TrajectoryPostProcessor::postprocess
   std::unique_ptr<aikido::trajectory::Spline> postprocess(
-      const aikido::trajectory::InterpolatedPtr& _inputTraj,
-      const aikido::common::RNG* _rng) override;
+      const aikido::trajectory::Interpolated& _inputTraj,
+      const aikido::common::RNG& _rng) override;
+
+  /// Performs parabolic retiming on an input *spline* trajectory.
+  /// \copydoc TrajectoryPostProcessor::postprocess
+  std::unique_ptr<aikido::trajectory::Spline> postprocess(
+      const aikido::trajectory::Spline& _inputTraj,
+      const aikido::common::RNG& _rng) override;
 
 private:
   /// Set to the value of \c _velocityLimits.

diff --git a/src/planner/parabolic/ParabolicSmoother.cpp b/src/planner/parabolic/ParabolicSmoother.cpp
@@ -141,29 +141,52 @@ ParabolicSmoother::ParabolicSmoother(
 
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
-    const aikido::trajectory::InterpolatedPtr& _inputTraj,
-    const aikido::common::RNG* _rng)
+    const aikido::trajectory::Interpolated& _inputTraj,
+    const aikido::common::RNG& _rng)
 {
-  if (!_rng)
-    throw std::invalid_argument(
-        "Passed nullptr _rng to ParabolicSmoother::postprocess");
-  if (!_inputTraj)
-    throw std::invalid_argument(
-        "Passed nullptr _inputTraj to "
-        "ParabolicSmoother::postprocess");
+  // Get timed trajectory for arm
+  auto timedTrajectory = computeParabolicTiming(
+      _inputTraj, mVelocityLimits, mAccelerationLimits);
+
+  auto shortcutOrBlendTrajectory
+      = handleShortcutOrBlend(*timedTrajectory, _rng);
+  if (shortcutOrBlendTrajectory)
+    return shortcutOrBlendTrajectory;
+
+  return timedTrajectory;
+}
 
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
+    const aikido::trajectory::Spline& _inputTraj,
+    const aikido::common::RNG& _rng)
+{
   // Get timed trajectory for arm
   auto timedTrajectory = computeParabolicTiming(
-      *_inputTraj, mVelocityLimits, mAccelerationLimits);
+      _inputTraj, mVelocityLimits, mAccelerationLimits);
+
+  auto shortcutOrBlendTrajectory
+      = handleShortcutOrBlend(*timedTrajectory, _rng);
+  if (shortcutOrBlendTrajectory)
+    return shortcutOrBlendTrajectory;
+
+  return timedTrajectory;
+}
 
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline>
+ParabolicSmoother::handleShortcutOrBlend(
+    const aikido::trajectory::Spline& _inputTraj,
+    const aikido::common::RNG& _rng)
+{
   if (mEnableShortcut && mEnableBlend)
   {
     return doShortcutAndBlend(
-        *timedTrajectory,
+        _inputTraj,
         mCollisionTestable,
         mVelocityLimits,
         mAccelerationLimits,
-        *_rng->clone(),
+        *_rng.clone(),
         mShortcutTimelimit,
         mBlendRadius,
         mBlendIterations,
@@ -173,19 +196,19 @@ std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
   else if (mEnableShortcut)
   {
     return doShortcut(
-        *timedTrajectory,
+        _inputTraj,
         mCollisionTestable,
         mVelocityLimits,
         mAccelerationLimits,
-        *_rng->clone(),
+        *_rng.clone(),
         mShortcutTimelimit,
         mFeasibilityCheckResolution,
         mFeasibilityApproxTolerance);
   }
   else if (mEnableBlend)
   {
     return doBlend(
-        *timedTrajectory,
+        _inputTraj,
         mCollisionTestable,
         mVelocityLimits,
         mAccelerationLimits,
@@ -195,7 +218,7 @@ std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
         mFeasibilityApproxTolerance);
   }
 
-  return timedTrajectory;
+  return nullptr;
 }
 
 } // namespace parabolic

diff --git a/src/planner/parabolic/ParabolicTimer.cpp b/src/planner/parabolic/ParabolicTimer.cpp
@@ -164,16 +164,20 @@ ParabolicTimer::ParabolicTimer(
 
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> ParabolicTimer::postprocess(
-    const aikido::trajectory::InterpolatedPtr& _inputTraj,
-    const aikido::common::RNG* /*_rng*/)
+    const aikido::trajectory::Interpolated& _inputTraj,
+    const aikido::common::RNG& /*_rng*/)
 {
-  if (!_inputTraj)
-    throw std::invalid_argument(
-        "Passed nullptr _inputTraj to "
-        "ParabolicTimer::postprocess");
+  return computeParabolicTiming(
+      _inputTraj, mVelocityLimits, mAccelerationLimits);
+}
 
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline> ParabolicTimer::postprocess(
+    const aikido::trajectory::Spline& _inputTraj,
+    const aikido::common::RNG& /*_rng*/)
+{
   return computeParabolicTiming(
-      *_inputTraj, mVelocityLimits, mAccelerationLimits);
+      _inputTraj, mVelocityLimits, mAccelerationLimits);
 }
 
 } // namespace parabolic

diff --git a/tests/planner/parabolic/CMakeLists.txt b/tests/planner/parabolic/CMakeLists.txt
@@ -13,6 +13,14 @@ target_link_libraries(test_ParabolicSmoother
   "${PROJECT_NAME}_planner_parabolic"
   "${PROJECT_NAME}_statespace")
 
+aikido_add_test(test_TimePostProcessor
+  test_TimePostProcessor.cpp)
+target_link_libraries(test_TimePostProcessor
+  "${PROJECT_NAME}_constraint"
+  "${PROJECT_NAME}_trajectory"
+  "${PROJECT_NAME}_planner_parabolic"
+  "${PROJECT_NAME}_statespace")
+
 aikido_add_test(test_SmoothPostProcessor
   test_SmoothPostProcessor.cpp)
 target_link_libraries(test_SmoothPostProcessor

diff --git a/tests/planner/parabolic/test_SmoothPostProcessor.cpp b/tests/planner/parabolic/test_SmoothPostProcessor.cpp
@@ -110,9 +110,8 @@ TEST_F(SmoothPostProcessorTests, useShortcutting)
       mFeasibilityCheckResolution,
       mFeasibilityApproxTolerance);
 
-  auto clonedRNG = std::move(cloneRNGFrom(mRng)[0]);
   auto smoothedTrajectory
-      = testSmoothPostProcessor.postprocess(mTrajectory, clonedRNG.get());
+      = testSmoothPostProcessor.postprocess(*mTrajectory, mRng);
 
   // Position.
   auto state = mStateSpace->createState();
@@ -153,9 +152,8 @@ TEST_F(SmoothPostProcessorTests, useBlend)
       mFeasibilityCheckResolution,
       mFeasibilityApproxTolerance);
 
-  auto clonedRNG = std::move(cloneRNGFrom(mRng)[0]);
   auto smoothedTrajectory
-      = testSmoothPostProcessor.postprocess(mTrajectory, clonedRNG.get());
+      = testSmoothPostProcessor.postprocess(*mTrajectory, mRng);
 
   // Position.
   auto state = mStateSpace->createState();
@@ -196,9 +194,8 @@ TEST_F(SmoothPostProcessorTests, useShortcuttingAndBlend)
       mFeasibilityCheckResolution,
       mFeasibilityApproxTolerance);
 
-  auto clonedRNG = std::move(cloneRNGFrom(mRng)[0]);
   auto smoothedTrajectory
-      = testSmoothPostProcessor.postprocess(mTrajectory, clonedRNG.get());
+      = testSmoothPostProcessor.postprocess(*mTrajectory, mRng);
 
   // Position.
   auto state = mStateSpace->createState();

diff --git a/tests/planner/parabolic/test_TimePostProcessor.cpp b/tests/planner/parabolic/test_TimePostProcessor.cpp
@@ -0,0 +1,122 @@
+#include <gtest/gtest.h>
+#include <aikido/common/RNG.hpp>
+#include <aikido/planner/parabolic/ParabolicTimer.hpp>
+#include <aikido/statespace/GeodesicInterpolator.hpp>
+#include <aikido/statespace/Rn.hpp>
+#include "eigen_tests.hpp"
+
+using Eigen::Vector2d;
+using aikido::trajectory::Interpolated;
+using aikido::statespace::GeodesicInterpolator;
+using aikido::statespace::R2;
+using aikido::planner::parabolic::convertToSpline;
+using aikido::planner::parabolic::ParabolicTimer;
+
+class TimePostProcessorTests : public ::testing::Test
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+protected:
+  void SetUp() override
+  {
+    mRng = aikido::common::RNGWrapper<std::mt19937>(0);
+    mStateSpace = std::make_shared<R2>();
+    mInterpolator = std::make_shared<GeodesicInterpolator>(mStateSpace);
+  }
+
+  aikido::common::RNGWrapper<std::mt19937> mRng;
+  std::shared_ptr<R2> mStateSpace;
+  std::shared_ptr<GeodesicInterpolator> mInterpolator;
+};
+
+TEST_F(TimePostProcessorTests, testTime)
+{
+  ParabolicTimer testTimePostProcessor(
+      Vector2d::Constant(2.), Vector2d::Constant(1.));
+
+  Interpolated inputTrajectory(mStateSpace, mInterpolator);
+
+  auto state = mStateSpace->createState();
+  Eigen::VectorXd tangentVector;
+
+  // The optimal timing of this trajectory should be a triangle centered at t =
+  // 1that accelerates at 1 rad/s^2 for 1 s, then deaccelerates at -1 rad/s^2
+  // for 1 s. This corresponds to moving each axis through 2 rad.
+  state.setValue(Vector2d(1., 2.));
+  inputTrajectory.addWaypoint(0., state);
+
+  state.setValue(Vector2d(2., 3.));
+  inputTrajectory.addWaypoint(2., state);
+
+  auto timedTrajectory
+      = testTimePostProcessor.postprocess(inputTrajectory, mRng);
+
+  // TODO: Why does this return three derivatives instead of two?
+  EXPECT_GE(timedTrajectory->getNumDerivatives(), 2);
+  EXPECT_EQ(2, timedTrajectory->getNumSegments());
+  EXPECT_DOUBLE_EQ(2., timedTrajectory->getDuration());
+
+  // Position.
+  timedTrajectory->evaluate(0., state);
+  EXPECT_TRUE(Vector2d(1.0, 2.0).isApprox(state.getValue()));
+
+  timedTrajectory->evaluate(1., state);
+  EXPECT_TRUE(Vector2d(1.5, 2.5).isApprox(state.getValue()));
+
+  timedTrajectory->evaluate(2., state);
+  EXPECT_TRUE(Vector2d(2.0, 3.0).isApprox(state.getValue()));
+
+  // Velocity
+  timedTrajectory->evaluateDerivative(0.5, 1, tangentVector);
+  EXPECT_TRUE(Vector2d(0.5, 0.5).isApprox(tangentVector));
+
+  timedTrajectory->evaluateDerivative(1.0, 1, tangentVector);
+  EXPECT_TRUE(Vector2d(1.0, 1.0).isApprox(tangentVector));
+
+  timedTrajectory->evaluateDerivative(1.5, 1, tangentVector);
+  EXPECT_TRUE(Vector2d(0.5, 0.5).isApprox(tangentVector));
+
+  // Acceleration.
+  timedTrajectory->evaluateDerivative(0.5, 2, tangentVector);
+  EXPECT_TRUE(Vector2d(1., 1.).isApprox(tangentVector));
+
+  timedTrajectory->evaluateDerivative(1.5, 2, tangentVector);
+  EXPECT_TRUE(Vector2d(-1., -1.).isApprox(tangentVector));
+}
+
+TEST_F(TimePostProcessorTests, testSplineTiming)
+{
+  ParabolicTimer testTimePostProcessor(
+      Vector2d::Constant(2.), Vector2d::Constant(1.));
+
+  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
+      = testTimePostProcessor.postprocess(interpolated, mRng);
+  auto timedSpline = testTimePostProcessor.postprocess(*spline, mRng);
+
+  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()));
+}