Add retimePathWithKunz to Robot (#505)

* Add retimePathWithKunz to Robot

* Clang format

* Update CHANGELOG.md

* Update CMakeLists

CHANGELOG.md

@@ -67,6 +67,7 @@
 * Robot
 
   * Added Robot, Manipulator, Hand interfaces, and ConcreteRobot, ConcreteManipulator classes: [#325](https://github.com/personalrobotics/aikido/pull/325), [#392](https://github.com/personalrobotics/aikido/pull/392)
+  * Added Kunz timer to Robot class: [#505](https://github.com/personalrobotics/aikido/pull/505)
 
 * RViz
 

include/aikido/robot/ConcreteManipulator.hpp

@@ -42,6 +42,13 @@ class ConcreteManipulator : public Manipulator
       const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
       const aikido::trajectory::Trajectory* path) override;
 
+  // Documentation inherited.
+  virtual std::unique_ptr<aikido::trajectory::Spline> retimePathWithKunz(
+      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+      const aikido::trajectory::Trajectory* path,
+      double maxDeviation,
+      double timestep) override;
+
   // Documentation inherited.
   virtual std::future<void> executeTrajectory(
       const trajectory::TrajectoryPtr& trajectory) const override;

include/aikido/robot/ConcreteRobot.hpp

@@ -62,6 +62,13 @@ class ConcreteRobot : public Robot
       const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
       const aikido::trajectory::Trajectory* path) override;
 
+  // Documentation inherited.
+  virtual std::unique_ptr<aikido::trajectory::Spline> retimePathWithKunz(
+      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+      const aikido::trajectory::Trajectory* path,
+      double maxDeviation,
+      double timestep) override;
+
   // Documentation inherited.
   virtual std::future<void> executeTrajectory(
       const trajectory::TrajectoryPtr& trajectory) const override;

include/aikido/robot/Robot.hpp

@@ -45,6 +45,18 @@ class Robot
       const aikido::trajectory::Trajectory* path)
       = 0;
 
+  /// Returns a timed trajectory computed with KunzRetimer
+  /// \param[in] metaSkeleton Metaskeleton of the path.
+  /// \param[in] path Geometric path to execute.
+  /// \param[in] maxDeviation Maximum deviation allowed from original path.
+  /// \param[in] timestep Time step between trajectory points.
+  virtual std::unique_ptr<aikido::trajectory::Spline> retimePathWithKunz(
+      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+      const aikido::trajectory::Trajectory* path,
+      double maxDeviation,
+      double timestep)
+      = 0;
+
   /// Executes a trajectory
   /// \param[in] trajectory Timed trajectory to execute
   virtual std::future<void> executeTrajectory(

src/robot/CMakeLists.txt

@@ -32,6 +32,7 @@ target_link_libraries("${PROJECT_NAME}_robot"
     "${PROJECT_NAME}_planner"
     "${PROJECT_NAME}_planner_ompl"
     "${PROJECT_NAME}_planner_parabolic"
+    "${PROJECT_NAME}_planner_kunzretimer"
     "${PROJECT_NAME}_planner_vectorfield"
     "${PROJECT_NAME}_constraint"
     "${PROJECT_NAME}_distance"

src/robot/ConcreteManipulator.cpp

@@ -35,6 +35,17 @@ std::unique_ptr<aikido::trajectory::Spline> ConcreteManipulator::retimePath(
   return mRobot->retimePath(metaSkeleton, path);
 }
 
+//==============================================================================
+std::unique_ptr<aikido::trajectory::Spline>
+ConcreteManipulator::retimePathWithKunz(
+    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+    const aikido::trajectory::Trajectory* path,
+    double maxDeviation,
+    double timestep)
+{
+  return mRobot->retimePathWithKunz(metaSkeleton, path, maxDeviation, timestep);
+}
+
 //==============================================================================
 std::future<void> ConcreteManipulator::executeTrajectory(
     const trajectory::TrajectoryPtr& trajectory) const

src/robot/ConcreteRobot.cpp

@@ -1,5 +1,6 @@
 #include "aikido/robot/ConcreteRobot.hpp"
 #include "aikido/constraint/TestableIntersection.hpp"
+#include "aikido/planner/kunzretimer/KunzRetimer.hpp"
 #include "aikido/robot/util.hpp"
 #include "aikido/statespace/StateSpace.hpp"
 
@@ -11,6 +12,7 @@ using constraint::dart::TSRPtr;
 using constraint::TestablePtr;
 using constraint::ConstTestablePtr;
 using planner::TrajectoryPostProcessor;
+using planner::kunzretimer::KunzRetimer;
 using planner::parabolic::ParabolicSmoother;
 using planner::parabolic::ParabolicTimer;
 using statespace::dart::MetaSkeletonStateSpace;
@@ -161,6 +163,29 @@ UniqueSplinePtr ConcreteRobot::retimePath(
   throw std::invalid_argument("Path should be either Spline or Interpolated.");
 }
 
+//==============================================================================
+UniqueSplinePtr ConcreteRobot::retimePathWithKunz(
+    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+    const aikido::trajectory::Trajectory* path,
+    double maxDeviation,
+    double timestep)
+{
+  Eigen::VectorXd velocityLimits = getVelocityLimits(*metaSkeleton);
+  Eigen::VectorXd accelerationLimits = getAccelerationLimits(*metaSkeleton);
+  auto retimer = std::make_shared<KunzRetimer>(
+      velocityLimits, accelerationLimits, maxDeviation, timestep);
+
+  auto interpolated = dynamic_cast<const Interpolated*>(path);
+  if (interpolated)
+    return retimer->postprocess(*interpolated, *(cloneRNG().get()));
+
+  auto spline = dynamic_cast<const Spline*>(path);
+  if (spline)
+    return retimer->postprocess(*spline, *(cloneRNG().get()));
+
+  throw std::invalid_argument("Path should be either Spline or Interpolated.");
+}
+
 //==============================================================================
 std::future<void> ConcreteRobot::executeTrajectory(
     const TrajectoryPtr& trajectory) const