Fix issues discovered during demo. (#442)

* Test with COnfitoConf and seems to work!

* Add getTrajectoryPostProcessor() method to ConcreteRobot class.

* Make ConfigurationToEndEffectorOffset and ConfigurationToTSR problems also use ScopedState internally.

* HACK: Make ConfigurationToEndEffectorOffset getDirection() method return bodyNode direction if passed direction was zero vector.

* Fix ConfigurationToConfiguration_to_ConfigurationToTSR adapter now that problems can store a cloned, scoped version of the passed state.

* HACK: Fix hack for returning current BN direction in Offset problem (for moving straight).

* HACK: Get around HERB hand BN not being in arm metaSkeleton, and nullptr RNG.

* HACK: Update hacks to work with MAGI.

* HACK: Got left out of last commit.

* HACK: Cap samples in DART TSR adapter. Use better random seed.

* Make getTrajectoryPostProcessor take needed args.

* Make getTrajectoryPostProcessor take constraint checking params.

* Do BN check in TSR adapter correctly by using the skeleton.

* Add RNG to ConfigurationToConfiguration_to_ConfigurationToTSR.

* Remove startState from DART problems. Need to fix tests.

* Add DART ConfigurationToConfigurationPlanner.

* Add new adapter that turns non-DART ConfTOConf planner into a DART one.

* Update tests for removing startState from DART problems.

* Run make format on last commit.

* Fix tests from adding RNG to TSR adapter.

* Fix naming ambiguity that made TSR adapter not build.

* Use boost:none instead of zero vector to get straight direction in offset problem.

* Make Offset problem take optional as reference.

* Clean up Offset problem comments and restore zero vector check.

* Remove TSR adapter hack to prevent inf loops.

* HACK BODY NODE FALLS OFF

* HACK HACK HACK.

* HACK HACK JH

* HACK: IKJDSHFKJDSHFKJDSHFKDSHFLJDHFSKJHDSFKJDHF

* Revert "HACK: IKJDSHFKJDSHFKJDSHFKDSHFLJDHFSKJHDSFKJDHF"

This reverts commit 40482d3.

* Revert "HACK HACK JH"

This reverts commit 42cd655.

* Revert "HACK HACK HACK."

This reverts commit 85eee66.

* Revert "HACK BODY NODE FALLS OFF"

This reverts commit 40b1207.

* Update ConfigurationToEndEffectorOffset to use multiple constructors.

* Update getStartState in ConfigurationToEndEffectorOffset.

* Update ConfigurationToEndEffectorPose to use two constructors and restore getStartState. Fix state passing in ConfigurationToEndEffectorOffset.

* Update ConfigurationToTSR to use multiple constructors and restore getStartState.

* Update TSR adapter to just use getStartState().

* Pass start state to VFP.

* Revert "Update TSR adapter to just use getStartState()."

This reverts commit 976b443.

* UpdateDART planning problems to not return dangling pointers with getStateState().

* Draft dart/ConfigurationToConfiguration.hpp

* Draft dart/ConfigurationToConfiguration.cpp

* Make adapter use right ConfigToConfig Problem.

* Trim fat in new getTrajectoryPostProcessor method.

* Fix tests with what we have now.

* Run make format.

* Make Planner take RNG.

* Add rng arg to SingleProblemPlanner and ConfigurationToConfigurationPlanner constructors to enable future OMPL planners to take RNG.

* Use delegate RNG in TSR adapter. Make tests build again.

* Move getEndEffectorDirection into robot/util

* Create planner/dart/util and put getEndEffectorDirection there instead.

* Change Planner to take raw RNG pointer, and getRng to return this raw pointer.

* Respond to nits.

* Fix RNG destruction in Planner class.

* Repond to latest review comments.

* Remove some std::moves to fix RVO build issue with OSX.

* Nab one std::move missed by the last commit..

* Use getState in DART planning problems.

* Remove uneeded aikido::'s in ConfigurationToConfiguration_to_ConfigurationToConfiguration.

* Run make format.

* Respond to all but one of Gilwoo's nits.

* Respond to some more nits.

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;
 };