Move MetaSkeleton-dependent problems to aikido::planner::dart.

CHANGELOG.md

@@ -38,7 +38,7 @@
 
   * 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), [#414](https://github.com/personalrobotics/aikido/pull/414), [#426](https://github.com/personalrobotics/aikido/pull/426)
+  * Refactored planning API: [#314](https://github.com/personalrobotics/aikido/pull/314), [#414](https://github.com/personalrobotics/aikido/pull/414), [#426](https://github.com/personalrobotics/aikido/pull/426), [#432](https://github.com/personalrobotics/aikido/pull/432)
   * 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)

include/aikido/planner/ConfigurationToEndEffectorOffset.hpp → include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp

@@ -1,5 +1,5 @@
-#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
-#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
 
 #include <dart/dart.hpp>
 #include "aikido/constraint/Testable.hpp"
@@ -9,6 +9,7 @@
 
 namespace aikido {
 namespace planner {
+namespace dart {
 
 /// Planning problem to plan to desired end-effector offset while maintaining
 /// the current end-effector orientation.
@@ -30,7 +31,7 @@ class ConfigurationToEndEffectorOffset : public Problem
   /// \throw If the size of \c direction is zero.
   ConfigurationToEndEffectorOffset(
       statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
-      dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
       const statespace::dart::MetaSkeletonStateSpace::State* startState,
       const Eigen::Vector3d& direction,
       double signedDistance,
@@ -44,7 +45,7 @@ class ConfigurationToEndEffectorOffset : public Problem
 
   /// Returns the end-effector BodyNode to be planned to move a desired offest
   /// while maintaining the current orientation.
-  dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
+  ::dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
 
   /// Returns the start state.
   const statespace::dart::MetaSkeletonStateSpace::State* getStartState() const;
@@ -59,7 +60,7 @@ class ConfigurationToEndEffectorOffset : public Problem
 
 protected:
   /// End-effector body node.
-  const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
+  const ::dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
 
   /// Start state.
   const statespace::dart::MetaSkeletonStateSpace::State* mStartState;
@@ -71,7 +72,8 @@ class ConfigurationToEndEffectorOffset : public Problem
   const double mDistance;
 };
 
+} // namespace dart
 } // namespace planner
 } // namespace aikido
 
-#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSET_HPP_

include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp → include/aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp

@@ -1,13 +1,14 @@
-#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
-#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
 
-#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp"
 #include "aikido/planner/SingleProblemPlanner.hpp"
+#include "aikido/planner/dart/ConfigurationToEndEffectorOffset.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 ConfigurationToEndEffectorOffsetPlanner
@@ -41,12 +42,13 @@ class ConfigurationToEndEffectorOffsetPlanner
   statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace();
 
 protected:
-  /// Stores stateSpace pointer as aConstMetaSkeletonStateSpacePtr. Prevents
+  /// Stores stateSpace pointer as a ConstMetaSkeletonStateSpacePtr. Prevents
   /// use of an expensive dynamic cast.
   statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
 };
 
+} // namespace dart
 } // namespace planner
 } // namespace aikido
 
-#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_

include/aikido/planner/ConfigurationToEndEffectorPose.hpp → include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp

@@ -1,5 +1,5 @@
-#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
-#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
 
 #include <dart/dart.hpp>
 #include "aikido/planner/Problem.hpp"
@@ -8,6 +8,7 @@
 
 namespace aikido {
 namespace planner {
+namespace dart {
 
 /// Planning problem to plan to a desired end-effector pose.
 class ConfigurationToEndEffectorPose : public Problem
@@ -24,7 +25,7 @@ class ConfigurationToEndEffectorPose : public Problem
   /// space.
   ConfigurationToEndEffectorPose(
       statespace::ConstStateSpacePtr stateSpace,
-      dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
       const statespace::StateSpace::State* startState,
       const Eigen::Isometry3d& goalPose,
       constraint::ConstTestablePtr constraint);
@@ -36,7 +37,7 @@ class ConfigurationToEndEffectorPose : public 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;
+  ::dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
 
   /// Returns the start state.
   const statespace::StateSpace::State* getStartState() const;
@@ -49,7 +50,7 @@ class ConfigurationToEndEffectorPose : public Problem
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   /// End-effector body node.
-  const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
+  const ::dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
 
   /// Start state.
   const statespace::StateSpace::State* mStartState;
@@ -58,7 +59,8 @@ class ConfigurationToEndEffectorPose : public Problem
   const Eigen::Isometry3d mGoalPose;
 };
 
+} // namespace dart
 } // namespace planner
 } // namespace aikido
 
-#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTORPOSE_HPP_
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOENDEFFECTORPOSE_HPP_

include/aikido/planner/ConfigurationToTSR.hpp → include/aikido/planner/dart/ConfigurationToTSR.hpp

@@ -1,5 +1,5 @@
-#ifndef AIKIDO_PLANNER_CONFIGURATIONTOTSR_HPP_
-#define AIKIDO_PLANNER_CONFIGURATIONTOTSR_HPP_
+#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOTSR_HPP_
+#define AIKIDO_PLANNER_DART_CONFIGURATIONTOTSR_HPP_
 
 #include <dart/dart.hpp>
 #include "aikido/constraint/dart/TSR.hpp"
