Planning adapters

include/aikido/constraint/dart/InverseKinematicsSampleable.hpp

@@ -31,7 +31,7 @@ class InverseKinematicsSampleable : public Sampleable
   /// \param _maxNumTrials Max number of trials for its sample generator
   ///        to retry sampling and finding an inverse kinematics solution.
   InverseKinematicsSampleable(
-      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
+      statespace::dart::ConstMetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
       ::dart::dynamics::MetaSkeletonPtr _metaskeleton,
       SampleablePtr _poseConstraint,
       SampleablePtr _seedConstraint,

include/aikido/planner/ConfigurationToConfigurationPlanner.hpp

@@ -16,7 +16,7 @@ class ConfigurationToConfigurationPlanner
 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,
+  // while the virtual function defined in this class takes SolvableProblem,
   // which is simply ConfigurationToConfiguration.
   using SingleProblemPlanner::plan;
 

include/aikido/planner/SequenceMetaPlanner.hpp

@@ -11,6 +11,8 @@ namespace planner {
 class SequenceMetaPlanner : public CompositePlanner
 {
 public:
+  using CompositePlanner::CompositePlanner;
+
   // Documentation inherited.
   trajectory::TrajectoryPtr plan(
       const Problem& problem, Result* result = nullptr) override;

include/aikido/planner/SnapConfigurationToConfigurationPlanner.hpp

@@ -28,7 +28,7 @@ class SnapConfigurationToConfigurationPlanner
   /// \param[in] interpolator Interpolator used to produce the output
   /// trajectory. If nullptr is passed in, GeodesicInterpolator is used by
   /// default.
-  SnapConfigurationToConfigurationPlanner(
+  explicit SnapConfigurationToConfigurationPlanner(
       statespace::ConstStateSpacePtr stateSpace,
       statespace::ConstInterpolatorPtr interpolator = nullptr);
 

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp

@@ -0,0 +1,29 @@
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONTOCONFIGURATIONTOTSR_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONTOCONFIGURATIONTOTSR_HPP_
+
+#include "aikido/planner/ConfigurationToConfigurationPlanner.hpp"
+#include "aikido/planner/dart/ConfigurationToTSRPlanner.hpp"
+#include "aikido/planner/dart/PlannerAdapter.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+class ConfigurationToConfiguration_to_ConfigurationToTSR
+    : public PlannerAdapter<ConfigurationToConfigurationPlanner,
+                            ConfigurationToTSRPlanner>
+{
+public:
+  ConfigurationToConfiguration_to_ConfigurationToTSR(
+      std::shared_ptr<ConfigurationToConfigurationPlanner> planner,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+
+  virtual trajectory::TrajectoryPtr plan(
+      const ConfigurationToTSR& problem, Planner::Result* result) override;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONTOCONFIGURATIONTOTSR_HPP_

include/aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp

@@ -1,8 +1,8 @@
 #ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
 #define AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
 
-#include "aikido/planner/SingleProblemPlanner.hpp"
 #include "aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp"
+#include "aikido/planner/dart/SingleProblemPlanner.hpp"
 #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Trajectory.hpp"
 
@@ -12,21 +12,23 @@ namespace dart {
 
 /// Base planner class for ConfigurationToEndEffectorOffset planning problem.
 class ConfigurationToEndEffectorOffsetPlanner
-    : public SingleProblemPlanner<ConfigurationToEndEffectorOffsetPlanner,
-                                  ConfigurationToEndEffectorOffset>
+    : public dart::SingleProblemPlanner<ConfigurationToEndEffectorOffsetPlanner,
+                                        ConfigurationToEndEffectorOffset>
 {
 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,
+  // while the virtual function defined in this class takes SolvableProblem,
   // which is simply ConfigurationToEndEffectorOffset.
   using SingleProblemPlanner::plan;
 
   /// Constructor
   ///
   /// \param[in] stateSpace State space that this planner associated with.
-  explicit ConfigurationToEndEffectorOffsetPlanner(
-      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace);
+  /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  ConfigurationToEndEffectorOffsetPlanner(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
 
   /// Solves \c problem returning the result to \c result.
   ///
@@ -36,15 +38,6 @@ class ConfigurationToEndEffectorOffsetPlanner
       const SolvableProblem& problem, Result* result = nullptr)
       = 0;
   // Note: SolvableProblem is defined in SingleProblemPlanner.
-
-  /// Return ConstMetaSkeletonStateSpacePtr by performing a static cast on
-  /// mStateSpace.
-  statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace();
-
-protected:
-  /// Stores stateSpace pointer as a ConstMetaSkeletonStateSpacePtr. Prevents
-  /// use of an expensive dynamic cast.
-  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
 };
 
 } // namespace dart

include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp

@@ -3,7 +3,7 @@
 
 #include <dart/dart.hpp>
 #include "aikido/planner/Problem.hpp"
-#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 
 namespace aikido {
@@ -24,9 +24,9 @@ class ConfigurationToEndEffectorPose : public Problem
   /// \throw If \c stateSpace is not compatible with \c constraint's state
   /// space.
   ConfigurationToEndEffectorPose(
-      statespace::ConstStateSpacePtr stateSpace,
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
       ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
-      const statespace::StateSpace::State* startState,
+      const statespace::dart::MetaSkeletonStateSpace::State* startState,
       const Eigen::Isometry3d& goalPose,
       constraint::ConstTestablePtr constraint);
 
@@ -40,7 +40,7 @@ class ConfigurationToEndEffectorPose : public Problem
   ::dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
 
   /// Returns the start state.
-  const statespace::StateSpace::State* getStartState() const;
+  const statespace::dart::MetaSkeletonStateSpace::State* getStartState() const;
 
   /// Returns the goal pose.
   const Eigen::Isometry3d& getGoalPose() const;
@@ -53,7 +53,7 @@ class ConfigurationToEndEffectorPose : public Problem
   const ::dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
 
   /// Start state.
-  const statespace::StateSpace::State* mStartState;
+  const statespace::dart::MetaSkeletonStateSpace::State* mStartState;
 
   /// Goal pose.
   const Eigen::Isometry3d mGoalPose;

include/aikido/planner/dart/ConfigurationToTSR.hpp

@@ -4,7 +4,7 @@
 #include <dart/dart.hpp>
 #include "aikido/constraint/dart/TSR.hpp"
 #include "aikido/planner/Problem.hpp"
-#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 
 namespace aikido {
@@ -20,15 +20,17 @@ class ConfigurationToTSR : public Problem
   /// \param[in] stateSpace State space.
   /// \param[in] endEffectorBodyNode BodyNode to be planned to move to a desired
   /// TSR.
+  /// \param[in] maxSamples Maximum number of TSR samples to plan to.
   /// \param[in] startState Start state.
   /// \param[in] goalTSR Goal TSR.
   /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
   /// \throw If \c stateSpace is not compatible with \c constraint's state
   /// space.
   ConfigurationToTSR(
-      statespace::ConstStateSpacePtr stateSpace,
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
       ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
-      const statespace::StateSpace::State* startState,
+      const statespace::dart::MetaSkeletonStateSpace::State* startState,
+      std::size_t maxSamples,
       constraint::dart::ConstTSRPtr goalTSR,
       constraint::ConstTestablePtr constraint);
 
@@ -41,8 +43,11 @@ class ConfigurationToTSR : public Problem
   /// Returns the end-effector BodyNode to be planned to move to a desired TSR.
   ::dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
 
+  /// Returns the maximum number of TSR samples to plan to.
+  std::size_t getMaxSamples() const;
+
   /// Returns the start state.
-  const statespace::StateSpace::State* getStartState() const;
+  const statespace::dart::MetaSkeletonStateSpace::State* getStartState() const;
 
   /// Returns the goal TSR.
   constraint::dart::ConstTSRPtr getGoalTSR() const;
@@ -51,10 +56,13 @@ class ConfigurationToTSR : public Problem
   /// End-effector body node.
   const ::dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
 
+  /// Maximum number of TSR samples to plan to.
+  std::size_t mMaxSamples;
+
   /// Start state.
-  const statespace::StateSpace::State* mStartState;
+  const statespace::dart::MetaSkeletonStateSpace::State* mStartState;
 
-  /// Goal TSR
+  /// Goal TSR.
   const constraint::dart::ConstTSRPtr mGoalTSR;
 };
 

include/aikido/planner/dart/ConfigurationToTSRPlanner.hpp

@@ -0,0 +1,47 @@
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRPLANNER_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRPLANNER_HPP_
+
+#include "aikido/planner/dart/ConfigurationToTSR.hpp"
+#include "aikido/planner/dart/SingleProblemPlanner.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+/// Base planner class for ConfigurationToTSR planning problem.
+class ConfigurationToTSRPlanner
+    : public dart::SingleProblemPlanner<ConfigurationToTSRPlanner,
+                                        ConfigurationToTSR>
+{
+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 SolvableProblem,
+  // which is simply ConfigurationToTSR.
+  using SingleProblemPlanner::plan;
+
+  /// Constructor
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  ConfigurationToTSRPlanner(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+
+  /// 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 dart
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRPLANNER_HPP_

include/aikido/planner/dart/PlannerAdapter.hpp

@@ -0,0 +1,72 @@
+#ifndef AIKIDO_PLANNER_DART_PLANNERADAPTER_HPP_
+#define AIKIDO_PLANNER_DART_PLANNERADAPTER_HPP_
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+/// Adapts a DelegatePlanner to solve the single problem that TargetPlanner can
+/// solve.
+///
+/// \tparam DelegatePlanner type of \c SingleProblemPlanner to delegate to
+/// \tparam TargetPlanner type of \c dart::SingleProblemPlanner to implement
+template <typename DelegatePlanner,
+          typename TargetPlanner,
+          typename DelegateIsDartPlanner = void>
+class PlannerAdapter : public TargetPlanner
+{
+public:
+  /// Constructor to adapt non-DART planners.
+  ///
+  /// \param[in] planner Delegate planner to use internally.
+  /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  explicit PlannerAdapter(
+      std::shared_ptr<DelegatePlanner> planner,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+
+  /// Default destructor.
+  virtual ~PlannerAdapter() = default;
+
+protected:
+  /// Internal planner to delegate planning calls
+  std::shared_ptr<DelegatePlanner> mDelegate;
+};
+
+/// Adapts a DelegatePlanner to solve the single problem that TargetPlanner can
+/// solve.
+///
+/// \tparam DelegatePlanner type of \c dart::SingleProblemPlanner to delegate to
+/// \tparam TargetPlanner type of \c dart::SingleProblemPlanner to implement
+template <typename DelegatePlanner, typename TargetPlanner>
+class
+    PlannerAdapter<DelegatePlanner,
+                   TargetPlanner,
+                   typename std::
+                       enable_if<std::
+                                     is_base_of<dart::
+                                                    SingleProblemPlanner<DelegatePlanner,
+                                                                         typename DelegatePlanner::
+                                                                             SolvableProblem>,
+                                                DelegatePlanner>::value>::type>
+{
+public:
+  /// Constructor to adapt DART planners.
+  ///
+  /// \param[in] planner DART delegate planner to use internally.
+  explicit PlannerAdapter(std::shared_ptr<DelegatePlanner> planner);
+
+  /// Default destructor.
+  virtual ~PlannerAdapter() = default;
+
+protected:
+  /// Internal planner to delegate planning calls
+  std::shared_ptr<DelegatePlanner> mDelegate;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#include "aikido/planner/dart/detail/PlannerAdapter-impl.hpp"
+
+#endif // AIKIDO_PLANNER_DART_PLANNERADAPTER_HPP_

include/aikido/planner/dart/SingleProblemPlanner.hpp

@@ -0,0 +1,48 @@
+#ifndef AIKIDO_PLANNER_DART_SINGLEPROBLEMPLANNER_HPP_
+#define AIKIDO_PLANNER_DART_SINGLEPROBLEMPLANNER_HPP_
+
+#include "aikido/planner/SingleProblemPlanner.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+/// Base planner class for all DART single problem planners. Avoids copying
+/// MetaSkeleton related code a bunch of times.
+template <typename Derived, typename ProblemT>
+class SingleProblemPlanner
+    : public planner::SingleProblemPlanner<Derived, ProblemT>
+{
+public:
+  /// Constructor
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  SingleProblemPlanner(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+
+  /// Return this planner's MetaSkeletonStateSpace.
+  statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace()
+      const;
+
+  /// Return this planner's MetaSkeleton.
+  ::dart::dynamics::MetaSkeletonPtr getMetaSkeleton();
+
+protected:
+  /// Stores stateSpace pointer as a ConstMetaSkeletonStateSpacePtr. Prevents
+  /// use of an expensive dynamic cast.
+  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+
+  /// MetaSkeleton to use for planning.
+  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#include "aikido/planner/dart/detail/SingleProblemPlanner-impl.hpp"
+
+#endif // AIKIDO_PLANNER_DART_SINGLEPROBLEMPLANNER_HPP_