Refactor VFP with new Planner API

include/aikido/planner/ConfigurationToEndEffectorOffset.hpp

@@ -4,7 +4,7 @@
 #include <dart/dart.hpp>
 #include "aikido/constraint/Testable.hpp"
 #include "aikido/planner/Problem.hpp"
-#include "aikido/statespace/StateSpace.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 #include "aikido/trajectory/Interpolated.hpp"
 
 namespace aikido {
@@ -29,9 +29,9 @@ class ConfigurationToEndEffectorOffset : public Problem
   /// \param[in] constraint Trajectory-wide constraint that must be satisfied.
   /// \throw If the size of \c direction is zero.
   ConfigurationToEndEffectorOffset(
-      statespace::ConstStateSpacePtr stateSpace,
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
       dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
-      const statespace::StateSpace::State* startState,
+      const statespace::dart::MetaSkeletonStateSpace::State* startState,
       const Eigen::Vector3d& direction,
       double signedDistance,
       constraint::ConstTestablePtr constraint);
@@ -47,7 +47,7 @@ class ConfigurationToEndEffectorOffset : 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 direction of motion specified in the world frame.
   ///
@@ -62,7 +62,7 @@ class ConfigurationToEndEffectorOffset : public Problem
   const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
 
   /// Start state.
-  const statespace::StateSpace::State* mStartState;
+  const statespace::dart::MetaSkeletonStateSpace::State* mStartState;
 
   /// Direction of motion.
   const Eigen::Vector3d mDirection;

include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp

@@ -0,0 +1,52 @@
+#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+
+#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp"
+#include "aikido/planner/SingleProblemPlanner.hpp"
+#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
+#include "aikido/trajectory/Trajectory.hpp"
+
+namespace aikido {
+namespace planner {
+
+/// Base planner class for ConfigurationToEndEffectorOffset planning problem.
+class ConfigurationToEndEffectorOffsetPlanner
+    : public 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,
+  // which is simply ConfigurationToEndEffectorOffset.
+  using SingleProblemPlanner::plan;
+
+  /// Constructor
+  ///
+  /// \param[in] stateSpace State space that this planner associated with.
+  explicit ConfigurationToEndEffectorOffsetPlanner(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr 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.
+
+  /// Return ConstMetaSkeletonStateSpacePtr by performing a static cast on
+  /// mStateSpace.
+  statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace();
+
+protected:
+  /// Stores stateSpace pointer as aConstMetaSkeletonStateSpacePtr. Prevents
+  /// use of an expensive dynamic cast.
+  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
+};
+
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_

include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

@@ -0,0 +1,92 @@
+#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+
+#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp"
+#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp"
+
+namespace aikido {
+namespace planner {
+namespace vectorfield {
+
+/// Planner that generates a trajectory that moves the end-effector by a given
+/// direction and distance.
+class VectorFieldConfigurationToEndEffectorOffsetPlanner
+    : public ConfigurationToEndEffectorOffsetPlanner
+{
+public:
+  /// Constructor.
+  ///
+  /// \param[in] stateSpace State space that this planner is associated with.
+  /// \param[in] metaskeleton MetaSkeleton to plan with.
+  /// \param[in] distanceTolerance How much a planned trajectory is allowed to
+  /// deviate from the requested distance to move the end-effector.
+  /// \param[in] positionTolerance How a planned trajectory is allowed to
+  /// deviated from a straight line segment defined by the direction and the
+  /// distance.
+  /// \param[in] angularTolerance How a planned trajectory is allowed to deviate
+  /// from a given direction.
+  /// \param[in] initialStepSize Initial step size.
+  /// \param[in] jointLimitTolerance If less then this distance to joint
+  /// limit, velocity is bounded in that direction to 0.
+  /// \param[in] constraintCheckResolution Resolution used in constraint
+  /// checking.
+  /// \param[in] timelimit timeout in seconds.
+  VectorFieldConfigurationToEndEffectorOffsetPlanner(
+      statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+      dart::dynamics::MetaSkeletonPtr metaskeleton,
+      double distanceTolerance,
+      double positionTolerance,
+      double angularTolerance,
+      double initialStepSize,
+      double jointLimitTolerance,
+      double constraintCheckResolution,
+      std::chrono::duration<double> timelimit);
+
+  /// Plan to a trajectory that moves the end-effector by a given direction and
+  /// distance.
+  ///
+  /// The planner returns success if the resulting trajectory satisfies
+  /// constraint at some resolution and failure (returning \c nullptr)
+  /// otherwise. The reason for the failure is stored in the \c result output
+  /// parameter.
+  ///
+  /// \param[in] problem Planning problem.
+  /// \param[out] result Information about success or failure.
+  /// \return Trajectory or \c nullptr if planning failed.
+  trajectory::TrajectoryPtr plan(
+      const SolvableProblem& problem, Result* result = nullptr) override;
+
+protected:
+  /// MetaSkeleton to plan with.
+  dart::dynamics::MetaSkeletonPtr mMetaskeleton;
+
+  /// How much a planned trajectory is allowed to deviate from the requested
+  /// distance to move the end-effector.
+  double mDistanceTolerance;
+
+  /// How a planned trajectory is allowed to deviated from a straight line
+  /// segment defined by the direction and the distance.
+  double mPositionTolerance;
+
+  /// How a planned trajectory is allowed to deviate from a given direction.
+  double mAngularTolerance;
+
+  /// Initial step size.
+  double mInitialStepSize;
+
+  /// If less then this distance to joint limit, velocity is bounded in that
+  /// direction to 0.
+  double mJointLimitTolerance;
+
+  /// Resolution used in constraint checking.
+  double mConstraintCheckResolution;
+
+  /// Timeout in seconds.
+  std::chrono::duration<double> mTimelimit;
+};
+
+} // namespace vectorfield
+} // namespace planner
+} // namespace aikido
+
+#endif // AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

@@ -55,6 +55,7 @@ std::unique_ptr<aikido::trajectory::Spline> followVectorField(
 /// \param[out] result information about success or failure.
 /// \return Trajectory or \c nullptr if planning failed.
 std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
+    const statespace::dart::MetaSkeletonStateSpace::State& startState,
     const aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr& stateSpace,
     dart::dynamics::MetaSkeletonPtr metaskeleton,
     const dart::dynamics::ConstBodyNodePtr& bn,

src/planner/CMakeLists.txt

@@ -4,6 +4,7 @@ set(sources
   ConfigurationToConfigurations.cpp
   ConfigurationToTSR.cpp
   ConfigurationToEndEffectorOffset.cpp
+  ConfigurationToEndEffectorOffsetPlanner.cpp
   ConfigurationToEndEffectorPose.cpp
   FirstSupportedMetaPlanner.cpp
   CompositePlanner.cpp

src/planner/ConfigurationToEndEffectorOffset.cpp

@@ -7,9 +7,9 @@ namespace planner {
 
 //==============================================================================
 ConfigurationToEndEffectorOffset::ConfigurationToEndEffectorOffset(
-    statespace::ConstStateSpacePtr stateSpace,
+    statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
     dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
-    const statespace::StateSpace::State* startState,
+    const statespace::dart::MetaSkeletonStateSpace::State* startState,
     const Eigen::Vector3d& direction,
     const double signedDistance,
     constraint::ConstTestablePtr constraint)
@@ -44,7 +44,7 @@ ConfigurationToEndEffectorOffset::getEndEffectorBodyNode() const
 }
 
 //==============================================================================
-const statespace::StateSpace::State*
+const statespace::dart::MetaSkeletonStateSpace::State*
 ConfigurationToEndEffectorOffset::getStartState() const
 {
   return mStartState;

src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp

@@ -0,0 +1,25 @@
+#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp"
+
+namespace aikido {
+namespace planner {
+
+//==============================================================================
+ConfigurationToEndEffectorOffsetPlanner::
+    ConfigurationToEndEffectorOffsetPlanner(
+        statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace)
+  : SingleProblemPlanner<ConfigurationToEndEffectorOffsetPlanner,
+                         ConfigurationToEndEffectorOffset>(stateSpace)
+  , mMetaSkeletonStateSpace(std::move(stateSpace))
+{
+  // Do nothing
+}
+
+//==============================================================================
+statespace::dart::ConstMetaSkeletonStateSpacePtr
+ConfigurationToEndEffectorOffsetPlanner::getMetaSkeletonStateSpace()
+{
+  return mMetaSkeletonStateSpace;
+}
+
+} // namespace planner
+} // namespace aikido

src/planner/vectorfield/CMakeLists.txt

@@ -1,7 +1,8 @@
-set(sources 
+set(sources
   VectorFieldPlanner.cpp
   VectorField.cpp
   VectorFieldUtil.cpp
+  VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp
   detail/VectorFieldIntegrator.cpp
   detail/VectorFieldPlannerExceptions.cpp
   BodyNodePoseVectorField.cpp
@@ -42,4 +43,3 @@ add_component_dependencies(${PROJECT_NAME} planner_vectorfield
 )
 
 format_add_sources(${sources})
-

src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp

@@ -0,0 +1,88 @@
+#include "aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp"
+
+#include "aikido/planner/vectorfield/VectorFieldPlanner.hpp"
+
+namespace aikido {
+namespace planner {
+namespace vectorfield {
+
+//==============================================================================
+VectorFieldConfigurationToEndEffectorOffsetPlanner::
+    VectorFieldConfigurationToEndEffectorOffsetPlanner(
+        statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
+        dart::dynamics::MetaSkeletonPtr metaskeleton,
+        double distanceTolerance,
+        double positionTolerance,
+        double angularTolerance,
+        double initialStepSize,
+        double jointLimitTolerance,
+        double constraintCheckResolution,
+        std::chrono::duration<double> timelimit)
+  : ConfigurationToEndEffectorOffsetPlanner(std::move(stateSpace))
+  , mMetaskeleton(std::move(metaskeleton))
+  , mDistanceTolerance(distanceTolerance)
+  , mPositionTolerance(positionTolerance)
+  , mAngularTolerance(angularTolerance)
+  , mInitialStepSize(initialStepSize)
+  , mJointLimitTolerance(jointLimitTolerance)
+  , mConstraintCheckResolution(constraintCheckResolution)
+  , mTimelimit(timelimit)
+{
+  // Do nothing here.
+}
+
+//==============================================================================
+trajectory::TrajectoryPtr
+VectorFieldConfigurationToEndEffectorOffsetPlanner::plan(
+    const SolvableProblem& problem, Result* result)
+{
+  // TODO (sniyaz): Check equality between state space of this planner and given
+  // problem.
+
+  using aikido::planner::vectorfield::planToEndEffectorOffset;
+  using aikido::statespace::dart::MetaSkeletonStateSpace;
+
+  // Handle the fact that distance can be negative.
+  double distance = problem.getDistance();
+  Eigen::Vector3d direction = problem.getDirection();
+
+  if (distance < 0)
+  {
+    distance = -1.0 * distance;
+    direction = -1.0 * direction;
+  }
+
+  double minDistance = distance - mDistanceTolerance;
+  double maxDistance = distance + mDistanceTolerance;
+
+  if (minDistance < 0.)
+  {
+    std::stringstream ss;
+    ss << "Distance must be non-negative; min distance to move is "
+       << problem.getDistance() - mDistanceTolerance << ".";
+    throw std::runtime_error(ss.str());
+  }
+
+  // Just call the core VFP function.
+  // TODO: How should start state be handled?
+  return planToEndEffectorOffset(
+      *problem.getStartState(),
+      getMetaSkeletonStateSpace(),
+      mMetaskeleton,
+      problem.getEndEffectorBodyNode(),
+      problem.getConstraint(),
+      direction,
+      minDistance,
+      maxDistance,
+      mPositionTolerance,
+      mAngularTolerance,
+      mInitialStepSize,
+      mJointLimitTolerance,
+      mConstraintCheckResolution,
+      mTimelimit,
+      result);
+}
+
+} // namespace vectorfield
+} // namespace planner
+} // namespace aikido

src/planner/vectorfield/VectorFieldPlanner.cpp

@@ -126,6 +126,7 @@ std::unique_ptr<aikido::trajectory::Spline> followVectorField(
 
 //==============================================================================
 std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
+    const statespace::dart::MetaSkeletonStateSpace::State& startState,
     const aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr& stateSpace,
     dart::dynamics::MetaSkeletonPtr metaskeleton,
     const dart::dynamics::ConstBodyNodePtr& bn,
@@ -152,23 +153,6 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
   // std::lock_guard<std::mutex> lock(metaskeleton->getLockableReference())
   // once https://github.com/dartsim/dart/pull/1011 is released.
 
-  if (minDistance < 0.)
-  {
-    std::stringstream ss;
-    ss << "Distance must be non-negative; got " << minDistance << ".";
-    throw std::runtime_error(ss.str());
-  }
-
-  if (maxDistance < minDistance)
-  {
-    throw std::runtime_error("Max distance is less than distance.");
-  }
-
-  if (direction.norm() == 0.0)
-  {
-    throw std::runtime_error("Direction vector is a zero vector");
-  }
-
   // TODO: Check compatibility between MetaSkeleton and MetaSkeletonStateSpace
 
   // Save the current state of the space
@@ -195,11 +179,10 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
   compoundConstraint->addConstraint(
       constraint::dart::createTestableBounds(stateSpace));
 
-  auto startState
-      = stateSpace->getScopedStateFromMetaSkeleton(metaskeleton.get());
+  stateSpace->setState(metaskeleton.get(), &startState);
   return followVectorField(
       *vectorfield,
-      *startState,
+      startState,
       *compoundConstraint,
       timelimit,
       initialStepSize,