diff --git a/CHANGELOG.md b/CHANGELOG.md index f9e8592061..428be699da 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) + * 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) * 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/ConfigurationToEndEffectorOffset.hpp index fc12671825..7df7c097df 100644 --- a/include/aikido/planner/ConfigurationToEndEffectorOffset.hpp +++ b/include/aikido/planner/ConfigurationToEndEffectorOffset.hpp @@ -4,7 +4,7 @@ #include #include "aikido/constraint/Testable.hpp" #include "aikido/planner/Problem.hpp" -#include "aikido/statespace/StateSpace.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" #include "aikido/trajectory/Interpolated.hpp" namespace aikido { @@ -29,9 +29,9 @@ class ConfigurationToEndEffectorOffset : public Problem /// \param[in] constraint Trajectory-wide constraint that must be satisfied. /// \throw If the size of \c direction is zero. ConfigurationToEndEffectorOffset( - statespace::ConstStateSpacePtr stateSpace, + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, - const statespace::StateSpace::State* startState, + const statespace::dart::MetaSkeletonStateSpace::State* startState, const Eigen::Vector3d& direction, double signedDistance, constraint::ConstTestablePtr constraint); @@ -47,7 +47,7 @@ class ConfigurationToEndEffectorOffset : public Problem dart::dynamics::ConstBodyNodePtr getEndEffectorBodyNode() const; /// Returns the start state. - const statespace::StateSpace::State* getStartState() const; + const statespace::dart::MetaSkeletonStateSpace::State* getStartState() const; /// Returns the direction of motion specified in the world frame. /// @@ -62,7 +62,7 @@ class ConfigurationToEndEffectorOffset : public Problem const dart::dynamics::ConstBodyNodePtr mEndEffectorBodyNode; /// Start state. - const statespace::StateSpace::State* mStartState; + const statespace::dart::MetaSkeletonStateSpace::State* mStartState; /// Direction of motion. const Eigen::Vector3d mDirection; diff --git a/include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp b/include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp new file mode 100644 index 0000000000..86556d2a0b --- /dev/null +++ b/include/aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp @@ -0,0 +1,52 @@ +#ifndef AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ +#define AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ + +#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp" +#include "aikido/planner/SingleProblemPlanner.hpp" +#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp" +#include "aikido/trajectory/Trajectory.hpp" + +namespace aikido { +namespace planner { + +/// Base planner class for ConfigurationToEndEffectorOffset planning problem. +class ConfigurationToEndEffectorOffsetPlanner + : public SingleProblemPlanner +{ +public: + // Expose the implementation of Planner::plan(const Problem&, Result*) in + // SingleProblemPlanner. Note that plan() of the base class takes Problem + // while the virtual function defined in this class takes SolverbleProblem, + // which is simply ConfigurationToEndEffectorOffset. + using SingleProblemPlanner::plan; + + /// Constructor + /// + /// \param[in] stateSpace State space that this planner associated with. + explicit ConfigurationToEndEffectorOffsetPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace); + + /// Solves \c problem returning the result to \c result. + /// + /// \param[in] problem Planning problem to be solved by the planner. + /// \param[out] result Result of planning procedure. + virtual trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr) + = 0; + // Note: SolvableProblem is defined in SingleProblemPlanner. + + /// Return ConstMetaSkeletonStateSpacePtr by performing a static cast on + /// mStateSpace. + statespace::dart::ConstMetaSkeletonStateSpacePtr getMetaSkeletonStateSpace(); + +protected: + /// Stores stateSpace pointer as aConstMetaSkeletonStateSpacePtr. Prevents + /// use of an expensive dynamic cast. + statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace; +}; + +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_CONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ diff --git a/include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp b/include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp new file mode 100644 index 0000000000..55eb4cd66a --- /dev/null +++ b/include/aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp @@ -0,0 +1,92 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ + +#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp" +#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp" + +namespace aikido { +namespace planner { +namespace vectorfield { + +/// Planner that generates a trajectory that moves the end-effector by a given +/// direction and distance. +class VectorFieldConfigurationToEndEffectorOffsetPlanner + : public ConfigurationToEndEffectorOffsetPlanner +{ +public: + /// Constructor. + /// + /// \param[in] stateSpace State space that this planner is associated with. + /// \param[in] metaskeleton MetaSkeleton to plan with. + /// \param[in] distanceTolerance How much a planned trajectory is allowed to + /// deviate from the requested distance to move the end-effector. + /// \param[in] positionTolerance How a planned trajectory is allowed to + /// deviated from a straight line segment defined by the direction and the + /// distance. + /// \param[in] angularTolerance How a planned trajectory is allowed to deviate + /// from a given direction. + /// \param[in] initialStepSize Initial step size. + /// \param[in] jointLimitTolerance If less then this distance to joint + /// limit, velocity is bounded in that direction to 0. + /// \param[in] constraintCheckResolution Resolution used in constraint + /// checking. + /// \param[in] timelimit timeout in seconds. + VectorFieldConfigurationToEndEffectorOffsetPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::MetaSkeletonPtr metaskeleton, + double distanceTolerance, + double positionTolerance, + double angularTolerance, + double initialStepSize, + double jointLimitTolerance, + double constraintCheckResolution, + std::chrono::duration timelimit); + + /// Plan to a trajectory that moves the end-effector by a given direction and + /// distance. + /// + /// The planner returns success if the resulting trajectory satisfies + /// constraint at some resolution and failure (returning \c nullptr) + /// otherwise. The reason for the failure is stored in the \c result output + /// parameter. + /// + /// \param[in] problem Planning problem. + /// \param[out] result Information about success or failure. + /// \return Trajectory or \c nullptr if planning failed. + trajectory::TrajectoryPtr plan( + const SolvableProblem& problem, Result* result = nullptr) override; + +protected: + /// MetaSkeleton to plan with. + dart::dynamics::MetaSkeletonPtr mMetaskeleton; + + /// How much a planned trajectory is allowed to deviate from the requested + /// distance to move the end-effector. + double mDistanceTolerance; + + /// How a planned trajectory is allowed to deviated from a straight line + /// segment defined by the direction and the distance. + double mPositionTolerance; + + /// How a planned trajectory is allowed to deviate from a given direction. + double mAngularTolerance; + + /// Initial step size. + double mInitialStepSize; + + /// If less then this distance to joint limit, velocity is bounded in that + /// direction to 0. + double mJointLimitTolerance; + + /// Resolution used in constraint checking. + double mConstraintCheckResolution; + + /// Timeout in seconds. + std::chrono::duration mTimelimit; +}; + +} // namespace vectorfield +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDCONFIGURATIONTOENDEFFECTOROFFSETPLANNER_HPP_ diff --git a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp index d6ab18ca0c..c1b5a6f389 100644 --- a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp +++ b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp @@ -55,6 +55,7 @@ std::unique_ptr followVectorField( /// \param[out] result information about success or failure. /// \return Trajectory or \c nullptr if planning failed. 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, diff --git a/src/planner/CMakeLists.txt b/src/planner/CMakeLists.txt index fabdfcc7f5..163b83c746 100644 --- a/src/planner/CMakeLists.txt +++ b/src/planner/CMakeLists.txt @@ -4,6 +4,7 @@ set(sources ConfigurationToConfigurations.cpp ConfigurationToTSR.cpp ConfigurationToEndEffectorOffset.cpp + ConfigurationToEndEffectorOffsetPlanner.cpp ConfigurationToEndEffectorPose.cpp FirstSupportedMetaPlanner.cpp CompositePlanner.cpp diff --git a/src/planner/ConfigurationToEndEffectorOffset.cpp b/src/planner/ConfigurationToEndEffectorOffset.cpp index 61cd796626..b5e5a73d8e 100644 --- a/src/planner/ConfigurationToEndEffectorOffset.cpp +++ b/src/planner/ConfigurationToEndEffectorOffset.cpp @@ -7,9 +7,9 @@ namespace planner { //============================================================================== ConfigurationToEndEffectorOffset::ConfigurationToEndEffectorOffset( - statespace::ConstStateSpacePtr stateSpace, + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, dart::dynamics::ConstBodyNodePtr endEffectorBodyNode, - const statespace::StateSpace::State* startState, + const statespace::dart::MetaSkeletonStateSpace::State* startState, const Eigen::Vector3d& direction, const double signedDistance, constraint::ConstTestablePtr constraint) @@ -44,7 +44,7 @@ ConfigurationToEndEffectorOffset::getEndEffectorBodyNode() const } //============================================================================== -const statespace::StateSpace::State* +const statespace::dart::MetaSkeletonStateSpace::State* ConfigurationToEndEffectorOffset::getStartState() const { return mStartState; diff --git a/src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp b/src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp new file mode 100644 index 0000000000..1ffb8c2439 --- /dev/null +++ b/src/planner/ConfigurationToEndEffectorOffsetPlanner.cpp @@ -0,0 +1,25 @@ +#include "aikido/planner/ConfigurationToEndEffectorOffsetPlanner.hpp" + +namespace aikido { +namespace planner { + +//============================================================================== +ConfigurationToEndEffectorOffsetPlanner:: + ConfigurationToEndEffectorOffsetPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace) + : SingleProblemPlanner(stateSpace) + , mMetaSkeletonStateSpace(std::move(stateSpace)) +{ + // Do nothing +} + +//============================================================================== +statespace::dart::ConstMetaSkeletonStateSpacePtr +ConfigurationToEndEffectorOffsetPlanner::getMetaSkeletonStateSpace() +{ + return mMetaSkeletonStateSpace; +} + +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/CMakeLists.txt b/src/planner/vectorfield/CMakeLists.txt index 4e63075c3c..33dcdc2a7b 100644 --- a/src/planner/vectorfield/CMakeLists.txt +++ b/src/planner/vectorfield/CMakeLists.txt @@ -1,7 +1,8 @@ -set(sources +set(sources VectorFieldPlanner.cpp VectorField.cpp VectorFieldUtil.cpp + VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp detail/VectorFieldIntegrator.cpp detail/VectorFieldPlannerExceptions.cpp BodyNodePoseVectorField.cpp @@ -42,4 +43,3 @@ add_component_dependencies(${PROJECT_NAME} planner_vectorfield ) format_add_sources(${sources}) - diff --git a/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp b/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp new file mode 100644 index 0000000000..07fa954012 --- /dev/null +++ b/src/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.cpp @@ -0,0 +1,88 @@ +#include "aikido/planner/vectorfield/VectorFieldConfigurationToEndEffectorOffsetPlanner.hpp" + +#include "aikido/planner/vectorfield/VectorFieldPlanner.hpp" + +namespace aikido { +namespace planner { +namespace vectorfield { + +//============================================================================== +VectorFieldConfigurationToEndEffectorOffsetPlanner:: + VectorFieldConfigurationToEndEffectorOffsetPlanner( + statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::MetaSkeletonPtr metaskeleton, + double distanceTolerance, + double positionTolerance, + double angularTolerance, + double initialStepSize, + double jointLimitTolerance, + double constraintCheckResolution, + std::chrono::duration timelimit) + : ConfigurationToEndEffectorOffsetPlanner(std::move(stateSpace)) + , mMetaskeleton(std::move(metaskeleton)) + , mDistanceTolerance(distanceTolerance) + , mPositionTolerance(positionTolerance) + , mAngularTolerance(angularTolerance) + , mInitialStepSize(initialStepSize) + , mJointLimitTolerance(jointLimitTolerance) + , mConstraintCheckResolution(constraintCheckResolution) + , mTimelimit(timelimit) +{ + // Do nothing here. +} + +//============================================================================== +trajectory::TrajectoryPtr +VectorFieldConfigurationToEndEffectorOffsetPlanner::plan( + const SolvableProblem& problem, Result* result) +{ + // TODO (sniyaz): Check equality between state space of this planner and given + // problem. + + using aikido::planner::vectorfield::planToEndEffectorOffset; + using aikido::statespace::dart::MetaSkeletonStateSpace; + + // Handle the fact that distance can be negative. + double distance = problem.getDistance(); + Eigen::Vector3d direction = problem.getDirection(); + + if (distance < 0) + { + distance = -1.0 * distance; + direction = -1.0 * direction; + } + + double minDistance = distance - mDistanceTolerance; + double maxDistance = distance + mDistanceTolerance; + + if (minDistance < 0.) + { + std::stringstream ss; + ss << "Distance must be non-negative; min distance to move is " + << problem.getDistance() - mDistanceTolerance << "."; + throw std::runtime_error(ss.str()); + } + + // Just call the core VFP function. + // TODO: How should start state be handled? + return planToEndEffectorOffset( + *problem.getStartState(), + getMetaSkeletonStateSpace(), + mMetaskeleton, + problem.getEndEffectorBodyNode(), + problem.getConstraint(), + direction, + minDistance, + maxDistance, + mPositionTolerance, + mAngularTolerance, + mInitialStepSize, + mJointLimitTolerance, + mConstraintCheckResolution, + mTimelimit, + result); +} + +} // namespace vectorfield +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index 5c4d509ccd..9d2dcf957e 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -126,6 +126,7 @@ 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, @@ -152,23 +153,6 @@ std::unique_ptr planToEndEffectorOffset( // std::lock_guard lock(metaskeleton->getLockableReference()) // once https://github.com/dartsim/dart/pull/1011 is released. - if (minDistance < 0.) - { - std::stringstream ss; - ss << "Distance must be non-negative; got " << minDistance << "."; - throw std::runtime_error(ss.str()); - } - - if (maxDistance < minDistance) - { - throw std::runtime_error("Max distance is less than distance."); - } - - if (direction.norm() == 0.0) - { - throw std::runtime_error("Direction vector is a zero vector"); - } - // TODO: Check compatibility between MetaSkeleton and MetaSkeletonStateSpace // Save the current state of the space @@ -195,11 +179,10 @@ std::unique_ptr planToEndEffectorOffset( compoundConstraint->addConstraint( constraint::dart::createTestableBounds(stateSpace)); - auto startState - = stateSpace->getScopedStateFromMetaSkeleton(metaskeleton.get()); + stateSpace->setState(metaskeleton.get(), &startState); return followVectorField( *vectorfield, - *startState, + startState, *compoundConstraint, timelimit, initialStepSize, diff --git a/src/robot/util.cpp b/src/robot/util.cpp index 5ff49b2a7b..fd70ca4ffc 100644 --- a/src/robot/util.cpp +++ b/src/robot/util.cpp @@ -401,10 +401,14 @@ trajectory::TrajectoryPtr planToEndEffectorOffset( auto saver = MetaSkeletonStateSaver(metaSkeleton); DART_UNUSED(saver); + statespace::dart::MetaSkeletonStateSpace::State startState; + space->getState(metaSkeleton.get(), &startState); + auto minDistance = distance - vfParameters.negativeDistanceTolerance; auto maxDistance = distance + vfParameters.positiveDistanceTolerance; auto traj = planner::vectorfield::planToEndEffectorOffset( + startState, space, metaSkeleton, bodyNode, diff --git a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp index 1e3a300b4a..2101320770 100644 --- a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp +++ b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp @@ -1,9 +1,11 @@ #include #include #include +#include "aikido/planner/ConfigurationToEndEffectorOffset.hpp" #include #include #include +#include #include #include #include @@ -313,13 +315,15 @@ TEST_F(VectorFieldPlannerTest, ComputeJointVelocityFromTwistTest) TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) { - using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField; + using aikido::planner::ConfigurationToEndEffectorOffset; + using aikido::planner::vectorfield:: + VectorFieldConfigurationToEndEffectorOffsetPlanner; Eigen::Vector3d direction; direction << 1., 1., 0.; direction.normalize(); - double minDistance = 0.4; - double maxDistance = 0.42; + double signedDistance = 0.41; + double distanceTolerance = 0.01; mSkel->setPositions(mStartConfig); auto startState = mStateSpace->createState(); @@ -334,14 +338,20 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) double constraintCheckResolution = 1e-3; std::chrono::duration timelimit(10.0); - auto traj = aikido::planner::vectorfield::planToEndEffectorOffset( + // Create problem. + auto offsetProblem = ConfigurationToEndEffectorOffset( mStateSpace, - mSkel, mBodynode, - mPassingConstraint, + startState, direction, - minDistance, - maxDistance, + signedDistance, + mPassingConstraint); + + // Create planner. + auto vfOffsetPlanner = VectorFieldConfigurationToEndEffectorOffsetPlanner( + mStateSpace, + mSkel, + distanceTolerance, positionTolerance, angularTolerance, initialStepSize, @@ -349,6 +359,10 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) constraintCheckResolution, timelimit); + // Invoke planning method. + auto resultTraj = vfOffsetPlanner.plan(offsetProblem); + auto traj = std::dynamic_pointer_cast(resultTraj); + EXPECT_FALSE(traj == nullptr) << "Trajectory not found"; if (traj == nullptr) @@ -372,6 +386,9 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) int stepNum = 10; double timeStep = traj->getDuration() / stepNum; + double minDistance = signedDistance - distanceTolerance; + double maxDistance = signedDistance + distanceTolerance; + for (double t = traj->getStartTime(); t <= traj->getEndTime(); t += timeStep) { auto waypoint = mStateSpace->createState(); @@ -403,36 +420,27 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) TEST_F(VectorFieldPlannerTest, DirectionZeroVector) { - using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField; + using aikido::planner::ConfigurationToEndEffectorOffset; + using aikido::planner::vectorfield:: + VectorFieldConfigurationToEndEffectorOffsetPlanner; Eigen::Vector3d direction = Eigen::Vector3d::Zero(); - double distance = 0.2; - double positionTolerance = 0.01; - double angularTolerance = 0.15; - double initialStepSize = 0.05; - double jointLimitTolerance = 1e-3; - double constraintCheckResolution = 1e-3; - std::chrono::duration timelimit(5.); - double maxDistance = 0.22; + double signedDistance = 0.21; mSkel->setPositions(mStartConfig); + auto startState = mStateSpace->createState(); + mStateSpace->convertPositionsToState(mStartConfig, startState); + // Create Problem, which should fail. EXPECT_THROW( - aikido::planner::vectorfield::planToEndEffectorOffset( + ConfigurationToEndEffectorOffset( mStateSpace, - mSkel, mBodynode, - mPassingConstraint, + startState, direction, - distance, - maxDistance, - positionTolerance, - angularTolerance, - initialStepSize, - jointLimitTolerance, - constraintCheckResolution, - timelimit), - std::runtime_error); + signedDistance, + mPassingConstraint), + std::invalid_argument); } TEST_F(VectorFieldPlannerTest, PlanToEndEffectorPoseTest)