Fix issues discovered during demo.

include/aikido/planner/ConfigurationToConfiguration.hpp

@@ -41,10 +41,10 @@ class ConfigurationToConfiguration : public Problem
 
 protected:
   /// Start state.
-  const statespace::StateSpace::State* mStartState;
+  statespace::StateSpace::ScopedState mStartState;
 
   /// Goal state.
-  const statespace::StateSpace::State* mGoalState;
+  statespace::StateSpace::ScopedState mGoalState;
 };
 
 } // namespace planner

include/aikido/planner/ConfigurationToConfigurationPlanner.hpp

@@ -20,11 +20,9 @@ class ConfigurationToConfigurationPlanner
   // which is simply ConfigurationToConfiguration.
   using SingleProblemPlanner::plan;
 
-  /// Constructor
-  ///
-  /// \param[in] stateSpace State space that this planner associated with.
+  /// \copydoc Planner::Planner
   explicit ConfigurationToConfigurationPlanner(
-      statespace::ConstStateSpacePtr stateSpace);
+      statespace::ConstStateSpacePtr stateSpace, common::RNG* rng = nullptr);
 
   /// Solves \c problem returning the result to \c result.
   ///

include/aikido/planner/Planner.hpp

@@ -22,14 +22,19 @@ class Planner
   /// Constructs from a state space.
   ///
   /// \param[in] stateSpace State space that this planner associated with.
-  explicit Planner(statespace::ConstStateSpacePtr stateSpace);
+  /// \param[in] rng RNG that planner uses. If nullptr, a default is created.
+  explicit Planner(
+      statespace::ConstStateSpacePtr stateSpace, common::RNG* rng = nullptr);
 
   /// Default destructor.
   virtual ~Planner() = default;
 
   /// Returns const state space.
   statespace::ConstStateSpacePtr getStateSpace() const;
 
+  /// Returns RNG.
+  common::RNG* getRng();
+
   /// Returns true if this planner can solve \c problem.
   virtual bool canSolve(const Problem& problem) const = 0;
 
@@ -44,6 +49,9 @@ class Planner
 protected:
   /// State space associated with this planner.
   statespace::ConstStateSpacePtr mStateSpace;
+
+  /// RNG the planner uses.
+  std::unique_ptr<common::RNG> mRng;
 };
 
 /// Base class for planning result of various planning problems.

include/aikido/planner/SingleProblemPlanner.hpp

@@ -24,10 +24,9 @@ class SingleProblemPlanner : public Planner
 public:
   using SolvableProblem = ProblemT;
 
-  /// Constructor
-  ///
-  /// \param[in] stateSpace State space associated with this planner.
-  explicit SingleProblemPlanner(statespace::ConstStateSpacePtr stateSpace);
+  /// \copydoc Planner::Planner
+  explicit SingleProblemPlanner(
+      statespace::ConstStateSpacePtr stateSpace, common::RNG* rng = nullptr);
 
   // Documentation inherited.
   bool canSolve(const Problem& problem) const final override;

include/aikido/planner/dart/ConfigurationToConfiguration.hpp

