Refactor planner API (#314)

* Add Planner class and associated APIs

* Fix code format

* Pass function argument as const reference

* Rename planning problems

* Revise snap planner

* Apply snap planner to util

* Add PlannerFor[Single/Multi]Problem

* Remove getPlannableProblems()

* Fix const-correctness of CompositePlanner

* Set state space when a planner is created

Move interpolator from problem to planner

* Rename Problem's getName() to getType()

* Fix grammar errors

* Use unordered_set for goal states

* Remove interpolator from problem

* Code format

* Address comments that are straightforward

* Add missing explicit specifier

* Use more descriptive parameter name

* Add utility functions for goal states

* Minor updates

* Remove PlannerForMultiProblem

rename PlannerForSingleProblem to SingleProblemPlanner

* Remove setters from problem

All the problem classes intentionally don't have setters. This is because we assume creating a new problem for different problem settings makes sense. We can revisit this design decision if we find a compelling reason to have setters.

* Rename TheProblem to SolverblaProblem

* Take problem as const reference

* Remove CompositePlanner::addPlanner()

* Move constraint to Problem

* Add missing [in]

* Fix util according to the API changes

* Address a few TODOs

* Remove constraint from concrete problems

* Code format

* Fix typo: SolverbleProblem --> SolvableProblem

* Be further strict to const-correctness

* Add type checking assertions

* Add docstrings

* Update changelog

* Address Brian's comments (needs code format)

* Format code

* Address Gilwoo's feedback

* Update header guard name

* Throw exception for zero-sized direction vector

* Use \throw instead of \throws

* Pass planner by const reference to check existency

* Fix compilation errors per Clang 9.1.0

Author: jslee02

CHANGELOG.md

@@ -4,9 +4,9 @@
 
 * Common
 
-  * Cleaned up doxygen errors: [#357](https://github.com/personalrobotics/aikido/pull/357)
   * Fixed bug in StepSequence::getMaxSteps(): [#305](https://github.com/personalrobotics/aikido/pull/305)
   * Fixed bug in StepSequence iterator: [#320](https://github.com/personalrobotics/aikido/pull/320)
+  * Cleaned up doxygen errors: [#357](https://github.com/personalrobotics/aikido/pull/357)
 
 * Constraint
 
@@ -31,10 +31,11 @@
 
 * Planner
 
-  * Changed interface for TrajectoryPostProcessor: [#341](https://github.com/personalrobotics/aikido/pull/341)
   * Added parabolic timing for linear spline [#302](https://github.com/personalrobotics/aikido/pull/302), [#324](https://github.com/personalrobotics/aikido/pull/324)
   * Fixed step sequence iteration in VPF: [#303](https://github.com/personalrobotics/aikido/pull/303)
+  * Refactored planning API: [#314](https://github.com/personalrobotics/aikido/pull/314)
   * Added flags to WorldStateSaver to specify what to save: [#339](https://github.com/personalrobotics/aikido/pull/339)
+  * Changed interface for TrajectoryPostProcessor: [#341](https://github.com/personalrobotics/aikido/pull/341)
   * Planning calls with InverseKinematicsSampleable constraints explicitly set MetaSkeleton to solve IK with: [#379](https://github.com/personalrobotics/aikido/pull/379)
 
 * Robot

include/aikido/planner/CompositePlanner.hpp

@@ -0,0 +1,47 @@
+#ifndef AIKIDO_PLANNER_COMPOSITEPLANNER_HPP_
+#define AIKIDO_PLANNER_COMPOSITEPLANNER_HPP_
+
+#include <vector>
+
+#include "aikido/planner/Planner.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+
+namespace aikido {
+namespace planner {
+
+AIKIDO_DECLARE_POINTERS(CompositePlanner)
+
+/// CompositePlanner is a base class for concrete planner classes that contain
+/// multiple planners.
+class CompositePlanner : public Planner
+{
+public:
+  /// Constructs given list of planners.
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  /// \param[in] planners Planners that this CompositePlanner will contain.
+  /// \throw If any of \c planners are null.
+  CompositePlanner(
+      statespace::ConstStateSpacePtr stateSpace,
+      const std::vector<PlannerPtr>& planners = std::vector<PlannerPtr>());
+
+  /// Returns true if this CompositePlanner contains \c planner.
+  bool hasPlanner(const Planner& planner) const;
+
+  // Documentation inherited.
+  bool canSolve(const Problem& problem) const override;
+
+protected:
+  /// Planners.
+  const std::vector<PlannerPtr> mPlanners;
+  // We use std::vector to keep the order of planners. This is because some
+  // concrete planners rely on the order. Alternatively, we could remove
+  // mPlanners from here and let the concrete classes maintain the planner
+  // containers accordingly. In that case, we should also remove getPlanner and
+  // make addPlanner/hasPlanner/getPlanners as pure virtual functions.
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_COMPOSITEPLANNER_HPP_

include/aikido/planner/ConfigurationToConfiguration.hpp

@@ -0,0 +1,53 @@
+#ifndef AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATION_HPP_
+#define AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATION_HPP_
+
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/Problem.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Planning problem to plan to a single goal configuration.
+class ConfigurationToConfiguration : public Problem
+{
+public:
+  /// Constructor.
+  ///
+  /// \param[in] stateSpace State space that this problem associated with.
+  /// \param[in] startState Start state.
+  /// \param[in] goalState Goal state.
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  /// \throw If \c stateSpace is not compatible with \c constraint's state
+  /// space.
+  ConfigurationToConfiguration(
+      statespace::ConstStateSpacePtr stateSpace,
+      const statespace::StateSpace::State* startState,
+      const statespace::StateSpace::State* goalState,
+      constraint::ConstTestablePtr constraint);
+
+  // Documentation inherited.
+  const std::string& getType() const override;
+
+  /// Returns the type of the planning problem.
+  static const std::string& getStaticType();
+
+  /// Returns the start state.
+  const statespace::StateSpace::State* getStartState() const;
+
+  /// Returns the goal state.
+  const statespace::StateSpace::State* getGoalState() const;
+
+protected:
+  /// Start state.
+  const statespace::StateSpace::State* mStartState;
+
+  /// Goal state.
+  const statespace::StateSpace::State* mGoalState;
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATION_HPP_

include/aikido/planner/ConfigurationToConfigurationPlanner.hpp

@@ -0,0 +1,42 @@
+#ifndef AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
+#define AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
+
+#include "aikido/planner/ConfigurationToConfiguration.hpp"
+#include "aikido/planner/SingleProblemPlanner.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Base planner class for ConfigurationToConfiguration planning problem.
+class ConfigurationToConfigurationPlanner
+    : public SingleProblemPlanner<ConfigurationToConfigurationPlanner,
+                                  ConfigurationToConfiguration>
+{
+public:
+  // Expose the implementation of Planner::plan(const Problem&, Result*) in
+  // SingleProblemPlanner. Note that plan() of the base class takes Problem
+  // while the virtual function defined in this class takes SolverbleProblem,
+  // which is simply ConfigurationToConfiguration.
+  using SingleProblemPlanner::plan;
+
+  /// Constructor
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  explicit ConfigurationToConfigurationPlanner(
+      statespace::ConstStateSpacePtr stateSpace);
+
+  /// Solves \c problem returning the result to \c result.
+  ///
+  /// \param[in] problem Planning problem to be solved by the planner.
+  /// \param[out] result Result of planning procedure.
+  virtual trajectory::TrajectoryPtr plan(
+      const SolvableProblem& problem, Result* result = nullptr)
+      = 0;
+  // Note: SolvableProblem is defined in SingleProblemPlanner.
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

include/aikido/planner/ConfigurationToConfigurations.hpp

@@ -0,0 +1,62 @@
+#ifndef AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONS_HPP_
+#define AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONS_HPP_
+
+#include <unordered_set>
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/Problem.hpp"
+#include "aikido/statespace/Interpolator.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Planning problem to plan to multiple goal configurations.
+///
+/// Plan a trajectory from start state to any of the goal states using an
+/// interpolator to interpolate between the states.
+class ConfigurationToConfigurations : public Problem
+{
+public:
+  using GoalStates = std::unordered_set<const statespace::StateSpace::State*>;
+
+  /// Constructor.
+  ///
+  /// \param[in] stateSpace State space.
+  /// \param[in] startState Start state.
+  /// \param[in] goalStates Goal states.
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  /// \throw If \c stateSpace is not compatible with \c constraint's state
+  /// space.
+  ConfigurationToConfigurations(
+      statespace::ConstStateSpacePtr stateSpace,
+      const statespace::StateSpace::State* startState,
+      const GoalStates& goalStates,
+      constraint::ConstTestablePtr constraint);
+
+  // Documentation inherited.
+  const std::string& getType() const override;
+
+  /// Returns the type of the planning problem.
+  static const std::string& getStaticType();
+
+  /// Returns the start state.
+  const statespace::StateSpace::State* getStartState() const;
+
+  /// Returns the number of the goal states.
+  std::size_t getNumGoalStates() const;
+
+  /// Returns goal states.
+  const GoalStates& getGoalStates() const;
+
+protected:
+  /// Start state.
+  const statespace::StateSpace::State* mStartState;
+
+  /// Goal States.
+  const GoalStates mGoalStates;
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_CONFIGURATIONTOCONFIGURATIONS_HPP_

include/aikido/planner/ConfigurationToEndEffectorOffset.hpp

@@ -0,0 +1,77 @@
+#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
+#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
+
+#include <dart/dart.hpp>
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/Problem.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Planning problem to plan to desired end-effector offset while maintaining
+/// the current end-effector orientation.
+class ConfigurationToEndEffectorOffset : public Problem
+{
+public:
+  /// Constructor.
+  ///
+  /// \param[in] stateSpace State space.
+  /// \param[in] endEffectorBodyNode BodyNode to be planned to move to a desired
+  /// offest while maintaining the current orientation.
+  /// \param[in] startState Start state.
+  /// \param[in] direction Unit vector that represents the direction of motion
+  /// [unit vector in the world frame].
+  /// \param[in] signedDistance Signed distance to move, in meters.
+  /// \param[in] interpolator Interpolator used to produce the output
+  /// trajectory.
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  /// \throw If the size of \c direction is zero.
+  ConfigurationToEndEffectorOffset(
+      statespace::ConstStateSpacePtr stateSpace,
+      dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      const statespace::StateSpace::State* startState,
+      const Eigen::Vector3d& direction,
+      double signedDistance,
+      constraint::ConstTestablePtr constraint);
+
+  // Documentation inherited.
+  const std::string& getType() const override;
+
+  /// Returns the type of the planning problem.
+  static const std::string& getStaticType();
+
+  /// Returns the end-effector BodyNode to be planned to move a desired offest
+  /// while maintaining the current orientation.
+  dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
+
+  /// Returns the start state.
+  const statespace::StateSpace::State* getStartState() const;
+
+  /// Returns the direction of motion specified in the world frame.
+  ///
+  /// \note The direction is allowed to be zero vector.
+  const Eigen::Vector3d& getDirection() const;
+
+  /// Returns the signed distance in meters to move in the specified direction.
+  double getDistance() const;
+
+protected:
+  /// End-effector body node.
+  const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
+
+  /// Start state.
+  const statespace::StateSpace::State* mStartState;
+
+  /// Direction of motion.
+  const Eigen::Vector3d mDirection;
+
+  /// Signed distance to move in the direction specified.
+  const double mDistance;
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_

include/aikido/planner/ConfigurationToEndEffectorPose.hpp

@@ -0,0 +1,64 @@
+#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
+#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
+
+#include <dart/dart.hpp>
+#include "aikido/planner/Problem.hpp"
+#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/trajectory/Interpolated.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Planning problem to plan to a desired end-effector pose.
+class ConfigurationToEndEffectorPose : public Problem
+{
+public:
+  /// Constructor.
+  ///
+  /// \param[in] stateSpace State space.
+  /// \param[in] endEffectorBodyNode BodyNode to be planned to move to a desired
+  /// pose.
+  /// \param[in] startState Start state.
+  /// \param[in] goalPose Goal pose.
+  /// \throw If \c stateSpace is not compatible with \c constraint's state
+  /// space.
+  ConfigurationToEndEffectorPose(
+      statespace::ConstStateSpacePtr stateSpace,
+      dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      const statespace::StateSpace::State* startState,
+      const Eigen::Isometry3d& goalPose,
+      constraint::ConstTestablePtr constraint);
+
+  // Documentation inherited.
+  const std::string& getType() const override;
+
+  /// Returns the type of the planning problem.
+  static const std::string& getStaticType();
+
+  /// Returns the end-effector BodyNode to be planned to move to a desired pose.
+  dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
+
+  /// Returns the start state.
+  const statespace::StateSpace::State* getStartState() const;
+
+  /// Returns the goal pose.
+  const Eigen::Isometry3d& getGoalPose() const;
+
+protected:
+  // Need this due to mGoalPose.
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  /// End-effector body node.
+  const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
+
+  /// Start state.
+  const statespace::StateSpace::State* mStartState;
+
+  /// Goal pose.
+  const Eigen::Isometry3d mGoalPose;
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTORPOSE_HPP_