@@ -9,6 +9,7 @@
 
 namespace aikido {
 namespace planner {
+namespace dart {
 
 /// Planning problem to plan to a given single Task Space Region (TSR).
 class ConfigurationToTSR : public Problem
@@ -26,7 +27,7 @@ class ConfigurationToTSR : public Problem
   /// space.
   ConfigurationToTSR(
       statespace::ConstStateSpacePtr stateSpace,
-      dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+      ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
       const statespace::StateSpace::State* startState,
       constraint::dart::ConstTSRPtr goalTSR,
       constraint::ConstTestablePtr constraint);
@@ -38,7 +39,7 @@ class ConfigurationToTSR : public Problem
   static const std::string& getStaticType();
 
   /// Returns the end-effector BodyNode to be planned to move to a desired TSR.
-  dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
+  ::dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const;
 
   /// Returns the start state.
   const statespace::StateSpace::State* getStartState() const;
@@ -48,7 +49,7 @@ class ConfigurationToTSR : public Problem
 
 protected:
   /// End-effector body node.
-  const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
+  const ::dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode;
 
   /// Start state.
   const statespace::StateSpace::State* mStartState;
@@ -57,7 +58,8 @@ class ConfigurationToTSR : public Problem
   const constraint::dart::ConstTSRPtr mGoalTSR;
 };
 
+} // namespace dart
 } // namespace planner
 } // namespace aikido
 
-#endif // AIKIDO_PLANNER_CONFIGURATIONTOTSR_HPP_
+#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOTSR_HPP_

include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp

@@ -37,8 +37,8 @@ class BodyNodePoseVectorField : public VectorField
   BodyNodePoseVectorField(
       aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
           metaSkeletonStateSpace,
-      dart::dynamics::MetaSkeletonPtr metaSkeleton,
-      dart::dynamics::ConstBodyNodePtr bodyNode,
+      ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+      ::dart::dynamics::ConstBodyNodePtr bodyNode,
       double maxStepSize,
       double jointLimitPadding,
       bool enforceJointVelocityLimits = false);
@@ -80,24 +80,24 @@ class BodyNodePoseVectorField : public VectorField
   getMetaSkeletonStateSpace() const;
 
   /// Returns meta skeleton.
-  dart::dynamics::MetaSkeletonPtr getMetaSkeleton();
+  ::dart::dynamics::MetaSkeletonPtr getMetaSkeleton();
 
   /// Returns const meta skeleton.
-  dart::dynamics::ConstMetaSkeletonPtr getMetaSkeleton() const;
+  ::dart::dynamics::ConstMetaSkeletonPtr getMetaSkeleton() const;
 
   /// Returns const body node of end-effector.
-  dart::dynamics::ConstBodyNodePtr getBodyNode() const;
+  ::dart::dynamics::ConstBodyNodePtr getBodyNode() const;
 
 protected:
   /// Meta skeleton state space.
   aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
       mMetaSkeletonStateSpace;
 
   /// Meta Skeleton
-  dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
+  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
 
   /// BodyNode
-  dart::dynamics::ConstBodyNodePtr mBodyNode;
+  ::dart::dynamics::ConstBodyNodePtr mBodyNode;
 
   /// Maximum step size of integrator.
   double mMaxStepSize;

include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp

