Move post-processor params into AIKIDO.

CHANGELOG.md

@@ -2,6 +2,10 @@
 
 ### 0.4.0 (2020-XX-XX)
 
+* Planner
+
+  * Defined post-processor parameter structs in AIKIDO: [#579](https://github.com/personalrobotics/aikido/pull/579)
+
 ### 0.3.0 (2020-05-22)
 
 * Common

include/aikido/planner.hpp

@@ -1,4 +1,5 @@
 #include "aikido/planner/PlanningResult.hpp"
+#include "aikido/planner/PostProcessorParams.hpp"
 #include "aikido/planner/SnapPlanner.hpp"
 #include "aikido/planner/TrajectoryPostProcessor.hpp"
 #include "aikido/planner/World.hpp"

include/aikido/planner/kunzretimer/KunzRetimer.hpp

@@ -11,6 +11,9 @@ namespace aikido {
 namespace planner {
 namespace kunzretimer {
 
+constexpr double DEFAULT_MAX_DEVIATION = 1e-2;
+constexpr double DEFAULT_TIME_STEP = 0.1;
+
 /// Computes the time-optimal timing of a trajectory consisting of a sequence
 /// Geodesic interpolations between states under velocity and acceleration
 /// bounds. The output is a parabolic spline, encoded in cubic polynomials.
@@ -36,23 +39,42 @@ std::unique_ptr<aikido::trajectory::Spline> computeKunzTiming(
     const aikido::trajectory::Interpolated& inputTrajectory,
     const Eigen::VectorXd& maxVelocity,
     const Eigen::VectorXd& maxAcceleration,
-    double maxDeviation = 1e-2,
-    double timeStep = 0.1);
+    double maxDeviation = DEFAULT_MAX_DEVIATION,
+    double timeStep = DEFAULT_TIME_STEP);
 
 /// Class for performing time-optimal trajectory retiming following subject to
 /// velocity and acceleration limits.
 class KunzRetimer : public aikido::planner::TrajectoryPostProcessor
 {
 public:
+  /// Kunz postprocessor parameters used with the main `PostProcessorParams`
+  /// struct.
+  struct Params
+  {
+    /// Maximum deviation in circular blending (in configuration space).
+    double mMaxDeviation = DEFAULT_MAX_DEVIATION;
+    /// Time step in following the path (in seconds).
+    double mTimeStep = DEFAULT_TIME_STEP;
+  };
+
+  /// \param[in] velocityLimits Maximum velocity for each dimension.
+  /// \param[in] accelerationLimits Maximum acceleration for each dimension.
+  /// \param[in] maxDeviation Maximum deviation in circular blending (in
+  /// configuration space).
+  /// \param[in] timeStep Time step in following the path (in seconds).
+  KunzRetimer(
+      const Eigen::VectorXd& velocityLimits,
+      const Eigen::VectorXd& accelerationLimits,
+      double maxDeviation = DEFAULT_MAX_DEVIATION,
+      double timeStep = DEFAULT_TIME_STEP);
+
   /// \param[in] velocityLimits Maximum velocity for each dimension.
   /// \param[in] accelerationLimits Maximum acceleration for each dimension.
-  /// \param[in] maxDeviation Maximum deviation in circular blending
-  /// \param[in] timeStep Time step in following the path
+  /// \param[in] params Postprocessor parameters.
   KunzRetimer(
       const Eigen::VectorXd& velocityLimits,
       const Eigen::VectorXd& accelerationLimits,
-      double maxDeviation,
-      double timeStep);
+      const Params& params);
 
   /// Performs parabolic retiming on an input trajectory.
   /// \copydoc TrajectoryPostProcessor::postprocess

include/aikido/planner/parabolic/ParabolicSmoother.hpp

@@ -11,7 +11,7 @@ namespace aikido {
 namespace planner {
 namespace parabolic {
 
-constexpr double DEFAULT_TIMELIMT = 3.0;
+constexpr double DEFAULT_TIMELIMIT = 3.0;
 constexpr double DEFAULT_BLEND_RADIUS = 0.5;
 constexpr int DEFAULT_BLEND_ITERATIONS = 4;
 constexpr double DEFAULT_CHECK_RESOLUTION = 1e-4;
@@ -50,7 +50,7 @@ std::unique_ptr<trajectory::Spline> doShortcut(
     const Eigen::VectorXd& _maxVelocity,
     const Eigen::VectorXd& _maxAcceleration,
     aikido::common::RNG& _rng,
-    double _timelimit = DEFAULT_TIMELIMT,
+    double _timelimit = DEFAULT_TIMELIMIT,
     double _checkResolution = DEFAULT_CHECK_RESOLUTION,
     double _tolerance = DEFAULT_TOLERANCE);
 
@@ -124,7 +124,7 @@ std::unique_ptr<trajectory::Spline> doShortcutAndBlend(
     const Eigen::VectorXd& _maxVelocity,
     const Eigen::VectorXd& _maxAcceleration,
     aikido::common::RNG& _rng,
-    double _timelimit = DEFAULT_TIMELIMT,
+    double _timelimit = DEFAULT_TIMELIMIT,
     double _blendRadius = DEFAULT_BLEND_RADIUS,
     int _blendIterations = DEFAULT_BLEND_ITERATIONS,
     double _checkResolution = DEFAULT_CHECK_RESOLUTION,
@@ -134,6 +134,19 @@ std::unique_ptr<trajectory::Spline> doShortcutAndBlend(
 class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor
 {
 public:
+  /// Hauser postprocessor parameters used with the main `PostProcessorParams`
+  /// struct.
+  struct Params
+  {
+    bool mEnableShortcut = true;
+    bool mEnableBlend = true;
+    double mShortcutTimelimit = DEFAULT_TIMELIMIT;
+    double mBlendRadius = DEFAULT_BLEND_RADIUS;
+    int mBlendIterations = DEFAULT_BLEND_ITERATIONS;
+    double mFeasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION;
+    double mFeasibilityApproxTolerance = DEFAULT_TOLERANCE;
+  };
+
   /// \param _velocityLimits Maximum velocity for each dimension.
   /// \param _accelerationLimits Maximum acceleration for each dimension.
   /// \param _enableShortcut Whether shortcutting is used in smoothing.
@@ -156,12 +169,20 @@ class ParabolicSmoother : public aikido::planner::TrajectoryPostProcessor
       const Eigen::VectorXd& _accelerationLimits,
       bool _enableShortcut = true,
       bool _enableBlend = true,
-      double _shortcutTimelimit = DEFAULT_TIMELIMT,
+      double _shortcutTimelimit = DEFAULT_TIMELIMIT,
       double _blendRadius = DEFAULT_BLEND_RADIUS,
       int _blendIterations = DEFAULT_BLEND_ITERATIONS,
       double _feasibilityCheckResolution = DEFAULT_CHECK_RESOLUTION,
       double _feasibilityApproxTolerance = DEFAULT_TOLERANCE);
 
+  /// \param _velocityLimits Maximum velocity for each dimension.
+  /// \param _accelerationLimits Maximum acceleration for each dimension.
+  /// \param _params Postprocessor parameters.
+  ParabolicSmoother(
+      const Eigen::VectorXd& _velocityLimits,
+      const Eigen::VectorXd& _accelerationLimits,
+      const Params& _params);
+
   /// Performs parabolic smoothing on an input trajectory.
   /// \param _inputTraj The untimed trajectory for the arm to process.
   /// \param _rng Random number generator.

include/aikido/robot/ConcreteRobot.hpp

@@ -126,6 +126,49 @@ class ConcreteRobot : public Robot
       double feasibilityCheckResolution,
       double feasibilityApproxTolerance) const;
 
+  /// Get a postprocessor that respects velocity and acceleration limits. The
+  /// specific postprocessor returned is controlled by `postProcessorParams`.
+  /// \param[in] velocityLimits Maximum velocity for each dimension.
+  /// \param[in] accelerationLimits Maximum acceleration for each dimension.
+  /// \param[in] postProcessorParams Postprocessor parameters.
+  /// \tparam PostProcessor The trajectory postprocessor to use.
+  template <typename PostProcessor>
+  std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
+  getTrajectoryPostProcessor(
+      const Eigen::VectorXd& velocityLimits,
+      const Eigen::VectorXd& accelerationLimits,
+      const typename PostProcessor::Params& postProcessorParams) const;
+
+  /// Returns a post-processed trajectory that can be executed by the robot.
+  /// \param[in] metaSkeleton Metaskeleton of the path.
+  /// \param[in] path Geometric path to execute.
+  /// \param[in] constraint Must be satisfied after postprocessing. Typically
+  /// collision constraint is passed.
+  /// \param[in] postProcessorParams Postprocessor parameters.
+  /// \tparam PostProcessor The trajectory postprocessor to use.
+  template <typename PostProcessor>
+  aikido::trajectory::UniqueSplinePtr postProcessPath(
+      const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+      const aikido::trajectory::Trajectory* path,
+      const constraint::TestablePtr& constraint,
+      const typename PostProcessor::Params& postProcessorParams);
+
+  /// Returns a post-processed trajectory that can be executed by the robot.
+  /// \param[in] velocityLimits Maximum velocity for each dimension.
+  /// \param[in] accelerationLimits Maximum acceleration for each dimension.
+  /// \param[in] path Geometric path to execute.
+  /// \param[in] constraint Must be satisfied after postprocessing. Typically
+  /// collision constraint is passed.
+  /// \param[in] postProcessorParams Postprocessor parameters.
+  /// \tparam PostProcessor The trajectory postprocessor to use.
+  template <typename PostProcessor>
+  aikido::trajectory::UniqueSplinePtr postProcessPath(
+      const Eigen::VectorXd& velocityLimits,
+      const Eigen::VectorXd& accelerationLimits,
+      const aikido::trajectory::Trajectory* path,
+      const constraint::TestablePtr& constraint,
+      const typename PostProcessor::Params& postProcessorParams);
+
   /// TODO: Replace this with Problem interface.
   /// Plan the robot to a specific configuration. Restores the robot to its
   /// initial configuration after planning.
@@ -310,4 +353,6 @@ class ConcreteRobot : public Robot
 } // namespace robot
 } // namespace aikido
 
+#include "detail/ConcreteRobot-impl.hpp"
+
 #endif // AIKIDO_ROBOT_CONCRETEROBOT_HPP_

include/aikido/robot/detail/ConcreteRobot-impl.hpp

@@ -0,0 +1,67 @@
+namespace aikido {
+namespace robot {
+
+//==============================================================================
+template <typename PostProcessor>
+std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
+ConcreteRobot::getTrajectoryPostProcessor(
+    const Eigen::VectorXd& velocityLimits,
+    const Eigen::VectorXd& accelerationLimits,
+    const typename PostProcessor::Params& postProcessorParams) const
+{
+  static_assert(
+      std::is_base_of<aikido::planner::TrajectoryPostProcessor, PostProcessor>::
+          value,
+      "PostProcessor must derive from "
+      "aikido::planner::TrajectoryPostProcessor");
+
+  return std::make_shared<PostProcessor>(
+      velocityLimits, accelerationLimits, postProcessorParams);
+}
+
+//==============================================================================
+template <typename PostProcessor>
+aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath(
+    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
+    const aikido::trajectory::Trajectory* path,
+    const constraint::TestablePtr& constraint,
+    const typename PostProcessor::Params& postProcessorParams)
+{
+  return postProcessPath(
+      getVelocityLimits(*metaSkeleton),
+      getAccelerationLimits(*metaSkeleton),
+      path,
+      constraint,
+      postProcessorParams);
+}
+
+//==============================================================================
+template <typename PostProcessor>
+aikido::trajectory::UniqueSplinePtr ConcreteRobot::postProcessPath(
+    const Eigen::VectorXd& velocityLimits,
+    const Eigen::VectorXd& accelerationLimits,
+    const aikido::trajectory::Trajectory* path,
+    const constraint::TestablePtr& constraint,
+    const typename PostProcessor::Params& postProcessorParams)
+{
+  auto postProcessor = getTrajectoryPostProcessor(
+      velocityLimits, accelerationLimits, postProcessorParams);
+
+  auto interpolated
+      = dynamic_cast<const aikido::trajectory::Interpolated*>(path);
+  if (interpolated)
+    return postProcessor->postprocess(
+        *interpolated, *(cloneRNG().get()), constraint);
+
+  auto spline = dynamic_cast<const aikido::trajectory::Spline*>(path);
+  if (spline)
+    return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint);
+
+  std::cerr
+      << "[postProcessPath]: Path should be either Spline or Interpolated."
+      << std::endl;
+  return nullptr;
+}
+
+} // namespace robot
+} // namespace aikido

src/planner/kunzretimer/KunzRetimer.cpp

@@ -160,6 +160,19 @@ KunzRetimer::KunzRetimer(
   // Do nothing
 }
 
+//==============================================================================
+KunzRetimer::KunzRetimer(
+    const Eigen::VectorXd& velocityLimits,
+    const Eigen::VectorXd& accelerationLimits,
+    const Params& params)
+  : mVelocityLimits{velocityLimits}
+  , mAccelerationLimits{accelerationLimits}
+  , mMaxDeviation(params.mMaxDeviation)
+  , mTimeStep(params.mTimeStep)
+{
+  // Do nothing
+}
+
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> KunzRetimer::postprocess(
     const aikido::trajectory::Interpolated& inputTraj,

src/planner/parabolic/ParabolicSmoother.cpp

@@ -139,6 +139,24 @@ ParabolicSmoother::ParabolicSmoother(
   // Do nothing
 }
 
+//==============================================================================
+ParabolicSmoother::ParabolicSmoother(
+    const Eigen::VectorXd& _velocityLimits,
+    const Eigen::VectorXd& _accelerationLimits,
+    const Params& _params)
+  : mFeasibilityCheckResolution{_params.mFeasibilityCheckResolution}
+  , mFeasibilityApproxTolerance{_params.mFeasibilityApproxTolerance}
+  , mVelocityLimits{_velocityLimits}
+  , mAccelerationLimits{_accelerationLimits}
+  , mEnableShortcut{_params.mEnableShortcut}
+  , mEnableBlend{_params.mEnableBlend}
+  , mShortcutTimelimit{_params.mShortcutTimelimit}
+  , mBlendRadius{_params.mBlendRadius}
+  , mBlendIterations{_params.mBlendIterations}
+{
+  // Do nothing
+}
+
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> ParabolicSmoother::postprocess(
     const aikido::trajectory::Interpolated& _inputTraj,