From e86e8e728dd4d9579c56fa7d752697cabcea3e39 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 19:21:34 -0800 Subject: [PATCH 1/5] Add ConfigurationToTSRwithTrajectoryConstraint, CRRTConfigurationToTSRwithTrajectoryConstraintPlanner --- ...onToTSRwithTrajectoryConstraintPlanner.hpp | 58 +++++++ ...igurationToTSRwithTrajectoryConstraint.hpp | 68 ++++++++ src/planner/CMakeLists.txt | 2 + ...onToTSRwithTrajectoryConstraintPlanner.cpp | 160 ++++++++++++++++++ ...igurationToTSRwithTrajectoryConstraint.cpp | 55 ++++++ 5 files changed, 343 insertions(+) create mode 100644 include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp create mode 100644 include/aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp create mode 100644 src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp create mode 100644 src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp diff --git a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp new file mode 100644 index 0000000000..33de67da56 --- /dev/null +++ b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp @@ -0,0 +1,58 @@ +#ifndef AIKIDO_PLANNER_DART_CRRTCONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINTPLANNER_HPP_ +#define AIKIDO_PLANNER_DART_CRRTCONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINTPLANNER_HPP_ + +#include "aikido/planner/dart/ConfigurationToTSRPlanner.hpp" +#include "aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Trajectory.hpp" +#include "aikido/robot/util.hpp" + +namespace aikido { +namespace planner { +namespace dart { + +class CRRTConfigurationToTSRwithTrajectoryConstraintPlanner + : public dart::SingleProblemPlanner< + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner, + ConfigurationToTSRwithTrajectoryConstraint> +{ +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 ConfigurationToTSR. + using SingleProblemPlanner::plan; + + /// Constructor + /// + /// \param[in] stateSpace State space that this planner associated with. + /// \param[in] metaSkeleton MetaSkeleton to use for planning. + /// \param[in] timelimit Timelimit for planning. + /// \param[in] crrtParameters CRRT planner parameters. + /// \param[in] configurationRanker Ranker for goalTSR. + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::MetaSkeletonPtr metaSkeleton, + double timelimit, + robot::util::CRRTPlannerParameters crrtParameters); + + /// 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. + trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr); + // Note: SolvableProblem is defined in SingleProblemPlanner. + +protected: + const robot::util::CRRTPlannerParameters mCRRTParameters; + + // Planning timelimit + const double mTimelimit; +}; + +} // namespace dart +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_DART_CRRTCONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINTPLANNER_HPP_ diff --git a/include/aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp b/include/aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp new file mode 100644 index 0000000000..d7ad94ba6d --- /dev/null +++ b/include/aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp @@ -0,0 +1,68 @@ +#ifndef AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINT_HPP_ +#define AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINT_HPP_ + +#include "aikido/planner/dart/ConfigurationToTSR.hpp" + +namespace aikido { +namespace planner { +namespace dart { + +/// Planning problem to plan to a given single Task Space Region (TSR). +class ConfigurationToTSRwithTrajectoryConstraint : public ConfigurationToTSR +{ +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] endEffectorBodyNode BodyNode to be planned to move to a desired + /// TSR. + /// \param[in] goalTSR Goal TSR. + /// \param[in] constraintTsr The constraint TSR for the trajectory. + /// \param[in] constraint Trajectory-wide constraint that must be satisfied. + /// \throw If \c stateSpace is not compatible with \c constraint's state + /// space. + ConfigurationToTSRwithTrajectoryConstraint( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + constraint::dart::ConstTSRPtr goalTSR, + constraint::dart::ConstTSRPtr constraintTSR, + 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] endEffectorBodyNode BodyNode to be planned to move to a desired + /// TSR. + /// \param[in] maxSamples Maximum number of TSR samples to plan to. + /// \param[in] goalTSR Goal TSR. + /// \param[in] constraintTsr The constraint TSR for the trajectory + /// \param[in] constraint Trajectory-wide constraint that must be satisfied. + /// \throw If \c stateSpace is not compatible with \c constraint's state + /// space. + ConfigurationToTSRwithTrajectoryConstraint( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + const statespace::dart::MetaSkeletonStateSpace::State* startState, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + constraint::dart::ConstTSRPtr goalTSR, + constraint::dart::ConstTSRPtr constraintTSR, + constraint::ConstTestablePtr constraint = nullptr); + + /// Return the trajectory constraint TSR + constraint::dart::ConstTSRPtr getConstraintTSR() const; + +protected: + /// Constraint TSR + const constraint::dart::ConstTSRPtr mConstraintTSR; +}; + +} // namespace dart +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_DART_CONFIGURATIONTOTSRWITHTRAJECTORYCONSTRAINT_HPP_ diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index 8358a7b6a6..952d116f9c 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -21,6 +21,8 @@ set(sources dart/ConfigurationToEndEffectorOffsetPlanner.cpp # dart/ConfigurationToEndEffectorPosePlanner.cpp dart/ConfigurationToTSRPlanner.cpp + dart/ConfigurationToTSRwithTrajectoryConstraint.cpp + dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp dart/util.cpp diff --git a/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp new file mode 100644 index 0000000000..4b6dcebac5 --- /dev/null +++ b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp @@ -0,0 +1,160 @@ +#include "aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp" + +#include + +#include "aikido/constraint.hpp" +#include "aikido/constraint/Testable.hpp" +#include "aikido/constraint/CyclicSampleable.hpp" +#include "aikido/constraint/FiniteSampleable.hpp" +#include "aikido/constraint/NewtonsMethodProjectable.hpp" +#include "aikido/constraint/TestableIntersection.hpp" +#include "aikido/constraint/dart/InverseKinematicsSampleable.hpp" +#include "aikido/distance/defaults.hpp" +#include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/statespace/GeodesicInterpolator.hpp" +#include "aikido/statespace/StateSpace.hpp" + +namespace aikido { +namespace planner { +namespace dart { + +using constraint::CyclicSampleable; +using constraint::Sampleable; +using constraint::dart::createSampleableBounds; +using constraint::dart::FrameDifferentiable; +using constraint::dart::InverseKinematicsSampleable; +using constraint::dart::createTestableBounds; +using constraint::dart::createProjectableBounds; +using constraint::dart::FrameDifferentiable; +using constraint::dart::FrameTestable; +using constraint::dart::TSR; +using constraint::Testable; +using constraint::NewtonsMethodProjectable; +using distance::createDistanceMetric; +using statespace::dart::MetaSkeletonStateSpace; +using statespace::dart::MetaSkeletonStateSaver; +using statespace::GeodesicInterpolator; +using planner::ompl::planCRRTConnect; + +using ::dart::dynamics::InverseKinematics; +using ::dart::dynamics::BodyNode; + +//============================================================================== +CRRTConfigurationToTSRwithTrajectoryConstraintPlanner:: + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::MetaSkeletonPtr metaSkeleton, + double timelimit, + robot::util::CRRTPlannerParameters crrtParameters) + : dart::SingleProblemPlanner( + std::move(stateSpace), std::move(metaSkeleton)) + , mCRRTParameters(std::move(crrtParameters)) + , mTimelimit(timelimit) +{ + // Do nothing +} + +//============================================================================== +trajectory::TrajectoryPtr CRRTConfigurationToTSRwithTrajectoryConstraintPlanner::plan( + const SolvableProblem& problem, Result* /*result*/) +{ + std::size_t projectionMaxIteration = mCRRTParameters.projectionMaxIteration; + double projectionTolerance = mCRRTParameters.projectionTolerance; + + auto robot = mMetaSkeleton->getBodyNode(0)->getSkeleton(); + std::lock_guard lock(robot->getMutex()); + + // Save the current state of the stateSpace + auto saver = MetaSkeletonStateSaver(mMetaSkeleton); + DART_UNUSED(saver); + + // Create seed constraint + std::shared_ptr seedConstraint + = createSampleableBounds(mMetaSkeletonStateSpace, mCRRTParameters.rng->clone()); + + // TODO: DART may be updated to check for single skeleton + if (mMetaSkeleton->getNumDofs() == 0) + throw std::invalid_argument("MetaSkeleton has 0 degrees of freedom."); + + auto skeleton = mMetaSkeleton->getDof(0)->getSkeleton(); + for (size_t i = 1; i < mMetaSkeleton->getNumDofs(); ++i) + { + if (mMetaSkeleton->getDof(i)->getSkeleton() != skeleton) + throw std::invalid_argument("MetaSkeleton has more than 1 skeleton."); + } + + const auto bodyNode = problem.getEndEffectorBodyNode(); + const auto goalTsr = problem.getGoalTSR(); + const auto constraintTsr = problem.getConstraintTSR(); + + // Create an IK solver with metaSkeleton dofs + auto ik = InverseKinematics::create(const_cast(bodyNode.get())); + + ik->setDofs(mMetaSkeleton->getDofs()); + + // create goal sampleable + auto goalSampleable = std::make_shared( + mMetaSkeletonStateSpace, + mMetaSkeleton, + std::make_shared(std::const_pointer_cast(goalTsr)), + seedConstraint, + ik, + mCRRTParameters.maxNumTrials); + + // create goal testable + auto goalTestable = std::make_shared( + std::const_pointer_cast(mMetaSkeletonStateSpace), + mMetaSkeleton, bodyNode.get(), + std::const_pointer_cast(goalTsr)); + + // create constraint sampleable + auto constraintSampleable = std::make_shared( + mMetaSkeletonStateSpace, + mMetaSkeleton, + std::const_pointer_cast(constraintTsr), + seedConstraint, + ik, + mCRRTParameters.maxNumTrials); + + // create constraint projectable + auto frameDiff = std::make_shared( + std::const_pointer_cast(mMetaSkeletonStateSpace), + mMetaSkeleton, + bodyNode.get(), + std::const_pointer_cast(constraintTsr)); + + std::vector projectionToleranceVec( + frameDiff->getConstraintDimension(), projectionTolerance); + auto constraintProjectable = std::make_shared( + frameDiff, projectionToleranceVec, projectionMaxIteration); + + // Current state + auto startState = mMetaSkeletonStateSpace->getScopedStateFromMetaSkeleton(mMetaSkeleton.get()); + + // Call planner + auto traj = planCRRTConnect( + startState, + goalTestable, + goalSampleable, + constraintProjectable, + mMetaSkeletonStateSpace, + std::make_shared(mMetaSkeletonStateSpace), + createDistanceMetric(mMetaSkeletonStateSpace), + constraintSampleable, + std::const_pointer_cast(problem.getConstraint()), + createTestableBounds(mMetaSkeletonStateSpace), + createProjectableBounds(mMetaSkeletonStateSpace), + mTimelimit, + mCRRTParameters.maxExtensionDistance, + mCRRTParameters.maxDistanceBtwProjections, + mCRRTParameters.minStepSize, + mCRRTParameters.minTreeConnectionDistance); + + return traj; +} + +} // namespace dart +} // namespace planner +} // namespace aikido diff --git a/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp b/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp new file mode 100644 index 0000000000..28ba7afa9e --- /dev/null +++ b/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp @@ -0,0 +1,55 @@ +#include "aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp" + +#include "aikido/constraint/Testable.hpp" + +namespace aikido { +namespace planner { +namespace dart { + +//============================================================================== +ConfigurationToTSRwithTrajectoryConstraint::ConfigurationToTSRwithTrajectoryConstraint( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + constraint::dart::ConstTSRPtr goalTSR, + constraint::dart::ConstTSRPtr constraintTSR, + constraint::ConstTestablePtr constraint) + : ConfigurationToTSR( + std::move(stateSpace), + std::move(metaSkeleton), + std::move(endEffectorBodyNode), + 0, // This parameter is not used in this class. + std::move(goalTSR), + std::move(constraint)) + , mConstraintTSR(constraintTSR) +{ + // Do nothing. +} + +//============================================================================== +ConfigurationToTSRwithTrajectoryConstraint::ConfigurationToTSRwithTrajectoryConstraint( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + const statespace::dart::MetaSkeletonStateSpace::State* startState, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + constraint::dart::ConstTSRPtr goalTSR, + constraint::dart::ConstTSRPtr constraintTSR, + constraint::ConstTestablePtr constraint) + : ConfigurationToTSR( + std::move(stateSpace), startState, + std::move(endEffectorBodyNode), + 0, // This parameter is not used in this class. + std::move(goalTSR), std::move(constraint)) + , mConstraintTSR(constraintTSR) +{ + // Do nothing. +} + +//============================================================================== +constraint::dart::ConstTSRPtr ConfigurationToTSRwithTrajectoryConstraint::getConstraintTSR() const +{ + return mConstraintTSR; +} + +} // namespace dart +} // namespace planner +} // namespace aikido From 1afc910f550d876651c8a858653c4f781b72d3d9 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 20:11:59 -0800 Subject: [PATCH 2/5] Update docstring --- ...ConfigurationToTSRwithTrajectoryConstraintPlanner.hpp | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp index 33de67da56..2f681d730f 100644 --- a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp +++ b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp @@ -29,20 +29,15 @@ class CRRTConfigurationToTSRwithTrajectoryConstraintPlanner /// \param[in] metaSkeleton MetaSkeleton to use for planning. /// \param[in] timelimit Timelimit for planning. /// \param[in] crrtParameters CRRT planner parameters. - /// \param[in] configurationRanker Ranker for goalTSR. CRRTConfigurationToTSRwithTrajectoryConstraintPlanner( statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, ::dart::dynamics::MetaSkeletonPtr metaSkeleton, double timelimit, robot::util::CRRTPlannerParameters crrtParameters); - /// 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. + /// \copydoc SingleProblemPlanner::plan() trajectory::TrajectoryPtr plan( - const SolvableProblem& problem, Result* result = nullptr); - // Note: SolvableProblem is defined in SingleProblemPlanner. + const SolvableProblem& problem, Result* result = nullptr) override; protected: const robot::util::CRRTPlannerParameters mCRRTParameters; From 3b9153b8e40de73f609f17cf7bc4384e73918eb4 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 20:13:00 -0800 Subject: [PATCH 3/5] Clang format --- ...onToTSRwithTrajectoryConstraintPlanner.hpp | 2 +- ...onToTSRwithTrajectoryConstraintPlanner.cpp | 48 ++++++++-------- ...igurationToTSRwithTrajectoryConstraint.cpp | 55 ++++++++++--------- 3 files changed, 57 insertions(+), 48 deletions(-) diff --git a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp index 2f681d730f..a13a23b26f 100644 --- a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp +++ b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp @@ -3,9 +3,9 @@ #include "aikido/planner/dart/ConfigurationToTSRPlanner.hpp" #include "aikido/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.hpp" +#include "aikido/robot/util.hpp" #include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" #include "aikido/trajectory/Trajectory.hpp" -#include "aikido/robot/util.hpp" namespace aikido { namespace planner { diff --git a/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp index 4b6dcebac5..53f0e0407a 100644 --- a/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp +++ b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp @@ -3,52 +3,53 @@ #include #include "aikido/constraint.hpp" -#include "aikido/constraint/Testable.hpp" #include "aikido/constraint/CyclicSampleable.hpp" #include "aikido/constraint/FiniteSampleable.hpp" #include "aikido/constraint/NewtonsMethodProjectable.hpp" +#include "aikido/constraint/Testable.hpp" #include "aikido/constraint/TestableIntersection.hpp" #include "aikido/constraint/dart/InverseKinematicsSampleable.hpp" #include "aikido/distance/defaults.hpp" #include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp" -#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp" -#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" #include "aikido/statespace/GeodesicInterpolator.hpp" #include "aikido/statespace/StateSpace.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" namespace aikido { namespace planner { namespace dart { using constraint::CyclicSampleable; +using constraint::NewtonsMethodProjectable; using constraint::Sampleable; +using constraint::Testable; +using constraint::dart::createProjectableBounds; using constraint::dart::createSampleableBounds; -using constraint::dart::FrameDifferentiable; -using constraint::dart::InverseKinematicsSampleable; using constraint::dart::createTestableBounds; -using constraint::dart::createProjectableBounds; using constraint::dart::FrameDifferentiable; using constraint::dart::FrameTestable; +using constraint::dart::InverseKinematicsSampleable; using constraint::dart::TSR; -using constraint::Testable; -using constraint::NewtonsMethodProjectable; using distance::createDistanceMetric; -using statespace::dart::MetaSkeletonStateSpace; -using statespace::dart::MetaSkeletonStateSaver; -using statespace::GeodesicInterpolator; using planner::ompl::planCRRTConnect; +using statespace::GeodesicInterpolator; +using statespace::dart::MetaSkeletonStateSaver; +using statespace::dart::MetaSkeletonStateSpace; -using ::dart::dynamics::InverseKinematics; using ::dart::dynamics::BodyNode; +using ::dart::dynamics::InverseKinematics; //============================================================================== CRRTConfigurationToTSRwithTrajectoryConstraintPlanner:: CRRTConfigurationToTSRwithTrajectoryConstraintPlanner( - statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, - ::dart::dynamics::MetaSkeletonPtr metaSkeleton, - double timelimit, - robot::util::CRRTPlannerParameters crrtParameters) - : dart::SingleProblemPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::MetaSkeletonPtr metaSkeleton, + double timelimit, + robot::util::CRRTPlannerParameters crrtParameters) + : dart::SingleProblemPlanner< + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner, + ConfigurationToTSRwithTrajectoryConstraint>( std::move(stateSpace), std::move(metaSkeleton)) , mCRRTParameters(std::move(crrtParameters)) , mTimelimit(timelimit) @@ -57,7 +58,8 @@ CRRTConfigurationToTSRwithTrajectoryConstraintPlanner:: } //============================================================================== -trajectory::TrajectoryPtr CRRTConfigurationToTSRwithTrajectoryConstraintPlanner::plan( +trajectory::TrajectoryPtr +CRRTConfigurationToTSRwithTrajectoryConstraintPlanner::plan( const SolvableProblem& problem, Result* /*result*/) { std::size_t projectionMaxIteration = mCRRTParameters.projectionMaxIteration; @@ -71,8 +73,8 @@ trajectory::TrajectoryPtr CRRTConfigurationToTSRwithTrajectoryConstraintPlanner: DART_UNUSED(saver); // Create seed constraint - std::shared_ptr seedConstraint - = createSampleableBounds(mMetaSkeletonStateSpace, mCRRTParameters.rng->clone()); + std::shared_ptr seedConstraint = createSampleableBounds( + mMetaSkeletonStateSpace, mCRRTParameters.rng->clone()); // TODO: DART may be updated to check for single skeleton if (mMetaSkeleton->getNumDofs() == 0) @@ -106,7 +108,8 @@ trajectory::TrajectoryPtr CRRTConfigurationToTSRwithTrajectoryConstraintPlanner: // create goal testable auto goalTestable = std::make_shared( std::const_pointer_cast(mMetaSkeletonStateSpace), - mMetaSkeleton, bodyNode.get(), + mMetaSkeleton, + bodyNode.get(), std::const_pointer_cast(goalTsr)); // create constraint sampleable @@ -131,7 +134,8 @@ trajectory::TrajectoryPtr CRRTConfigurationToTSRwithTrajectoryConstraintPlanner: frameDiff, projectionToleranceVec, projectionMaxIteration); // Current state - auto startState = mMetaSkeletonStateSpace->getScopedStateFromMetaSkeleton(mMetaSkeleton.get()); + auto startState = mMetaSkeletonStateSpace->getScopedStateFromMetaSkeleton( + mMetaSkeleton.get()); // Call planner auto traj = planCRRTConnect( diff --git a/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp b/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp index 28ba7afa9e..1ea8b6b6ee 100644 --- a/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp +++ b/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp @@ -7,45 +7,50 @@ namespace planner { namespace dart { //============================================================================== -ConfigurationToTSRwithTrajectoryConstraint::ConfigurationToTSRwithTrajectoryConstraint( - statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, - ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton, - ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, - constraint::dart::ConstTSRPtr goalTSR, - constraint::dart::ConstTSRPtr constraintTSR, - constraint::ConstTestablePtr constraint) +ConfigurationToTSRwithTrajectoryConstraint:: + ConfigurationToTSRwithTrajectoryConstraint( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::ConstMetaSkeletonPtr metaSkeleton, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + constraint::dart::ConstTSRPtr goalTSR, + constraint::dart::ConstTSRPtr constraintTSR, + constraint::ConstTestablePtr constraint) : ConfigurationToTSR( - std::move(stateSpace), - std::move(metaSkeleton), - std::move(endEffectorBodyNode), - 0, // This parameter is not used in this class. - std::move(goalTSR), - std::move(constraint)) + std::move(stateSpace), + std::move(metaSkeleton), + std::move(endEffectorBodyNode), + 0, // This parameter is not used in this class. + std::move(goalTSR), + std::move(constraint)) , mConstraintTSR(constraintTSR) { // Do nothing. } //============================================================================== -ConfigurationToTSRwithTrajectoryConstraint::ConfigurationToTSRwithTrajectoryConstraint( - statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, - const statespace::dart::MetaSkeletonStateSpace::State* startState, - ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, - constraint::dart::ConstTSRPtr goalTSR, - constraint::dart::ConstTSRPtr constraintTSR, - constraint::ConstTestablePtr constraint) +ConfigurationToTSRwithTrajectoryConstraint:: + ConfigurationToTSRwithTrajectoryConstraint( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + const statespace::dart::MetaSkeletonStateSpace::State* startState, + ::dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, + constraint::dart::ConstTSRPtr goalTSR, + constraint::dart::ConstTSRPtr constraintTSR, + constraint::ConstTestablePtr constraint) : ConfigurationToTSR( - std::move(stateSpace), startState, - std::move(endEffectorBodyNode), - 0, // This parameter is not used in this class. - std::move(goalTSR), std::move(constraint)) + std::move(stateSpace), + startState, + std::move(endEffectorBodyNode), + 0, // This parameter is not used in this class. + std::move(goalTSR), + std::move(constraint)) , mConstraintTSR(constraintTSR) { // Do nothing. } //============================================================================== -constraint::dart::ConstTSRPtr ConfigurationToTSRwithTrajectoryConstraint::getConstraintTSR() const +constraint::dart::ConstTSRPtr +ConfigurationToTSRwithTrajectoryConstraint::getConstraintTSR() const { return mConstraintTSR; } From ca458ee899a593867262d1d857f2f8d87530a190 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 20:26:57 -0800 Subject: [PATCH 4/5] Remove override from CRRTConfigurationToTSR...Planner --- .../CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp index a13a23b26f..f71b6b9e42 100644 --- a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp +++ b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp @@ -20,7 +20,7 @@ class CRRTConfigurationToTSRwithTrajectoryConstraintPlanner // 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 ConfigurationToTSR. + // which is simply ConfigurationToTSRwithTrajectoryConstraint. using SingleProblemPlanner::plan; /// Constructor @@ -37,7 +37,7 @@ class CRRTConfigurationToTSRwithTrajectoryConstraintPlanner /// \copydoc SingleProblemPlanner::plan() trajectory::TrajectoryPtr plan( - const SolvableProblem& problem, Result* result = nullptr) override; + const SolvableProblem& problem, Result* result = nullptr); protected: const robot::util::CRRTPlannerParameters mCRRTParameters; From 34599662ffda3d4c25bf99c276c719c9e7610209 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 25 Feb 2020 21:44:43 -0800 Subject: [PATCH 5/5] Add planner/dart/CMakeLists, add planner_dart component, update relevant CMakeLists --- ...onToTSRwithTrajectoryConstraintPlanner.hpp | 4 +- src/planner/CMakeLists.txt | 14 +--- src/planner/dart/CMakeLists.txt | 64 +++++++++++++++++++ ...onToTSRwithTrajectoryConstraintPlanner.cpp | 8 +-- src/planner/vectorfield/CMakeLists.txt | 1 + tests/planner/CMakeLists.txt | 3 +- 6 files changed, 74 insertions(+), 20 deletions(-) create mode 100644 src/planner/dart/CMakeLists.txt diff --git a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp index f71b6b9e42..2a37482970 100644 --- a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp +++ b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp @@ -36,8 +36,8 @@ class CRRTConfigurationToTSRwithTrajectoryConstraintPlanner robot::util::CRRTPlannerParameters crrtParameters); /// \copydoc SingleProblemPlanner::plan() - trajectory::TrajectoryPtr plan( - const SolvableProblem& problem, Result* result = nullptr); + virtual trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr) final; protected: const robot::util::CRRTPlannerParameters mCRRTParameters; diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index 952d116f9c..f572d8fbae 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -13,19 +13,6 @@ set(sources SequenceMetaPlanner.cpp World.cpp WorldStateSaver.cpp - dart/ConfigurationToConfiguration.cpp - dart/ConfigurationToConfigurationPlanner.cpp - dart/ConfigurationToEndEffectorOffset.cpp - dart/ConfigurationToEndEffectorPose.cpp - dart/ConfigurationToTSR.cpp - dart/ConfigurationToEndEffectorOffsetPlanner.cpp - # dart/ConfigurationToEndEffectorPosePlanner.cpp - dart/ConfigurationToTSRPlanner.cpp - dart/ConfigurationToTSRwithTrajectoryConstraint.cpp - dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp - dart/ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp - dart/ConfigurationToConfiguration_to_ConfigurationToTSR.cpp - dart/util.cpp ) add_library("${PROJECT_NAME}_planner" SHARED ${sources}) @@ -61,6 +48,7 @@ add_component_dependencies(${PROJECT_NAME} planner clang_format_add_sources(${sources}) add_subdirectory("ompl") # [constraint], [distance], [statespace], [trajectory], dart, ompl +add_subdirectory("dart") # [constraint], [distance], [trajectory], [statespace], [planner_ompl], dart, ompl add_subdirectory("parabolic") # [external], [common], [trajectory], [statespace], dart add_subdirectory("vectorfield") # [common], [trajectory], [statespace], dart add_subdirectory("kunzretimer") # [external], [common], [trajectory], [statespace], dart diff --git a/src/planner/dart/CMakeLists.txt b/src/planner/dart/CMakeLists.txt new file mode 100644 index 0000000000..4243468821 --- /dev/null +++ b/src/planner/dart/CMakeLists.txt @@ -0,0 +1,64 @@ +if(CMAKE_COMPILER_IS_GNUCXX) + if(OMPL_VERSION VERSION_GREATER 1.2.0 OR OMPL_VERSION VERSION_EQUAL 1.2.0) + if(Boost_VERSION VERSION_LESS 106501) + message(STATUS "OMPL planners are disabled for OMPL (>=1.2.0) + GCC + " + "Boost (< 1.65.1). For details, please see: " + " https://github.com/personalrobotics/aikido/issues/363" + ) + return() + endif() + endif() +endif() + +#============================================================================== +# Libraries +# +set(sources + ConfigurationToConfiguration.cpp + ConfigurationToConfigurationPlanner.cpp + ConfigurationToEndEffectorOffset.cpp + ConfigurationToEndEffectorPose.cpp + ConfigurationToTSR.cpp + ConfigurationToEndEffectorOffsetPlanner.cpp + ConfigurationToTSRPlanner.cpp + ConfigurationToTSRwithTrajectoryConstraint.cpp + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp + ConfigurationToConfiguration_to_ConfigurationToConfiguration.cpp + ConfigurationToConfiguration_to_ConfigurationToTSR.cpp + util.cpp +) + +add_library("${PROJECT_NAME}_planner_dart" SHARED ${sources}) +target_include_directories("${PROJECT_NAME}_planner_dart" SYSTEM + PUBLIC + ${DART_INCLUDE_DIRS} + ${OMPL_INCLUDE_DIRS} +) +target_link_libraries("${PROJECT_NAME}_planner_dart" + PUBLIC + "${PROJECT_NAME}_common" + "${PROJECT_NAME}_constraint" + "${PROJECT_NAME}_distance" + "${PROJECT_NAME}_trajectory" + "${PROJECT_NAME}_statespace" + "${PROJECT_NAME}_planner" + "${PROJECT_NAME}_planner_ompl" + ${DART_LIBRARIES} + ${OMPL_LIBRARIES} +) +target_compile_options("${PROJECT_NAME}_planner_dart" + PUBLIC ${AIKIDO_CXX_STANDARD_FLAGS} +) + +add_component(${PROJECT_NAME} planner_dart) +add_component_targets(${PROJECT_NAME} planner_dart "${PROJECT_NAME}_planner_dart") +add_component_dependencies(${PROJECT_NAME} planner_dart + constraint + distance + planner + planner_ompl + statespace + trajectory +) + +clang_format_add_sources(${sources}) diff --git a/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp index 53f0e0407a..44d9c8b1d1 100644 --- a/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp +++ b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp @@ -10,7 +10,7 @@ #include "aikido/constraint/TestableIntersection.hpp" #include "aikido/constraint/dart/InverseKinematicsSampleable.hpp" #include "aikido/distance/defaults.hpp" -#include "aikido/planner/ompl/OMPLConfigurationToConfigurationPlanner.hpp" +#include "aikido/planner/ompl/Planner.hpp" #include "aikido/statespace/GeodesicInterpolator.hpp" #include "aikido/statespace/StateSpace.hpp" #include "aikido/statespace/dart/MetaSkeletonStateSaver.hpp" @@ -87,9 +87,9 @@ CRRTConfigurationToTSRwithTrajectoryConstraintPlanner::plan( throw std::invalid_argument("MetaSkeleton has more than 1 skeleton."); } - const auto bodyNode = problem.getEndEffectorBodyNode(); - const auto goalTsr = problem.getGoalTSR(); - const auto constraintTsr = problem.getConstraintTSR(); + auto bodyNode = problem.getEndEffectorBodyNode(); + auto goalTsr = problem.getGoalTSR(); + auto constraintTsr = problem.getConstraintTSR(); // Create an IK solver with metaSkeleton dofs auto ik = InverseKinematics::create(const_cast(bodyNode.get())); diff --git a/src/planner/vectorfield/CMakeLists.txt b/src/planner/vectorfield/CMakeLists.txt index 74ce82c4d6..deac97111e 100644 --- a/src/planner/vectorfield/CMakeLists.txt +++ b/src/planner/vectorfield/CMakeLists.txt @@ -19,6 +19,7 @@ target_link_libraries("${PROJECT_NAME}_planner_vectorfield" "${PROJECT_NAME}_trajectory" "${PROJECT_NAME}_statespace" "${PROJECT_NAME}_planner" + "${PROJECT_NAME}_planner_dart" ${DART_LIBRARIES} ${Boost_LIBRARIES} ) diff --git a/tests/planner/CMakeLists.txt b/tests/planner/CMakeLists.txt index 9e5e213871..eb6f7e69b6 100644 --- a/tests/planner/CMakeLists.txt +++ b/tests/planner/CMakeLists.txt @@ -15,7 +15,8 @@ target_link_libraries(test_DartPlanners "${PROJECT_NAME}_constraint" "${PROJECT_NAME}_distance" "${PROJECT_NAME}_trajectory" - "${PROJECT_NAME}_planner") + "${PROJECT_NAME}_planner" + "${PROJECT_NAME}_planner_dart") aikido_add_test(test_World test_World.cpp) target_link_libraries(test_World