@@ -39,8 +39,8 @@ class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField
   /// limit, velocity is bounded in that direction to 0.
   MoveEndEffectorOffsetVectorField(
       aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
-      dart::dynamics::MetaSkeletonPtr metaskeleton,
-      dart::dynamics::ConstBodyNodePtr bn,
+      ::dart::dynamics::MetaSkeletonPtr metaskeleton,
+      ::dart::dynamics::ConstBodyNodePtr bn,
       const Eigen::Vector3d& direction,
       double minDistance,
       double maxDistance,

include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp

@@ -1,8 +1,8 @@
 #ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
 #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_
 
-#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp"
-#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp"
+#include "aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp"
+#include "aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp"
 
 namespace aikido {
 namespace planner {
@@ -11,7 +11,7 @@ namespace vectorfield {
 /// Planner that generates a trajectory that moves the end-effector by a given
 /// direction and distance.
 class VectorFieldConfigurationToEndEffectorOffsetPlanner
-    : public ConfigurationToEndEffectorOffsetPlanner
+    : public planner::dart::ConfigurationToEndEffectorOffsetPlanner
 {
 public:
   /// Constructor.
@@ -33,7 +33,7 @@ class VectorFieldConfigurationToEndEffectorOffsetPlanner
   /// \param[in] timelimit timeout in seconds.
   VectorFieldConfigurationToEndEffectorOffsetPlanner(
       statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
-      dart::dynamics::MetaSkeletonPtr metaskeleton,
+      ::dart::dynamics::MetaSkeletonPtr metaskeleton,
       double distanceTolerance,
       double positionTolerance,
       double angularTolerance,
@@ -58,7 +58,7 @@ class VectorFieldConfigurationToEndEffectorOffsetPlanner
 
 protected:
   /// MetaSkeleton to plan with.
-  dart::dynamics::MetaSkeletonPtr mMetaskeleton;
+  ::dart::dynamics::MetaSkeletonPtr mMetaskeleton;
 
   /// How much a planned trajectory is allowed to deviate from the requested
   /// distance to move the end-effector.

include/aikido/planner/vectorfield/VectorFieldPlanner.hpp

@@ -57,8 +57,8 @@ 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,
+    ::dart::dynamics::MetaSkeletonPtr metaskeleton,
+    const ::dart::dynamics::ConstBodyNodePtr& bn,
     const aikido::constraint::ConstTestablePtr& constraint,
     const Eigen::Vector3d& direction,
     double minDistance,
@@ -93,8 +93,8 @@ std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorOffset(
 /// \return Trajectory or \c nullptr if planning failed.
 std::unique_ptr<aikido::trajectory::Spline> planToEndEffectorPose(
     const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace,
-    dart::dynamics::MetaSkeletonPtr metaskeleton,
-    const dart::dynamics::BodyNodePtr& bn,
+    ::dart::dynamics::MetaSkeletonPtr metaskeleton,
+    const ::dart::dynamics::BodyNodePtr& bn,
     const aikido::constraint::TestablePtr& constraint,
     const Eigen::Isometry3d& goalPose,
     double poseErrorTolerance,

include/aikido/planner/vectorfield/VectorFieldUtil.hpp

@@ -31,8 +31,8 @@ namespace vectorfield {
 bool computeJointVelocityFromTwist(
     Eigen::VectorXd& jointVelocity,
     const Eigen::Vector6d& desiredTwist,
-    const dart::dynamics::MetaSkeletonPtr metaSkeleton,
-    const dart::dynamics::ConstBodyNodePtr bodyNode,
+    const ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
+    const ::dart::dynamics::ConstBodyNodePtr bodyNode,
     double jointLimitPadding,
     const Eigen::VectorXd& jointVelocityLowerLimits,
     const Eigen::VectorXd& jointVelocityUpperLimits,

src/planner/CMakeLists.txt

@@ -2,10 +2,6 @@ set(sources
   ConfigurationToConfiguration.cpp
   ConfigurationToConfigurationPlanner.cpp
   ConfigurationToConfigurations.cpp
-  ConfigurationToTSR.cpp
-  ConfigurationToEndEffectorOffset.cpp
-  ConfigurationToEndEffectorOffsetPlanner.cpp
-  ConfigurationToEndEffectorPose.cpp
   FirstSupportedMetaPlanner.cpp
   CompositePlanner.cpp
   Planner.cpp
@@ -17,6 +13,12 @@ set(sources
   SequenceMetaPlanner.cpp
   World.cpp
   WorldStateSaver.cpp
+  dart/ConfigurationToEndEffectorOffset.cpp
+  dart/ConfigurationToEndEffectorPose.cpp
+  dart/ConfigurationToTSR.cpp
+  dart/ConfigurationToEndEffectorOffsetPlanner.cpp
+  # dart/ConfigurationToEndEffectorPosePlanner.cpp
+  # dart/ConfigurationToTSRPlanner.cpp
 )
 
 add_library("${PROJECT_NAME}_planner" SHARED ${sources})

src/planner/ConfigurationToEndEffectorOffset.cpp → src/planner/dart/ConfigurationToEndEffectorOffset.cpp

@@ -1,14 +1,15 @@
-#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp"
+#include "aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp"
 
 #include "aikido/constraint/Testable.hpp"
 
 namespace aikido {
 namespace planner {
+namespace dart {
 
 //==============================================================================
 ConfigurationToEndEffectorOffset::ConfigurationToEndEffectorOffset(
     statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace,
-    dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
+    ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode,
     const statespace::dart::MetaSkeletonStateSpace::State* startState,
     const Eigen::Vector3d& direction,
     const double signedDistance,
@@ -37,7 +38,7 @@ const std::string& ConfigurationToEndEffectorOffset::getStaticType()
 }
 
 //==============================================================================
-dart::dynamics::ConstBodyNodePtr
+::dart::dynamics::ConstBodyNodePtr
 ConfigurationToEndEffectorOffset::getEndEffectorBodyNode() const
 {
   return mEndEffectorBodyNode;
@@ -62,5 +63,6 @@ double ConfigurationToEndEffectorOffset::getDistance() const
   return mDistance;
 }
 
+} // namespace dart
 } // namespace planner
 } // namespace aikido

src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp → src/planner/dart/ConfigurationToEndEffectorOffsetPlanner.cpp

@@ -1,7 +1,8 @@
-#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp"
+#include "aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp"
 
 namespace aikido {
 namespace planner {
+namespace dart {
 
 //==============================================================================
 ConfigurationToEndEffectorOffsetPlanner::
@@ -21,5 +22,6 @@ ConfigurationToEndEffectorOffsetPlanner::getMetaSkeletonStateSpace()
   return mMetaSkeletonStateSpace;
 }
 
+} // namespace dart
 } // namespace planner
 } // namespace aikido