diff --git a/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp new file mode 100644 index 0000000000..2a37482970 --- /dev/null +++ b/include/aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp @@ -0,0 +1,53 @@ +#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/robot/util.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Trajectory.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 ConfigurationToTSRwithTrajectoryConstraint. + 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. + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + ::dart::dynamics::MetaSkeletonPtr metaSkeleton, + double timelimit, + robot::util::CRRTPlannerParameters crrtParameters); + + /// \copydoc SingleProblemPlanner::plan() + virtual trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr) final; + +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/dart/CMakeLists.txt b/src/planner/dart/CMakeLists.txt index 88bfcadacc..94a0287c66 100644 --- a/src/planner/dart/CMakeLists.txt +++ b/src/planner/dart/CMakeLists.txt @@ -23,6 +23,8 @@ set(sources ConfigurationToEndEffectorOffsetPlanner.cpp ConfigurationToTSR.cpp ConfigurationToTSRPlanner.cpp + ConfigurationToTSRwithTrajectoryConstraint.cpp + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp util.cpp ) diff --git a/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp new file mode 100644 index 0000000000..44d9c8b1d1 --- /dev/null +++ b/src/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.cpp @@ -0,0 +1,164 @@ +#include "aikido/planner/dart/CRRTConfigurationToTSRwithTrajectoryConstraintPlanner.hpp" + +#include + +#include "aikido/constraint.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/Planner.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::createTestableBounds; +using constraint::dart::FrameDifferentiable; +using constraint::dart::FrameTestable; +using constraint::dart::InverseKinematicsSampleable; +using constraint::dart::TSR; +using distance::createDistanceMetric; +using planner::ompl::planCRRTConnect; +using statespace::GeodesicInterpolator; +using statespace::dart::MetaSkeletonStateSaver; +using statespace::dart::MetaSkeletonStateSpace; + +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< + CRRTConfigurationToTSRwithTrajectoryConstraintPlanner, + ConfigurationToTSRwithTrajectoryConstraint>( + 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."); + } + + 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())); + + 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..1ea8b6b6ee --- /dev/null +++ b/src/planner/dart/ConfigurationToTSRwithTrajectoryConstraint.cpp @@ -0,0 +1,60 @@ +#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