diff --git a/CHANGELOG.md b/CHANGELOG.md index 428be699da..60d2602f89 100644 --- a/CHANGELOG.md +++ b/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) diff --git a/include/aikido/planner/ConfigurationToEndEffectorOffset.hpp b/include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp similarity index 84% rename from include/aikido/planner/ConfigurationToEndEffectorOffset.hpp rename to include/aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp index 7df7c097df..2f20e146b9 100644 --- a/include/aikido/planner/ConfigurationToEndEffectorOffset.hpp +++ b/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 #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_ diff --git a/include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp b/include/aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp similarity index 81% rename from include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp rename to include/aikido/planner/dart/ConfigurationToEndEffectorOffsetPlanner.hpp index 86556d2a0b..faf73d3dd4 100644 --- a/include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp +++ b/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_ diff --git a/include/aikido/planner/ConfigurationToEndEffectorPose.hpp b/include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp similarity index 78% rename from include/aikido/planner/ConfigurationToEndEffectorPose.hpp rename to include/aikido/planner/dart/ConfigurationToEndEffectorPose.hpp index 676f303319..9c9164698c 100644 --- a/include/aikido/planner/ConfigurationToEndEffectorPose.hpp +++ b/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 #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_ diff --git a/include/aikido/planner/ConfigurationToTSR.hpp b/include/aikido/planner/dart/ConfigurationToTSR.hpp similarity index 80% rename from include/aikido/planner/ConfigurationToTSR.hpp rename to include/aikido/planner/dart/ConfigurationToTSR.hpp index 2553fe422f..8acc25634f 100644 --- a/include/aikido/planner/ConfigurationToTSR.hpp +++ b/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 #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_ diff --git a/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp b/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp index 3089293c62..5b948d654c 100644 --- a/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp +++ b/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,13 +80,13 @@ 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. @@ -94,10 +94,10 @@ class BodyNodePoseVectorField : public VectorField 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; diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp index b0e0bbc25e..c1e5dc5e03 100644 --- a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp +++ b/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, diff --git a/include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp b/include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp index 55eb4cd66a..b49552a966 100644 --- a/include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp +++ b/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. diff --git a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp index c1b5a6f389..22ffb802c2 100644 --- a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp +++ b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp @@ -57,8 +57,8 @@ std::unique_ptr followVectorField( std::unique_ptr 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 planToEndEffectorOffset( /// \return Trajectory or \c nullptr if planning failed. std::unique_ptr 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, diff --git a/include/aikido/planner/vectorfield/VectorFieldUtil.hpp b/include/aikido/planner/vectorfield/VectorFieldUtil.hpp index 5a3daafd9c..526d8d78d2 100644 --- a/include/aikido/planner/vectorfield/VectorFieldUtil.hpp +++ b/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, diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index 163b83c746..50f706302d 100644 --- a/src/planner/CMakeLists.txt +++ b/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}) diff --git a/src/planner/ConfigurationToEndEffectorOffset.cpp b/src/planner/dart/ConfigurationToEndEffectorOffset.cpp similarity index 91% rename from src/planner/ConfigurationToEndEffectorOffset.cpp rename to src/planner/dart/ConfigurationToEndEffectorOffset.cpp index b5e5a73d8e..11de7325a9 100644 --- a/src/planner/ConfigurationToEndEffectorOffset.cpp +++ b/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 diff --git a/src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp b/src/planner/dart/ConfigurationToEndEffectorOffsetPlanner.cpp similarity index 87% rename from src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp rename to src/planner/dart/ConfigurationToEndEffectorOffsetPlanner.cpp index 1ffb8c2439..59a550bd48 100644 --- a/src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp +++ b/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 diff --git a/src/planner/ConfigurationToEndEffectorPose.cpp b/src/planner/dart/ConfigurationToEndEffectorPose.cpp similarity index 89% rename from src/planner/ConfigurationToEndEffectorPose.cpp rename to src/planner/dart/ConfigurationToEndEffectorPose.cpp index 1696a24a85..5bfc20f37b 100644 --- a/src/planner/ConfigurationToEndEffectorPose.cpp +++ b/src/planner/dart/ConfigurationToEndEffectorPose.cpp @@ -1,14 +1,15 @@ -#include "aikido/planner/ConfigurationToEndEffectorPose.hpp" +#include "aikido/planner/dart/ConfigurationToEndEffectorPose.hpp" #include "aikido/constraint/Testable.hpp" namespace aikido { namespace planner { +namespace dart { //============================================================================== ConfigurationToEndEffectorPose::ConfigurationToEndEffectorPose( statespace::ConstStateSpacePtr stateSpace, - dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, const statespace::StateSpace::State* startState, const Eigen::Isometry3d& goalPose, constraint::ConstTestablePtr constraint) @@ -34,7 +35,7 @@ const std::string& ConfigurationToEndEffectorPose::getStaticType() } //============================================================================== -dart::dynamics::ConstBodyNodePtr +::dart::dynamics::ConstBodyNodePtr ConfigurationToEndEffectorPose::getEndEffectorBodyNode() const { return mEndEffectorBodyNode; @@ -53,5 +54,6 @@ const Eigen::Isometry3d& ConfigurationToEndEffectorPose::getGoalPose() const return mGoalPose; } +} // namespace dart } // namespace planner } // namespace aikido diff --git a/src/planner/ConfigurationToTSR.cpp b/src/planner/dart/ConfigurationToTSR.cpp similarity index 86% rename from src/planner/ConfigurationToTSR.cpp rename to src/planner/dart/ConfigurationToTSR.cpp index 5bd9db1654..969b9d5c47 100644 --- a/src/planner/ConfigurationToTSR.cpp +++ b/src/planner/dart/ConfigurationToTSR.cpp @@ -1,14 +1,15 @@ -#include "aikido/planner/ConfigurationToTSR.hpp" +#include "aikido/planner/dart/ConfigurationToTSR.hpp" #include "aikido/constraint/Testable.hpp" namespace aikido { namespace planner { +namespace dart { //============================================================================== ConfigurationToTSR::ConfigurationToTSR( statespace::ConstStateSpacePtr stateSpace, - dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, const statespace::StateSpace::State* startState, constraint::dart::ConstTSRPtr goalTSR, constraint::ConstTestablePtr constraint) @@ -34,7 +35,7 @@ const std::string& ConfigurationToTSR::getStaticType() } //============================================================================== -dart::dynamics::ConstBodyNodePtr ConfigurationToTSR::getEndEffectorBodyNode() +::dart::dynamics::ConstBodyNodePtr ConfigurationToTSR::getEndEffectorBodyNode() const { return mEndEffectorBodyNode; @@ -52,5 +53,6 @@ constraint::dart::ConstTSRPtr ConfigurationToTSR::getGoalTSR() const return mGoalTSR; } +} // namespace dart } // namespace planner } // namespace aikido diff --git a/src/planner/vectorfield/BodyNodePoseVectorField.cpp b/src/planner/vectorfield/BodyNodePoseVectorField.cpp index ec0adfeb8b..b8d46f33a6 100644 --- a/src/planner/vectorfield/BodyNodePoseVectorField.cpp +++ b/src/planner/vectorfield/BodyNodePoseVectorField.cpp @@ -11,8 +11,8 @@ namespace vectorfield { BodyNodePoseVectorField::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) @@ -134,20 +134,20 @@ BodyNodePoseVectorField::getMetaSkeletonStateSpace() const } //============================================================================== -dart::dynamics::MetaSkeletonPtr BodyNodePoseVectorField::getMetaSkeleton() +::dart::dynamics::MetaSkeletonPtr BodyNodePoseVectorField::getMetaSkeleton() { return mMetaSkeleton; } //============================================================================== -dart::dynamics::ConstMetaSkeletonPtr BodyNodePoseVectorField::getMetaSkeleton() - const +::dart::dynamics::ConstMetaSkeletonPtr +BodyNodePoseVectorField::getMetaSkeleton() const { return mMetaSkeleton; } //============================================================================== -dart::dynamics::ConstBodyNodePtr BodyNodePoseVectorField::getBodyNode() const +::dart::dynamics::ConstBodyNodePtr BodyNodePoseVectorField::getBodyNode() const { return mBodyNode; } diff --git a/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp b/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp index c76d722815..9bb022f452 100644 --- a/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp +++ b/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp @@ -13,8 +13,8 @@ namespace vectorfield { //============================================================================== MoveEndEffectorOffsetVectorField::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, @@ -50,7 +50,7 @@ MoveEndEffectorOffsetVectorField::MoveEndEffectorOffsetVectorField( bool MoveEndEffectorOffsetVectorField::evaluateCartesianVelocity( const Eigen::Isometry3d& pose, Eigen::Vector6d& cartesianVelocity) const { - using dart::math::logMap; + using ::dart::math::logMap; using aikido::planner::vectorfield::computeGeodesicError; Eigen::Vector6d desiredTwist = computeGeodesicTwist(pose, mStartPose); diff --git a/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp b/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp index 07fa954012..696bfff460 100644 --- a/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp @@ -10,7 +10,7 @@ namespace vectorfield { VectorFieldConfigurationToEndEffectorOffsetPlanner:: VectorFieldConfigurationToEndEffectorOffsetPlanner( statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, - dart::dynamics::MetaSkeletonPtr metaskeleton, + ::dart::dynamics::MetaSkeletonPtr metaskeleton, double distanceTolerance, double positionTolerance, double angularTolerance, diff --git a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp index 2101320770..e02d2f818f 100644 --- a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp +++ b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp @@ -1,7 +1,7 @@ #include #include #include -#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp" +#include "aikido/planner/dart/ConfigurationToEndEffectorOffset.hpp" #include #include #include @@ -315,7 +315,7 @@ TEST_F(VectorFieldPlannerTest, ComputeJointVelocityFromTwistTest) TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) { - using aikido::planner::ConfigurationToEndEffectorOffset; + using aikido::planner::dart::ConfigurationToEndEffectorOffset; using aikido::planner::vectorfield:: VectorFieldConfigurationToEndEffectorOffsetPlanner; @@ -420,7 +420,7 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) TEST_F(VectorFieldPlannerTest, DirectionZeroVector) { - using aikido::planner::ConfigurationToEndEffectorOffset; + using aikido::planner::dart::ConfigurationToEndEffectorOffset; using aikido::planner::vectorfield:: VectorFieldConfigurationToEndEffectorOffsetPlanner;