@@ -0,0 +1,75 @@
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATION_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATION_HPP_
+
+#include <dart/dart.hpp>
+#include "aikido/constraint/Testable.hpp"
+#include "aikido/planner/Problem.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+/// Planning problem to plan to a single goal configuration.
+class ConfigurationToConfiguration : public Problem
+{
+public:
+  /// Constructor. Note that this constructor takes the start state from the
+  /// current state of the passed MetaSkeleton.
+  ///
+  /// \param[in] stateSpace State space.
+  /// param[in] metaSkeleton MetaSkeleton that getStartState will return the
+  /// current state of when called.
+  /// \param[in] goalState Goal state to plan to.
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  ConfigurationToConfiguration(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton,
+      const statespace::dart::MetaSkeletonStateSpace::State* goalState,
+      constraint::ConstTestablePtr constraint = nullptr);
+
+  /// Constructor. Note that this constructor sets the start state on
+  /// construction.
+  ///
+  /// \param[in] stateSpace State space.
+  /// \param[in] startState Start state to plan from.
+  /// \param[in] goalState Goal state to plan to.
+  /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
+  ConfigurationToConfiguration(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      const statespace::dart::MetaSkeletonStateSpace::State* startState,
+      const statespace::dart::MetaSkeletonStateSpace::State* goalState,
+      constraint::ConstTestablePtr constraint = nullptr);
+
+  // 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::dart::MetaSkeletonStateSpace::State* getStartState() const;
+
+  /// Returns the goal state.
+  const statespace::dart::MetaSkeletonStateSpace::State* getGoalState() const;
+
+protected:
+  /// MetaSkeletonStateSpace. Prevents use of expensive dynamic cast on
+  /// mStateSpace.
+  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+
+  /// MetaSkeleton, if given.
+  ::dart::dynamics::ConstMetaSkeletonPtr mMetaSkeleton;
+
+  /// Start state.
+  statespace::dart::MetaSkeletonStateSpace::ScopedState mStartState;
+
+  /// Goal state.
+  statespace::dart::MetaSkeletonStateSpace::ScopedState mGoalState;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATION_HPP_

include/aikido/planner/dart/ConfigurationToConfigurationPlanner.hpp

@@ -0,0 +1,47 @@
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_
+
+#include "aikido/planner/dart/ConfigurationToConfiguration.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 ConfigurationToEndEffectorOffset planning problem.
+class ConfigurationToConfigurationPlanner
+    : public dart::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 SolvableProblem,
+  // which is simply ConfigurationToConfiguration.
+  using SingleProblemPlanner::plan;
+
+  /// Constructor
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  /// \param[in] metaSkeleton MetaSkeleton to use for planning.
+  ConfigurationToConfigurationPlanner(
+      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_CONFIGURATIONTOCONFIGURATIONPLANNER_HPP_

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.hpp

@@ -0,0 +1,37 @@
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONTOCONFIGURATIONTOCONFIGURATION_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONTOCONFIGURATIONTOCONFIGURATION_HPP_
+
+#include "aikido/planner/ConfigurationToConfigurationPlanner.hpp"
+#include "aikido/planner/dart/ConfigurationToConfigurationPlanner.hpp"
+#include "aikido/planner/dart/PlannerAdapter.hpp"
+
+namespace aikido {
+namespace planner {
+namespace dart {
+
+/// Converts a non-DART ConfigurationToConfiguration planner into the DART
+/// version.
+class ConfigurationToConfiguration_to_ConfigurationToConfiguration
+    : public PlannerAdapter<planner::ConfigurationToConfigurationPlanner,
+                            planner::dart::ConfigurationToConfigurationPlanner>
+{
+public:
+  /// Constructor
+  ///
+  /// \param[in] planner Non-DART planner to convert.
+  /// \param[in] metaSkeleton MetaSkeleton for adapted planner to operate on.
+  ConfigurationToConfiguration_to_ConfigurationToConfiguration(
+      std::shared_ptr<planner::ConfigurationToConfigurationPlanner> planner,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
+
+  // Documentation inherited.
+  virtual trajectory::TrajectoryPtr plan(
+      const planner::dart::ConfigurationToConfiguration& problem,
+      Planner::Result* result) override;
+};
+
+} // namespace dart
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOCONFIGURATIONTOCONFIGURATIONTOCONFIGURATION_HPP_

include/aikido/planner/dart/ConfigurationToConfiguration_to_ConfigurationToTSR.hpp

@@ -9,15 +9,25 @@ namespace aikido {
 namespace planner {
 namespace dart {
 
+/// Converts a non-DART ConfigurationToConfiguration planner into a DART
+/// ConfigurationToTSR planner.
 class ConfigurationToConfiguration_to_ConfigurationToTSR
-    : public PlannerAdapter<ConfigurationToConfigurationPlanner,
+    : public PlannerAdapter<aikido::planner::
+                                ConfigurationToConfigurationPlanner,
                             ConfigurationToTSRPlanner>
 {
 public:
+  /// Constructor
+  ///
+  /// \param[in] planner Non-DART ConfigurationToConfigurationPlanner planner to
+  /// convert.
+  /// \param[in] metaSkeleton MetaSkeleton for adapted planner to operate on.
   ConfigurationToConfiguration_to_ConfigurationToTSR(
-      std::shared_ptr<ConfigurationToConfigurationPlanner> planner,
+      std::shared_ptr<aikido::planner::ConfigurationToConfigurationPlanner>
+          planner,
       ::dart::dynamics::MetaSkeletonPtr metaSkeleton);
 
+  // Documentation inherited.
   virtual trajectory::TrajectoryPtr plan(
       const ConfigurationToTSR& problem, Planner::Result* result) override;
 };