From 9267dbd9286fd0eb2752237d93ea7e0a5583dcc3 Mon Sep 17 00:00:00 2001 From: Daqing Yi Date: Sat, 9 Dec 2017 22:29:50 -0800 Subject: [PATCH] New vector field planner impelementation (#268) * new vector field planner and added move end effector pose vector field * add move to end effector pose test cases * refactor code and format * add new tests to plan to end effector pose * add functions of enabling collision checking and dof limit checking * fix doc strings * add velocity gain to support controlling in complex scenarios * add new class for collision error * address JS's comments * address JS's comments and add doc strings for some member variables * Fix const-correctness in ConfigurationSpaceVectorField * Fix code style * fix doc string style * Add invalid max distance variable * update cpde style * fix typos * address Mike's comments * add parameter for constraint check resolution * add doc strings, moved implementation details, add plannerStatus as output * address Mike's comments * update for formatting * address Mike's comments * address Mike's comments * change to use TestableIntersection * fix doc strings * add changelog.md --- CHANGELOG.md | 2 +- .../vectorfield/BodyNodePoseVectorField.hpp | 126 ++++++ .../MoveEndEffectorOffsetVectorField.hpp | 120 +++--- .../MoveEndEffectorPoseVectorField.hpp | 70 ++++ .../planner/vectorfield/VectorField.hpp | 78 ++++ .../vectorfield/VectorFieldPlanner.hpp | 153 +++---- .../VectorFieldPlannerExceptions.hpp | 46 -- .../vectorfield/VectorFieldPlannerStatus.hpp | 26 ++ .../planner/vectorfield/VectorFieldUtil.hpp | 124 +++--- include/aikido/statespace/StateSpace.hpp | 1 + .../dart/MetaSkeletonStateSpace.hpp | 2 + .../vectorfield/BodyNodePoseVectorField.cpp | 165 ++++++++ src/planner/vectorfield/CMakeLists.txt | 6 +- .../MoveEndEffectorOffsetVectorField.cpp | 169 +++----- .../MoveEndEffectorPoseVectorField.cpp | 58 +++ src/planner/vectorfield/VectorField.cpp | 28 ++ .../vectorfield/VectorFieldPlanner.cpp | 396 ++++++++---------- .../VectorFieldPlannerExceptions.cpp | 29 -- src/planner/vectorfield/VectorFieldUtil.cpp | 244 ++++++----- .../detail/VectorFieldIntegrator.cpp | 161 +++++++ .../detail/VectorFieldIntegrator.hpp | 130 ++++++ .../detail/VectorFieldPlannerExceptions.cpp | 51 +++ .../detail/VectorFieldPlannerExceptions.hpp | 74 ++++ .../vectorfield/test_VectorFieldPlanner.cpp | 298 +++++++------ 24 files changed, 1730 insertions(+), 827 deletions(-) create mode 100644 include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp create mode 100644 include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp create mode 100644 include/aikido/planner/vectorfield/VectorField.hpp delete mode 100644 include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp create mode 100644 include/aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp create mode 100644 src/planner/vectorfield/BodyNodePoseVectorField.cpp create mode 100644 src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp create mode 100644 src/planner/vectorfield/VectorField.cpp delete mode 100644 src/planner/vectorfield/VectorFieldPlannerExceptions.cpp create mode 100644 src/planner/vectorfield/detail/VectorFieldIntegrator.cpp create mode 100644 src/planner/vectorfield/detail/VectorFieldIntegrator.hpp create mode 100644 src/planner/vectorfield/detail/VectorFieldPlannerExceptions.cpp create mode 100644 src/planner/vectorfield/detail/VectorFieldPlannerExceptions.hpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 334b0fc0a4..6a16d87349 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,7 +17,7 @@ * Planner * Added World class: [#243](https://github.com/personalrobotics/aikido/pull/243), [#252](https://github.com/personalrobotics/aikido/pull/252) - * Added vector field planner [#246](https://github.com/personalrobotics/aikido/pull/246), [#262](https://github.com/personalrobotics/aikido/pull/262) + * Added vector field planner [#246](https://github.com/personalrobotics/aikido/pull/246), [#262](https://github.com/personalrobotics/aikido/pull/262), [#268](https://github.com/personalrobotics/aikido/pull/268) * RViz diff --git a/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp b/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp new file mode 100644 index 0000000000..4b8927b00c --- /dev/null +++ b/include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp @@ -0,0 +1,126 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_ + +#include +#include +#include + +namespace aikido { +namespace planner { +namespace vectorfield { + +/// This class is a vector field over a MetaSkeletonStateSpace that is +/// defined in terms of a Cartesian vector field over the pose of a +/// BodyNode in that MetaSkeleton. +/// The Jacobian pseudoinverse is used to map velocities in se(3) to +/// velocities in configuration space. +/// If multiple solutions exist, i.e. the kinematic chain leading to +/// the BodyNode in the MetaSkeleton is redundant, then a heuristic is +/// used to select velocities from the null space to avoid violating +/// joint limits. +class BodyNodePoseVectorField : public VectorField +{ +public: + /// Constructor. + /// + /// \param[in] stateSpace MetaSkeleton state space. + /// \param[in] bn Body node of end-effector. + /// \param[in] maxStepSize The maximum step size used to guarantee + /// that the integrator does not step out of joint limits. + /// \param[in] jointLimitpadding If less then this distance to joint + /// limit, velocity is bounded in that direction to 0. + /// \param[in] enforceJointVelocityLimits Whether joint velocity limits + /// are considered in computation. + BodyNodePoseVectorField( + aikido::statespace::dart::MetaSkeletonStateSpacePtr + metaSkeletonStateSpace, + dart::dynamics::BodyNodePtr bodyNode, + double maxStepSize, + double jointLimitPadding, + bool enforceJointVelocityLimits = false); + + // Documentation inherited. + bool evaluateVelocity( + const aikido::statespace::StateSpace::State* state, + Eigen::VectorXd& qd) const override; + + // Documentation inherited. + VectorFieldPlannerStatus evaluateStatus( + const aikido::statespace::StateSpace::State* state) const override; + + /// Evaluate Cartesian velocity by current pose of body node. + /// + /// \param[in] pose Current pose of body node. + /// \param[out] cartesianVelocity Cartesian velocity defined in se(3). + virtual bool evaluateCartesianVelocity( + const Eigen::Isometry3d& pose, + Eigen::Vector6d& cartesianVelocity) const = 0; + + /// Evaluate current pose to determine the status of planning. + /// + /// \param[in] pose Current pose of body node. + /// \return Status of planner. + virtual VectorFieldPlannerStatus evaluateCartesianStatus( + const Eigen::Isometry3d& pose) const = 0; + + // Documentation inherited. + bool evaluateTrajectory( + const aikido::trajectory::Trajectory& trajectory, + const aikido::constraint::Testable* constraint, + double evalStepSize, + double& evalTimePivot, + bool excludeEndTime) const override; + + /// Return meta skeleton state space. + aikido::statespace::dart::MetaSkeletonStateSpacePtr + getMetaSkeletonStateSpace(); + + /// Return const meta skeleton state space. + aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr + getMetaSkeletonStateSpace() const; + + /// Returns meta skeleton. + dart::dynamics::MetaSkeletonPtr getMetaSkeleton(); + + /// Returns const meta skeleton. + dart::dynamics::ConstMetaSkeletonPtr getMetaSkeleton() const; + + /// Returns body node of end-effector. + dart::dynamics::BodyNodePtr getBodyNode(); + + /// Returns const body node of end-effector. + dart::dynamics::ConstBodyNodePtr getBodyNode() const; + +protected: + /// Meta skeleton state space. + aikido::statespace::dart::MetaSkeletonStateSpacePtr mMetaSkeletonStateSpace; + + /// Meta Skeleton + dart::dynamics::MetaSkeletonPtr mMetaSkeleton; + + /// BodyNode + dart::dynamics::BodyNodePtr mBodyNode; + + /// Maximum step size of integrator. + double mMaxStepSize; + + /// Padding of joint limits. + double mJointLimitPadding; + + /// Joint velocities lower limits. + Eigen::VectorXd mVelocityLowerLimits; + + /// Joint velocities upper limits. + Eigen::VectorXd mVelocityUpperLimits; + + /// Enfoce joint velocity limits + bool mEnforceJointVelocityLimits; +}; + +using BodyNodePoseVectorFieldPtr = std::shared_ptr; + +} // namespace vectorfield +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_ diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp index a4001e030f..3930c7480e 100644 --- a/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp +++ b/include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp @@ -1,81 +1,91 @@ #ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTOROFFSETVECTORFIELD_HPP_ #define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTOROFFSETVECTORFIELD_HPP_ -#include +#include namespace aikido { namespace planner { namespace vectorfield { /// Vector field for moving end-effector by a direction and distance. +/// It defines a vector field in meta-skeleton state space that moves +/// an end-effector a desired offset with move-hand-straight constraint. +/// The move-hand-straight constraint is defined by the direction and +/// distance range [minimum distance, maximum distance). +/// Movement less than minimum distance will return failure. +/// The motion will not move further than maximum distance. /// /// This class defines two callback functions for vectorfield planner. /// One for generating joint velocity in MetaSkeleton state space, /// and one for determining vectorfield planner status. -class MoveEndEffectorOffsetVectorField +class MoveEndEffectorOffsetVectorField : public BodyNodePoseVectorField { public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW /// Constructor /// - /// \param[in] _bn Body node of end-effector - /// \param[in] _linearVelocity Linear velocity in all the directions - /// \param[in] _startTime Start time of a planned trajectory - /// \param[in] _endTime End time of a planned trajectory - /// \param[in] _timestep Time step size - /// \param[in] _linearGain Gain of P control on linear deviation - /// \param[in] _linearTolerance Tolerance on linear deviation - /// \param[in] _angularGain Gain of P control on angular deviation - /// \param[in] _angularTolerance Tolerance on angular deviation - /// \param[in] _optimizationTolerance Tolerance on optimization - /// \param[in] _padding Padding to the boundary + /// \param[in] stateSpace MetaSkeleton state space. + /// \param[in] bn Body node of end-effector. + /// \param[in] direction Unit vector in the direction of motion. + /// \param[in] minDistance Minimum distance in meters. + /// \param[in] maxDistance Maximum distance in meters. Maximum distance + /// should be larger than minimum distance so that a feasible solution + /// could be found. + /// \param[in] positionTolerance Constraint tolerance in meters. + /// \param[in] angularTolerance Constraint tolerance in radians. + /// \param[in] linearGain Proportional control gain that corrects linear + /// orthogonal error. + /// \param[in] rotationGain Proportional control gain that corrects + /// rotational error + /// \param[in] maxStepSize The maximum step size used to guarantee + /// that the integrator does not step out of joint limits. + /// \param[in] jointLimitPadding If less then this distance to joint + /// limit, velocity is bounded in that direction to 0. MoveEndEffectorOffsetVectorField( - dart::dynamics::BodyNodePtr _bn, - const Eigen::Vector3d& _linearVelocity, - double _startTime, - double _endTime, - double _timestep, - double _linearGain = 1., - double _linearTolerance = 0.01, - double _angularGain = 1., - double _angularTolerance = 0.01, - double _optimizationTolerance = 1e-3, - double _padding = 1e-5); + aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::BodyNodePtr bn, + const Eigen::Vector3d& direction, + double minDistance, + double maxDistance, + double positionTolerance, + double angularTolerance, + double maxStepSize, + double jointLimitPadding); - /// Vectorfield callback function - /// - /// \param[in] _stateSpace MetaSkeleton state space - /// \param[in] _t Current time being planned - /// \param[out] _dq Joint velocities - /// \return Whether joint velocities are successfully computed - bool operator()( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - double _t, - Eigen::VectorXd& _dq); + // Documentation inherited. + bool evaluateCartesianVelocity( + const Eigen::Isometry3d& pose, + Eigen::Vector6d& cartesianVelocity) const override; - /// Vectorfield planning status callback function - /// - /// \param[in] _stateSpace MetaSkeleton state space - /// \param[in] _t Current time being planned - /// \return Status of planning - VectorFieldPlannerStatus operator()( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - double _t); + // Documentation inherited. + VectorFieldPlannerStatus evaluateCartesianStatus( + const Eigen::Isometry3d& pose) const override; -private: - dart::dynamics::BodyNodePtr mBodynode; - Eigen::Vector3d mVelocity; - Eigen::Vector3d mLinearDirection; - double mStartTime; - double mEndTime; - double mTimestep; - double mLinearGain; - double mLinearTolerance; - double mAngularGain; +protected: + /// Movement direction. + Eigen::Vector3d mDirection; + + /// Minimum movement distance. + double mMinDistance; + + /// Maximum distance allowed to move. + double mMaxDistance; + + /// Tolerance of linear deviation error. + double mPositionTolerance; + + /// Tolerance of angular error. double mAngularTolerance; - double mOptimizationTolerance; - double mPadding; + + /// Linear gain of a proportional control that corrects linear orthogonal + /// error. + double mLinearGain; + + /// Rotation gain of a proportional control that corrects rotational error. + double mRotationGain; + + /// Start pose of the end-effector. Eigen::Isometry3d mStartPose; - Eigen::Isometry3d mTargetPose; }; } // namespace vectorfield diff --git a/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp new file mode 100644 index 0000000000..fc70d99e84 --- /dev/null +++ b/include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp @@ -0,0 +1,70 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_ + +#include + +namespace aikido { +namespace planner { +namespace vectorfield { + +/// Vector field for moving end-effector to a goal pose in SE(3). +/// It defines a vector field in meta-skeleton state space that moves +/// an end-effector a desired pose in SE(3) by following a geodesic +/// loss function in SE(3) via an optimized Jacobian. +/// The geodesic loss function is defined as the geodesic (shortest +/// path) from the current pose to the goal pose. +/// +/// This class defines two callback functions for vectorfield planner. +/// One for generating joint velocity in MetaSkeleton state space, +/// and one for determining vectorfield planner status. +class MoveEndEffectorPoseVectorField : public BodyNodePoseVectorField +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + /// Constructor. + /// + /// \param[in] stateSpace MetaSkeleton state space. + /// \param[in] bn Body node of end-effector. + /// \param[in] goalPose Desired end-effector pose. + /// \param[in] poseErrorTolerance Constraint error tolerance in meters. + /// \param[in] r Conversion of radius to meters in computing Geodesic + /// distance. + /// \param[in] maxStepSize The maximum step size used to guarantee + /// that the integrator does not step out of joint limits. + /// \param[in] jointLimitPadding If less then this distance to joint + /// limit, velocity is bounded in that direction to 0. + MoveEndEffectorPoseVectorField( + aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::BodyNodePtr bn, + const Eigen::Isometry3d& goalPose, + double poseErrorTolerance, + double r, + double maxStepSize, + double jointLimitPadding); + + // Documentation inherited. + bool evaluateCartesianVelocity( + const Eigen::Isometry3d& pose, + Eigen::Vector6d& cartesianVelocity) const override; + + // Documentation inherited. + VectorFieldPlannerStatus evaluateCartesianStatus( + const Eigen::Isometry3d& pose) const override; + +protected: + /// Goal pose. + Eigen::Isometry3d mGoalPose; + + /// Tolerance of pose error. + double mPoseErrorTolerance; + + /// Conversion ratio from radius to meter. + double mConversionRatioFromRadiusToMeter; +}; + +} // namespace vectorfield +} // namespace planner +} // namespace aikido + +#endif // ifndef + // AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_ diff --git a/include/aikido/planner/vectorfield/VectorField.hpp b/include/aikido/planner/vectorfield/VectorField.hpp new file mode 100644 index 0000000000..e91c628d31 --- /dev/null +++ b/include/aikido/planner/vectorfield/VectorField.hpp @@ -0,0 +1,78 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELD_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELD_HPP_ + +#include +#include +#include +#include + +namespace aikido { +namespace planner { +namespace vectorfield { + +/// This class defines a vector field. +/// +/// Any vector field should inherit this class to implememnt functions +/// (1) evaluateVelocity() that calculates velocity given a state; and +/// (2) evaluteStatus() that checks the planner status given a state. +class VectorField +{ +public: + /// Constructor. + /// + /// \param[in] stateSpace State space that vector field is defined in. + explicit VectorField(aikido::statespace::StateSpacePtr stateSpace); + + /// Vectorfield callback function. + /// + /// \param[in] state Statespace state. + /// \param[out] qd Joint velocities. + /// \return Whether joint velocities are successfully computed. + virtual bool evaluateVelocity( + const aikido::statespace::StateSpace::State* state, + Eigen::VectorXd& qd) const = 0; + + /// Vectorfield planning status callback function. + /// + /// \praram[in] state State to evaluate. + /// \return Status of planning. + virtual VectorFieldPlannerStatus evaluateStatus( + const aikido::statespace::StateSpace::State* state) const = 0; + + /// Evaludate whether a trajectory satisfies a constraint. + /// It is checked by a user-defined evaluation step size. + /// + /// \param[in] trajectory Trajectory to be evaluated. + /// \param[in] constraint Constraint to be satisfied. + /// \param[in] evalStepSize The step size used in evaluating constraint. + /// \param[in/out] evalTimePivot Input provides the start time of the + /// trajectory + /// to evaluate; output returns the end time of the trajectory evaluate. + /// \param[in] excludeEndTime Whether end time is excluded in evaluation. + /// evaluate. + /// satisfaction. + virtual bool evaluateTrajectory( + const aikido::trajectory::Trajectory& trajectory, + const aikido::constraint::Testable* constraint, + double evalStepSize, + double& evalTimePivot, + bool excludeEndTime) const = 0; + + /// Returns state space. + aikido::statespace::StateSpacePtr getStateSpace(); + + /// Returns const state space. + aikido::statespace::ConstStateSpacePtr getStateSpace() const; + +protected: + /// State space + aikido::statespace::StateSpacePtr mStateSpace; +}; + +using VectorFieldPtr = std::shared_ptr; + +} // namespace vectorfield +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_VECTORFIELD_VECTORFIELD_HPP_ diff --git a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp index 8ba3319124..7999aede4c 100644 --- a/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp +++ b/include/aikido/planner/vectorfield/VectorFieldPlanner.hpp @@ -1,8 +1,9 @@ #ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNER_HPP_ #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNER_HPP_ -#include #include +#include +#include #include #include @@ -10,84 +11,94 @@ namespace aikido { namespace planner { namespace vectorfield { -enum class VectorFieldPlannerStatus -{ - TERMINATE, - CACHE_AND_TERMINATE, - CACHE_AND_CONTINUE, - CONTINUE -}; - -/// Callback function of joint velocity calculated by a vector field and a -/// MetaSkeleton. -/// -/// \param[in] _stateSpace MetaSkeleton state space -/// \param[in] _t Planned time of a given duration -/// \param[out] _dq Joint velocity calculated by a vector field and meta -/// skeleton -/// \return Whether vectorfield evaluation succeeds -using VectorFieldCallback = std::function; - -/// Callback function of status of planning -/// -/// \param[in] _stateSpace MetaSkeleton state space -/// \param[in] _t Planned time of a given duration -/// \return Status of vectorfield planner -using VectorFieldStatusCallback = std::function; - -/// Plan to a trajectory by a given vector field. +/// Generate a trajectory following the vector field along given time. /// -/// \param[in] _stateSpace MetaSkeleton state space -/// \param[in] _constraint Trajectory-wide constraint that must be satisfied -/// \param[in] _timestep How long an evaluation step is -/// \param[in] _vectorField Callback of vector field calculation -/// \param[in] _statusCb Callback of planning status -/// \return Trajectory or \c nullptr if planning failed -std::unique_ptr planPathByVectorField( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - const aikido::constraint::TestablePtr& _constraint, - double _timestep, - const VectorFieldCallback& _vectorFieldCb, - const VectorFieldStatusCallback& _statusCb); +/// \param[in] vectorField Vector field to follow. +/// \param[in] startState Start state of the planning. +/// \param[in] constraint Constraint to be satisfied. +/// \param[in] timelimit Timelimit for integration calculation. +/// \param[in] initialStepSize Initial step size of integator in following +/// vector field. +/// \param[in] checkConstraintResolution Resolution used in checking +/// constraint satisfaction in generated trajectory. +/// \param[out] planningResult information about success or failure. +/// \return A trajectory following the vector field. +std::unique_ptr followVectorField( + const aikido::planner::vectorfield::VectorField& vectorField, + const aikido::statespace::StateSpace::State& startState, + const aikido::constraint::Testable& constraint, + std::chrono::duration timelimit, + double initialStepSize, + double checkConstraintResolution, + planner::PlanningResult* planningResult); /// Plan to a trajectory that moves the end-effector by a given direction and /// distance. /// -/// \param[in] _stateSpace MetaSkeleton state space -/// \param[in] _bn Body node of the end-effector -/// \param[in] _constraint Trajectory-wide constraint that must be satisfied -/// \param[in] _direction Direction of moving the end-effector -/// \param[in] _distance Distance of moving the end-effector -/// \param[in] _linearVelocity Linear velocity of moving the end-effector -/// \param[in] _linearTolerance How a planned trajectory is allowed to +/// \param[in] stateSpace MetaSkeleton state space. +/// \param[in] bn Body node of the end-effector. +/// \param[in] constraint Trajectory-wide constraint that must be satisfied. +/// \param[in] direction Direction of moving the end-effector. +/// \param[in] minDistance Distance of moving the end-effector. +/// \param[in] maxDistance Max distance of moving 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 +/// distance. /// \param[in] angularTolerance How a planned trajectory is allowed to deviate -/// from a given direction -/// \param[in] _linearGain Linear gain for a P controller to correct linear -/// deviation -/// \param[in] _angularGain Angular gain for a P controller to correct angular -/// deviation -/// \param[in] _timestep How often velocity should be updated from a vector -/// field -/// \return Trajectory or \c nullptr if planning failed +/// 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. +/// \param[out] planningResult information about success or failure. +/// \return Trajectory or \c nullptr if planning failed. std::unique_ptr planToEndEffectorOffset( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - const dart::dynamics::BodyNodePtr& _bn, - const aikido::constraint::TestablePtr& _constraint, - const Eigen::Vector3d& _direction, - double _distance, - double _linearVelocity, - double _linearTolerance = 0.005, - double _angularTolerance = 0.2, - double _linearGain = 10.0, - double _angularGain = 10.0, - double _timestep = 0.01); + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const dart::dynamics::BodyNodePtr& bn, + const aikido::constraint::TestablePtr& constraint, + const Eigen::Vector3d& direction, + double minDistance, + double maxDistance, + double positionTolerance, + double angularTolerance, + double initialStepSize, + double jointLimitTolerance, + double constraintCheckResolution, + std::chrono::duration timelimit, + planner::PlanningResult* planningResult = nullptr); + +/// Plan to an end effector pose by following a geodesic loss function +/// in SE(3) via an optimized Jacobian. +/// +/// \param[in] stateSpace MetaSkeleton state space. +/// \param[in] bn Body node of the end-effector. +/// \param[in] constraint Trajectory-wide constraint that must be satisfied. +/// \param[in] goalPose Desired end-effector pose. +/// \param[in] positionErrorTolerance How a planned trajectory is allowed to +/// deviated from a straight line segment defined by the direction and the +/// distance. +/// \param[in] conversionRatioInGeodesicDistance Conversion ratio from radius to +/// meter in computing geodesic distance. +/// \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. +/// \param[out] planningResult information about success or failure. +/// \return Trajectory or \c nullptr if planning failed. +std::unique_ptr planToEndEffectorPose( + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const dart::dynamics::BodyNodePtr& bn, + const aikido::constraint::TestablePtr& constraint, + const Eigen::Isometry3d& goalPose, + double poseErrorTolerance, + double conversionRatioInGeodesicDistance, + double initialStepSize, + double jointLimitTolerance, + double constraintCheckResolution, + std::chrono::duration timelimit, + planner::PlanningResult* planningResult = nullptr); } // namespace vectorfield } // namespace planner diff --git a/include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp b/include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp deleted file mode 100644 index f9891c65d6..0000000000 --- a/include/aikido/planner/vectorfield/VectorFieldPlannerExceptions.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_ -#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_ - -#include -#include -#include - -namespace aikido { -namespace planner { -namespace vectorfield { - -/// Define runtime error for the termination of vectorfield planner. -class VectorFieldTerminated : public std::runtime_error -{ -public: - /// Constructor - /// - /// \param[in] _whatArg Error string - VectorFieldTerminated(const std::string& _whatArg); -}; - -/// Define termination error of vectorfield planner due to DOF limit error. -class DofLimitError : public VectorFieldTerminated -{ -public: - /// Constructor - /// - /// \param[in] _dof Degree of freedom - /// \param[in] _whatArg Error string - DofLimitError( - const dart::dynamics::DegreeOfFreedom* _dof, const std::string& _whatArg); - - /// Get degree of freedom - /// - /// \return Degree of freedom - const dart::dynamics::DegreeOfFreedom* dof() const; - -private: - const dart::dynamics::DegreeOfFreedom* mDof; -}; - -} // namespace vectorfield -} // namespace aikido -} // namespace planner - -#endif // ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_ diff --git a/include/aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp b/include/aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp new file mode 100644 index 0000000000..06b259ada8 --- /dev/null +++ b/include/aikido/planner/vectorfield/VectorFieldPlannerStatus.hpp @@ -0,0 +1,26 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNERSTATUS_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNERSTATUS_HPP_ + +namespace aikido { +namespace planner { +namespace vectorfield { + +/// Status of planning. +/// TERMINATE - stop gracefully and output the CACHEd trajectory. +/// CACHE_AND_CONTINUE - save the current trajectory and CONTINUE. +/// return the saved trajectory if TERMINATEd. +/// CONTINUE - keep going. +/// CACHE_AND_TERMINATE - save the current trajectory and TERMINATE. +enum class VectorFieldPlannerStatus +{ + TERMINATE, + CACHE_AND_CONTINUE, + CONTINUE, + CACHE_AND_TERMINATE +}; + +} // namespace vectorfield +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNERSTATUS_HPP_ diff --git a/include/aikido/planner/vectorfield/VectorFieldUtil.hpp b/include/aikido/planner/vectorfield/VectorFieldUtil.hpp index 48a9a75b4b..d96fc9120e 100644 --- a/include/aikido/planner/vectorfield/VectorFieldUtil.hpp +++ b/include/aikido/planner/vectorfield/VectorFieldUtil.hpp @@ -2,86 +2,76 @@ #define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDUTIL_HPP_ #include -#include -#include -#include #include #include +#include #include namespace aikido { namespace planner { namespace vectorfield { -struct Knot -{ - double mT; - Eigen::VectorXd mPositions; - Eigen::VectorXd mVelocities; -}; - -/// Convert a sequence of knots into a Spline trajectory. +/// Compute joint velocity from a given twist. /// -/// \param[in] _knots A sequence of knots -/// \param[in] _cache_index Total cache index number -/// \param[in] _stateSpace MetaSkeleton state space -/// \return A Spline trajectory -std::unique_ptr convertToSpline( - const std::vector& _knots, - int _cacheIndex, - aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace); - -/// A function class that defines an objective. The objective measures -/// the difference between a desired twist and Jacobian * joint velocities. -class DesiredTwistFunction : public dart::optimizer::Function -{ -public: - using Twist = Eigen::Vector6d; - using Jacobian = dart::math::Jacobian; - - /// Constructor. - /// - /// \param[in] twist A desired twist - /// \param[in] jacobian System Jacobian - DesiredTwistFunction(const Twist& twist, const Jacobian& jacobian); - - /// Implementation inherited. - /// Evaluating an objective by a state value. - /// - /// \param[in] _qd Joint velocities - /// \return Objective value - double eval(const Eigen::VectorXd& _qd) override; +/// \param[out] jointVelocity Calculated joint velocities. +/// \param[in] desiredTwist Desired twist, which consists of angular velocity +/// and linear velocity. +/// \param[in] stateSpace MetaSkeleton state space. +/// \param[in] bodyNode Body node of the end-effector. +/// \param[in] jointLimitPadding If less then this distance to joint +/// limit, velocity is bounded in that direction to 0. +/// \param[in] jointVelocityLowerLimits Joint velocity lower bounds. +/// \param[in] jointVelocityUpperLimits Joint velocity upper bounds. +/// \param[in] enforceJointVelocityLimits Whether joint velocity limits are +/// considered in computation. +/// \param[in] stepSize Step size in second. It is used in evaluating +/// position bounds violation. It assumes that whether moving the time of +/// stepSize by maximum joint velocity will reach the limit. +/// \return Whether a joint velocity is found +bool computeJointVelocityFromTwist( + Eigen::VectorXd& jointVelocity, + const Eigen::Vector6d& desiredTwist, + aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::BodyNodePtr bodyNode, + double jointLimitPadding, + const Eigen::VectorXd& jointVelocityLowerLimits, + const Eigen::VectorXd& jointVelocityUpperLimits, + bool enforceJointVelocityLimits, + double stepSize); - /// Implementation inherited. - /// Evaluating gradient of an objective by a state value. - /// \param[in] _qd Joint velocities - /// \param[out] _grad Gradient of a defined objective - void evalGradient( - const Eigen::VectorXd& _qd, Eigen::Map _grad) override; +/// Compute the twist in global coordinate that corresponds to the gradient of +/// the geodesic distance between two transforms. +/// +/// \param[in] fromTrans Current transformation. +/// \param[in] toTrans Goal transformation. +/// \return Geodesic twist in global coordinate. It corresponds to the gradient +/// of the geodesic distance between two transforms. The first three are angular +/// velocities (meters per second), and the last three are linear velocities +/// (radian per sec). +Eigen::Vector6d computeGeodesicTwist( + const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans); -private: - Twist mTwist; - Jacobian mJacobian; -}; +/// Compute the error in gloabl coordinate between two transforms. +/// +/// \param[in] fromTrans Current transformation. +/// \param[in] toTrans Goal transformation. +/// \return Geodesic error in global coordinate. It is a 4d vector, in which +/// the first element is the norm of angle difference (in radian), and the +/// last three elements are translation difference (in meter). +Eigen::Vector4d computeGeodesicError( + const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans); -/// Compute joint velocity from a given twist. +/// Compute the geodesic distance between two transforms. +/// gd = norm( relative translation + r * axis-angle error ) /// -/// \param[in] _desiredTwist Desired twist, which consists of angular velocity -/// and linear velocity -/// \param[in] _stateSpace MetaSkeleton state space -/// \param[in] _bodyNode Body node of the end-effector -/// \param[in] _optimizationTolerance Callback of vector field calculation -/// \param[in] _timestep How long will the computed joint velocities be executed -/// \param[in] _padding Padding for joint limits -/// \param[out] _jointVelocity Calculated joint velocities -bool computeJointVelocityFromTwist( - const Eigen::Vector6d& _desiredTwist, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace, - const dart::dynamics::BodyNodePtr _bodyNode, - double _optimizationTolerance, - double _timestep, - double _padding, - Eigen::VectorXd& _jointVelocity); +/// \param[in] fromTrans Current transformation. +/// \param[in] toTrans Goal transformation. +/// \param[in] r In units of meters/radians converts radians to meters. +/// \return Geodesic distance in meter. +double computeGeodesicDistance( + const Eigen::Isometry3d& fromTrans, + const Eigen::Isometry3d& toTrans, + double r); } // namespace vectorfield } // namespace planner diff --git a/include/aikido/statespace/StateSpace.hpp b/include/aikido/statespace/StateSpace.hpp index 8e9387f525..4ba9bf4940 100644 --- a/include/aikido/statespace/StateSpace.hpp +++ b/include/aikido/statespace/StateSpace.hpp @@ -167,6 +167,7 @@ class StateSpace }; using StateSpacePtr = std::shared_ptr; +using ConstStateSpacePtr = std::shared_ptr; } // namespace statespace } // namespace aikido diff --git a/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp b/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp index be746636af..2714a93017 100644 --- a/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp +++ b/include/aikido/statespace/dart/MetaSkeletonStateSpace.hpp @@ -84,6 +84,8 @@ class MetaSkeletonStateSpace : public CartesianProduct }; using MetaSkeletonStateSpacePtr = std::shared_ptr; +using ConstMetaSkeletonStateSpacePtr + = std::shared_ptr; } // namespace dart } // namespace statespace diff --git a/src/planner/vectorfield/BodyNodePoseVectorField.cpp b/src/planner/vectorfield/BodyNodePoseVectorField.cpp new file mode 100644 index 0000000000..5b54f46a8a --- /dev/null +++ b/src/planner/vectorfield/BodyNodePoseVectorField.cpp @@ -0,0 +1,165 @@ +#include +#include +#include +#include +#include "detail/VectorFieldPlannerExceptions.hpp" + +namespace aikido { +namespace planner { +namespace vectorfield { + +//============================================================================== +BodyNodePoseVectorField::BodyNodePoseVectorField( + aikido::statespace::dart::MetaSkeletonStateSpacePtr metaSkeletonStateSpace, + dart::dynamics::BodyNodePtr bodyNode, + double maxStepSize, + double jointLimitPadding, + bool enforceJointVelocityLimits) + : VectorField(metaSkeletonStateSpace) + , mMetaSkeletonStateSpace(metaSkeletonStateSpace) + , mMetaSkeleton(metaSkeletonStateSpace->getMetaSkeleton()) + , mBodyNode(bodyNode) + , mMaxStepSize(maxStepSize) + , mJointLimitPadding(jointLimitPadding) + , mEnforceJointVelocityLimits(enforceJointVelocityLimits) +{ + if (mMaxStepSize <= 0) + { + throw std::invalid_argument("Maximum step size must be non-negative."); + } + if (mJointLimitPadding <= 0) + { + throw std::invalid_argument("Joint limit padding must be non-negative."); + } + + mVelocityLowerLimits = mMetaSkeleton->getVelocityLowerLimits(); + mVelocityUpperLimits = mMetaSkeleton->getVelocityUpperLimits(); +} + +//============================================================================== +bool BodyNodePoseVectorField::evaluateVelocity( + const aikido::statespace::StateSpace::State* state, + Eigen::VectorXd& qd) const +{ + using Eigen::Isometry3d; + using Eigen::Vector3d; + using Eigen::Vector6d; + using aikido::planner::vectorfield::computeJointVelocityFromTwist; + + Eigen::VectorXd position(mMetaSkeleton->getNumDofs()); + auto newState + = static_cast(state); + mMetaSkeletonStateSpace->convertStateToPositions(newState, position); + mMetaSkeleton->setPositions(position); + + const Isometry3d currentPose = mBodyNode->getTransform(); + + Vector6d desiredTwist = Eigen::Vector6d::Zero(); + if (!evaluateCartesianVelocity(currentPose, desiredTwist)) + { + return false; + } + + bool result = computeJointVelocityFromTwist( + qd, + desiredTwist, + mMetaSkeletonStateSpace, + mBodyNode, + mJointLimitPadding, + mVelocityLowerLimits, + mVelocityUpperLimits, + mEnforceJointVelocityLimits, + mMaxStepSize); + return result; +} + +//============================================================================== +VectorFieldPlannerStatus BodyNodePoseVectorField::evaluateStatus( + const aikido::statespace::StateSpace::State* state) const +{ + using Eigen::Isometry3d; + + Eigen::VectorXd position(mMetaSkeleton->getNumDofs()); + auto newState = static_cast(state); + mMetaSkeletonStateSpace->convertStateToPositions(newState, position); + mMetaSkeleton->setPositions(position); + + const Isometry3d currentPose = mBodyNode->getTransform(); + + return evaluateCartesianStatus(currentPose); +} + +//============================================================================== +bool BodyNodePoseVectorField::evaluateTrajectory( + const aikido::trajectory::Trajectory& trajectory, + const aikido::constraint::Testable* constraint, + double evalStepSize, + double& evalTimePivot, + bool excludeEndTime) const +{ + if (constraint == nullptr) + { + return true; + } + auto state = mMetaSkeletonStateSpace->createState(); + + aikido::common::StepSequence seq( + evalStepSize, excludeEndTime, evalTimePivot, trajectory.getEndTime()); + + for (double t : seq) + { + trajectory.evaluate(t, state); + // update current evaluation time pivot + evalTimePivot = t; + if (!constraint->isSatisfied(state)) + { + return false; + } + } + + return true; +} + +//============================================================================== +aikido::statespace::dart::MetaSkeletonStateSpacePtr +BodyNodePoseVectorField::getMetaSkeletonStateSpace() +{ + return mMetaSkeletonStateSpace; +} + +//============================================================================== +aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr +BodyNodePoseVectorField::getMetaSkeletonStateSpace() const +{ + return mMetaSkeletonStateSpace; +} + +//============================================================================== +dart::dynamics::MetaSkeletonPtr BodyNodePoseVectorField::getMetaSkeleton() +{ + return mMetaSkeleton; +} + +//============================================================================== +dart::dynamics::ConstMetaSkeletonPtr BodyNodePoseVectorField::getMetaSkeleton() + const +{ + return mMetaSkeleton; +} + +//============================================================================== +dart::dynamics::BodyNodePtr BodyNodePoseVectorField::getBodyNode() +{ + return mBodyNode; +} + +//============================================================================== +dart::dynamics::ConstBodyNodePtr BodyNodePoseVectorField::getBodyNode() const +{ + return mBodyNode; +} + +} // namespace vectorfield +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/CMakeLists.txt b/src/planner/vectorfield/CMakeLists.txt index 1a1d657cb7..1dd8e6e739 100644 --- a/src/planner/vectorfield/CMakeLists.txt +++ b/src/planner/vectorfield/CMakeLists.txt @@ -1,8 +1,12 @@ set(sources VectorFieldPlanner.cpp + VectorField.cpp VectorFieldUtil.cpp - VectorFieldPlannerExceptions.cpp + detail/VectorFieldIntegrator.cpp + detail/VectorFieldPlannerExceptions.cpp + BodyNodePoseVectorField.cpp MoveEndEffectorOffsetVectorField.cpp + MoveEndEffectorPoseVectorField.cpp ) add_library("${PROJECT_NAME}_planner_vectorfield" SHARED ${sources}) diff --git a/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp b/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp index ec616ea532..e00f8ecc62 100644 --- a/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp +++ b/src/planner/vectorfield/MoveEndEffectorOffsetVectorField.cpp @@ -4,144 +4,103 @@ #include #include #include +#include "detail/VectorFieldPlannerExceptions.hpp" namespace aikido { namespace planner { namespace vectorfield { +//============================================================================== MoveEndEffectorOffsetVectorField::MoveEndEffectorOffsetVectorField( - dart::dynamics::BodyNodePtr _bn, - const Eigen::Vector3d& _linearVelocity, - double _startTime, - double _endTime, - double _timestep, - double _linearGain, - double _linearTolerance, - double _rotationGain, - double _rotationTolerance, - double _optimizationTolerance, - double _padding) - : mBodynode(std::move(_bn)) - , mVelocity(_linearVelocity) - , mLinearDirection(_linearVelocity.normalized()) - , mStartTime(_startTime) - , mEndTime(_endTime) - , mTimestep(_timestep) - , mLinearGain(_linearGain) - , mLinearTolerance(_linearTolerance) - , mAngularGain(_rotationGain) - , mAngularTolerance(_rotationTolerance) - , mOptimizationTolerance(_optimizationTolerance) - , mPadding(_padding) - , mStartPose(_bn->getTransform()) + aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::BodyNodePtr bn, + const Eigen::Vector3d& direction, + double minDistance, + double maxDistance, + double positionTolerance, + double angularTolerance, + double maxStepSize, + double jointLimitPadding) + : BodyNodePoseVectorField(stateSpace, bn, maxStepSize, jointLimitPadding) + , mDirection(direction) + , mMinDistance(minDistance) + , mMaxDistance(maxDistance) + , mPositionTolerance(positionTolerance) + , mAngularTolerance(angularTolerance) + , mStartPose(bn->getTransform()) { - if (mStartTime < 0.0) - throw std::invalid_argument("Start time is negative"); - if (mEndTime < mStartTime) - throw std::invalid_argument("End time is smaller than start time"); - if (mTimestep <= 0.0) - throw std::invalid_argument("Time step is negative"); - - if (mLinearGain < 0) - throw std::invalid_argument("Linear gain is negative"); - if (mLinearTolerance < 0) - throw std::invalid_argument("Linear tolerance is negative"); - if (mAngularGain < 0) - throw std::invalid_argument("Angular gain is negative"); + if (mMinDistance < 0) + throw std::invalid_argument("Minimum distance must be non-negative."); + if (mDirection.norm() == 0) + throw std::invalid_argument("Direction must be non-zero"); + if (mMaxDistance < mMinDistance) + throw std::invalid_argument("Max distance is less than minimum distance."); + if (mPositionTolerance < 0) + throw std::invalid_argument("Position tolerance is negative"); if (mAngularTolerance < 0) throw std::invalid_argument("Angular tolerance is negative"); - if (mOptimizationTolerance < 0) - throw std::invalid_argument("Optimization tolerance is negative"); + if (mLinearGain < 0) + throw std::invalid_argument("Linear gain is negative"); + if (mRotationGain < 0) + throw std::invalid_argument("Rotation gain is negative"); - mTargetPose = mStartPose; - mTargetPose.translation() += mVelocity * (mEndTime - mStartTime); - mEndTime += _padding; + // Normalize the direction vector + mDirection.normalize(); } //============================================================================== -bool MoveEndEffectorOffsetVectorField::operator()( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - double, - Eigen::VectorXd& _dq) +bool MoveEndEffectorOffsetVectorField::evaluateCartesianVelocity( + const Eigen::Isometry3d& pose, Eigen::Vector6d& cartesianVelocity) const { - using Eigen::Isometry3d; - using Eigen::Vector3d; - using Eigen::Vector6d; using dart::math::logMap; + using aikido::planner::vectorfield::computeGeodesicError; - const Isometry3d currentPose = mBodynode->getTransform(); - Vector3d currentWorkspacePose = currentPose.translation(); - Vector3d targetWorkspacePose = mTargetPose.translation(); - - // Compute the "feed-forward" term necessary to move at a constant velocity - // from the start to the goal. - const Vector3d& linearFeedforward = mVelocity; - - // Compute positional error orthogonal to the direction of motion by - // projecting out the component of the error along that direction. - const Vector3d linearError = targetWorkspacePose - currentWorkspacePose; - const Vector3d linearOrthogonalError - = linearError - linearError.dot(mLinearDirection) * mLinearDirection; - // Compute rotational error. - const Vector3d rotationError - = logMap(mTargetPose.rotation().inverse() * currentPose.rotation()); - - // Compute the desired twist using a proportional controller. - Vector6d desiredTwist; - desiredTwist.head<3>() = mAngularGain * rotationError; - desiredTwist.tail<3>() - = linearFeedforward + mLinearGain * linearOrthogonalError; - - bool result = computeJointVelocityFromTwist( - desiredTwist, - _stateSpace, - mBodynode, - mOptimizationTolerance, - mTimestep, - mPadding, - _dq); - return result; + Eigen::Vector6d desiredTwist = computeGeodesicTwist(pose, mStartPose); + desiredTwist.tail<3>() = mDirection; + cartesianVelocity = desiredTwist; + return true; } //============================================================================== -VectorFieldPlannerStatus MoveEndEffectorOffsetVectorField::operator()( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - double _t) +VectorFieldPlannerStatus +MoveEndEffectorOffsetVectorField::evaluateCartesianStatus( + const Eigen::Isometry3d& pose) const { - using Eigen::Isometry3d; - using Eigen::Vector3d; - - DART_UNUSED(_stateSpace); - const Isometry3d currentPose = mBodynode->getTransform(); + using aikido::planner::vectorfield::computeGeodesicError; // Check for deviation from the straight-line trajectory. - const Vector3d linearError - = mTargetPose.translation() - currentPose.translation(); - const Vector3d linearOrthogonalError - = linearError - linearError.dot(mLinearDirection) * mLinearDirection; - double linearOrthogonalMagnitude = linearOrthogonalError.norm(); - - if (linearOrthogonalMagnitude >= mLinearTolerance) + const Eigen::Vector4d geodesicError = computeGeodesicError(mStartPose, pose); + const double orientationError = geodesicError[0]; + const Eigen::Vector3d positionError = geodesicError.tail<3>(); + double movedDistance = positionError.transpose() * mDirection; + double positionDeviation + = (positionError - movedDistance * mDirection).norm(); + + if (fabs(orientationError) > mAngularTolerance) { - dtwarn << "Trajectory deviated from the straight line by " - << linearOrthogonalMagnitude << " m; the tolerance is " - << mLinearTolerance << " m.\n"; + dtwarn << "Deviated from orientation constraint."; return VectorFieldPlannerStatus::TERMINATE; } - // Check if we've reached the target. - if (_t > mEndTime) + if (positionDeviation > mPositionTolerance) { + dtwarn << "Deviated from straight line constraint."; return VectorFieldPlannerStatus::TERMINATE; } - else if (_t >= mStartTime) + + // if larger than max distance, terminate + // if larger than min distance, cache and continue + // if smaller than min distance, continue + if (movedDistance > mMaxDistance) { - return VectorFieldPlannerStatus::CACHE_AND_CONTINUE; + return VectorFieldPlannerStatus::TERMINATE; } - else + else if (movedDistance >= mMinDistance) { - return VectorFieldPlannerStatus::CONTINUE; + return VectorFieldPlannerStatus::CACHE_AND_CONTINUE; } + + return VectorFieldPlannerStatus::CONTINUE; } } // namespace vectorfield diff --git a/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp b/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp new file mode 100644 index 0000000000..b12960f319 --- /dev/null +++ b/src/planner/vectorfield/MoveEndEffectorPoseVectorField.cpp @@ -0,0 +1,58 @@ +#include +#include +#include +#include +#include +#include +#include "detail/VectorFieldPlannerExceptions.hpp" + +namespace aikido { +namespace planner { +namespace vectorfield { + +//============================================================================== +MoveEndEffectorPoseVectorField::MoveEndEffectorPoseVectorField( + aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace, + dart::dynamics::BodyNodePtr bn, + const Eigen::Isometry3d& goalPose, + double poseErrorTolerance, + double r, + double maxStepSize, + double jointLimitPadding) + : BodyNodePoseVectorField(stateSpace, bn, maxStepSize, jointLimitPadding) + , mGoalPose(goalPose) + , mPoseErrorTolerance(poseErrorTolerance) + , mConversionRatioFromRadiusToMeter(r) +{ + if (mPoseErrorTolerance < 0) + throw std::invalid_argument("Pose error tolerance is negative"); +} + +//============================================================================== +bool MoveEndEffectorPoseVectorField::evaluateCartesianVelocity( + const Eigen::Isometry3d& pose, Eigen::Vector6d& cartesianVelocity) const +{ + using aikido::planner::vectorfield::computeGeodesicTwist; + Eigen::Vector6d desiredTwist = computeGeodesicTwist(pose, mGoalPose); + cartesianVelocity = desiredTwist; + return true; +} + +//============================================================================== +VectorFieldPlannerStatus +MoveEndEffectorPoseVectorField::evaluateCartesianStatus( + const Eigen::Isometry3d& pose) const +{ + double poseError = computeGeodesicDistance( + pose, mGoalPose, mConversionRatioFromRadiusToMeter); + + if (poseError < mPoseErrorTolerance) + { + return VectorFieldPlannerStatus::CACHE_AND_TERMINATE; + } + return VectorFieldPlannerStatus::CONTINUE; +} + +} // namespace vectorfield +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/VectorField.cpp b/src/planner/vectorfield/VectorField.cpp new file mode 100644 index 0000000000..f76c86994b --- /dev/null +++ b/src/planner/vectorfield/VectorField.cpp @@ -0,0 +1,28 @@ +#include + +namespace aikido { +namespace planner { +namespace vectorfield { + +//============================================================================== +VectorField::VectorField(aikido::statespace::StateSpacePtr stateSpace) + : mStateSpace(stateSpace) +{ + // Do nothing +} + +//============================================================================== +aikido::statespace::StateSpacePtr VectorField::getStateSpace() +{ + return mStateSpace; +} + +//============================================================================== +aikido::statespace::ConstStateSpacePtr VectorField::getStateSpace() const +{ + return mStateSpace; +} + +} // namespace vectorfield +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/VectorFieldPlanner.cpp b/src/planner/vectorfield/VectorFieldPlanner.cpp index aef536ddf4..60b0c2bfc7 100644 --- a/src/planner/vectorfield/VectorFieldPlanner.cpp +++ b/src/planner/vectorfield/VectorFieldPlanner.cpp @@ -1,278 +1,236 @@ -#include -#include +#include +#include +#include #include +#include #include -#include #include #include #include +#include "detail/VectorFieldIntegrator.hpp" +#include "detail/VectorFieldPlannerExceptions.hpp" +using aikido::planner::vectorfield::detail::VectorFieldIntegrator; +using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField; +using aikido::planner::vectorfield::MoveEndEffectorPoseVectorField; using aikido::statespace::dart::MetaSkeletonStateSpaceSaver; namespace aikido { namespace planner { namespace vectorfield { -static void checkDofLimits( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, - Eigen::VectorXd const& q, - Eigen::VectorXd const& qd) -{ - using dart::dynamics::DegreeOfFreedom; - std::stringstream ss; - - for (std::size_t i = 0; i < stateSpace->getMetaSkeleton()->getNumDofs(); ++i) - { - DegreeOfFreedom* const dof = stateSpace->getMetaSkeleton()->getDof(i); - - if (q[i] < dof->getPositionLowerLimit()) - { - ss << "DOF " << dof->getName() << " exceeds lower position limit: "; - ss << q[i] << " < " << dof->getPositionLowerLimit(); - throw DofLimitError(dof, ss.str()); - } - else if (q[i] > dof->getPositionUpperLimit()) - { - ss << "DOF " << dof->getName() << " exceeds upper position limit: "; - ss << q[i] << " > " << dof->getPositionUpperLimit(); - throw DofLimitError(dof, ss.str()); - } - else if (qd[i] < dof->getVelocityLowerLimit()) - { - ss << "DOF " << dof->getName() << " exceeds lower velocity limit: "; - ss << qd[i] << " < " << dof->getVelocityLowerLimit(); - throw DofLimitError(dof, ss.str()); - } - else if (qd[i] > dof->getVelocityUpperLimit()) - { - ss << "DOF " << dof->getName() << " exceeds upper velocity limit: "; - ss << qd[i] << " > " << dof->getVelocityUpperLimit(); - throw DofLimitError(dof, ss.str()); - } - } -} +constexpr double integrationTimeInterval = 10.0; //============================================================================== -static void checkCollision( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, - const aikido::constraint::TestablePtr& constraint) +std::unique_ptr followVectorField( + const aikido::planner::vectorfield::VectorField& vectorField, + const aikido::statespace::StateSpace::State& startState, + const aikido::constraint::Testable& constraint, + std::chrono::duration timelimit, + double initialStepSize, + double checkConstraintResolution, + planner::PlanningResult* planningResult) { - // Get current position - auto state = stateSpace->getScopedStateFromMetaSkeleton(); - // Throw a termination if in collision - if (!constraint->isSatisfied(state)) - { - throw VectorFieldTerminated("state in collision"); - } -} + using namespace std::placeholders; + using errorStepper = boost::numeric::odeint:: + runge_kutta_dopri5; -//============================================================================== -std::unique_ptr planPathByVectorField( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - const aikido::constraint::TestablePtr& _constraint, - double _dt, - const VectorFieldCallback& _vectorFiledCb, - const VectorFieldStatusCallback& _statusCb) -{ - auto saver = MetaSkeletonStateSpaceSaver(_stateSpace); - DART_UNUSED(saver); + std::size_t dimension = vectorField.getStateSpace()->getDimension(); + auto integrator = std::make_shared( + &vectorField, &constraint, timelimit.count(), checkConstraintResolution); - dart::dynamics::MetaSkeletonPtr skeleton = _stateSpace->getMetaSkeleton(); + integrator->start(); - if (skeleton->getPositionUpperLimits() == skeleton->getPositionLowerLimits()) - { - throw std::invalid_argument("State space volume zero"); - } - if (skeleton->getVelocityUpperLimits() == skeleton->getVelocityLowerLimits()) + try { - throw std::invalid_argument("Velocity space volume zero"); + Eigen::VectorXd initialQ(dimension); + vectorField.getStateSpace()->logMap(&startState, initialQ); + // The current implementation works only in real vector spaces. + + // Integrate the vector field to get a configuration space path. + boost::numeric::odeint::integrate_adaptive( + errorStepper(), + std::bind( + &detail::VectorFieldIntegrator::step, + integrator, + std::placeholders::_1, + std::placeholders::_2, + std::placeholders::_3), + initialQ, + 0., + integrationTimeInterval, + initialStepSize, + std::bind( + &detail::VectorFieldIntegrator::check, + integrator, + std::placeholders::_1, + std::placeholders::_2)); } - - for (std::size_t i = 0; i < skeleton->getNumDofs(); ++i) + // VectorFieldTerminated is an exception that is raised internally to + // terminate + // integration, which does not indicate that an error has occurred. + catch (const detail::VectorFieldTerminated& e) { - std::stringstream ss; - if (skeleton->getPositionLowerLimit(i) > skeleton->getPositionUpperLimit(i)) - { - ss << "Position lower limit is larger than upper limit at DOF " << i; - throw std::invalid_argument(ss.str()); - } - if (skeleton->getVelocityLowerLimit(i) > skeleton->getVelocityUpperLimit(i)) + if (planningResult) { - ss << "Velocity lower limit is larger than upper limit at DOF " << i; - throw std::invalid_argument(ss.str()); + planningResult->message = e.what(); } } - - const std::size_t numDof = _stateSpace->getDimension(); - - std::vector knots; - VectorFieldPlannerStatus terminationStatus - = VectorFieldPlannerStatus::CONTINUE; - std::exception_ptr terminationError; - - int cacheIndex = -1; - int index = 0; - double t = 0; - Eigen::VectorXd q = _stateSpace->getMetaSkeleton()->getPositions(); - - auto startState = _stateSpace->createState(); - _stateSpace->convertPositionsToState(q, startState); - Eigen::VectorXd dq(numDof); - assert(static_cast(q.size()) == numDof); - - do + catch (const detail::VectorFieldError& e) { - // Evaluate the vector field. - try - { - if (!_vectorFiledCb(_stateSpace, t, dq)) - { - throw VectorFieldTerminated("Failed evaluating VectorField."); - } - } - catch (const VectorFieldTerminated& e) + dtwarn << e.what() << std::endl; + if (planningResult) { - dtwarn << "Terminating vector field evaluation." << std::endl; - terminationStatus = VectorFieldPlannerStatus::TERMINATE; - terminationError = std::current_exception(); - break; + planningResult->message = e.what(); } + return nullptr; + } - if (static_cast(dq.size()) != numDof) + if (integrator->getCacheIndex() <= 1) + { + // no enough waypoints cached to make a trajectory output. + if (planningResult) { - throw std::length_error( - "Vector field returned an incorrect number of DOF velocities."); + planningResult->message = "No segment cached."; } + return nullptr; + } - // TODO: This should be computed from the DOF resolutions. - const std::size_t numSteps = 1; - - // Compute the number of collision checks we need. - for (std::size_t istep = 0; istep < numSteps; ++istep) - { - try - { - checkDofLimits(_stateSpace, q, dq); - _stateSpace->getMetaSkeleton()->setPositions(q); - checkCollision(_stateSpace, _constraint); - } - catch (const VectorFieldTerminated& e) - { - terminationStatus = VectorFieldPlannerStatus::TERMINATE; - terminationError = std::current_exception(); - break; - } - - // Insert the waypoint. - assert(static_cast(q.size()) == numDof); - assert(static_cast(dq.size()) == numDof); - - Knot knot; - knot.mT = t; - knot.mPositions = q; - knot.mVelocities = dq; - knots.push_back(knot); - - // Take a step. - auto currentState = _stateSpace->createState(); - _stateSpace->convertPositionsToState(q, currentState); - auto deltaState = _stateSpace->createState(); - _stateSpace->convertPositionsToState(_dt * dq, deltaState); - auto nextState = _stateSpace->createState(); - _stateSpace->compose(currentState, deltaState, nextState); - _stateSpace->convertStateToPositions(nextState, q); - - t += _dt; - index++; - - // Check if we should terminate. - terminationStatus = _statusCb(_stateSpace, t); - if (terminationStatus == VectorFieldPlannerStatus::CACHE_AND_CONTINUE - || terminationStatus == VectorFieldPlannerStatus::CACHE_AND_TERMINATE) - { - cacheIndex = index; - } - } - } while (terminationStatus != VectorFieldPlannerStatus::TERMINATE - && terminationStatus - != VectorFieldPlannerStatus::CACHE_AND_TERMINATE); + std::vector newKnots( + integrator->getKnots().begin(), + integrator->getKnots().begin() + integrator->getCacheIndex()); + auto outputTrajectory + = detail::convertToSpline(newKnots, vectorField.getStateSpace()); - // Print the termination condition. - if (terminationError) + // evaluate constraint satisfaction on last piece of trajectory + double lastEvaluationTime = integrator->getLastEvaluationTime(); + if (outputTrajectory->getEndTime() > lastEvaluationTime) { - try - { - std::rethrow_exception(terminationError); - } - catch (const VectorFieldTerminated& e) + if (!vectorField.evaluateTrajectory( + *outputTrajectory, + &constraint, + checkConstraintResolution, + lastEvaluationTime, + false)) { - dtwarn << "[VectorFieldPlanner::Plan] Terminated early: " << e.what() - << '\n'; + planningResult->message = "Constraint violated."; return nullptr; } } - - if (cacheIndex >= 0) - { - return convertToSpline(knots, cacheIndex, _stateSpace); - } - else - { - throw VectorFieldTerminated( - "Planning was terminated by the StatusCallback."); - } - return nullptr; + return outputTrajectory; } //============================================================================== std::unique_ptr planToEndEffectorOffset( - const aikido::statespace::dart::MetaSkeletonStateSpacePtr& _stateSpace, - const dart::dynamics::BodyNodePtr& _bn, - const aikido::constraint::TestablePtr& _constraint, - const Eigen::Vector3d& _direction, - double _distance, - double _linearVelocity, - double _linearTolerance, - double _angularTolerance, - double _linearGain, - double _angularGain, - double _timestep) + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const dart::dynamics::BodyNodePtr& bn, + const aikido::constraint::TestablePtr& constraint, + const Eigen::Vector3d& direction, + double minDistance, + double maxDistance, + double positionTolerance, + double angularTolerance, + double initialStepSize, + double jointLimitTolerance, + double constraintCheckResolution, + std::chrono::duration timelimit, + planner::PlanningResult* planningResult) { - if (_distance < 0.) + if (minDistance < 0.) { std::stringstream ss; - ss << "Distance must be non-negative; got " << _distance << "."; + ss << "Distance must be non-negative; got " << minDistance << "."; throw std::runtime_error(ss.str()); } - if (_linearVelocity <= 0.0) + if (maxDistance < minDistance) { - std::stringstream ss; - ss << "Linear velocity must be positive; got " << _linearVelocity << "."; - throw std::runtime_error(ss.str()); + throw std::runtime_error("Max distance is less than distance."); } - if (_direction.norm() == 0.0) + if (direction.norm() == 0.0) { throw std::runtime_error("Direction vector is a zero vector"); } - Eigen::Vector3d velocity = _direction.normalized() * _linearVelocity; - double duration = _distance / _linearVelocity; + // Save the current state of the space + auto saver = MetaSkeletonStateSpaceSaver(stateSpace); + DART_UNUSED(saver); + + auto vectorfield = std::make_shared( + stateSpace, + bn, + direction, + minDistance, + maxDistance, + positionTolerance, + angularTolerance, + initialStepSize, + jointLimitTolerance); + + auto compoundConstraint + = std::make_shared(stateSpace); + compoundConstraint->addConstraint(constraint); + compoundConstraint->addConstraint( + aikido::constraint::createTestableBounds(stateSpace)); + + auto startState = stateSpace->getScopedStateFromMetaSkeleton(); + return followVectorField( + *vectorfield, + *startState, + *compoundConstraint, + timelimit, + initialStepSize, + constraintCheckResolution, + planningResult); +} - auto vectorfield = MoveEndEffectorOffsetVectorField( - _bn, - velocity, - 0.0, - duration, - _timestep, - _linearGain, - _linearTolerance, - _angularGain, - _angularTolerance); +//============================================================================== +std::unique_ptr planToEndEffectorPose( + const aikido::statespace::dart::MetaSkeletonStateSpacePtr& stateSpace, + const dart::dynamics::BodyNodePtr& bn, + const aikido::constraint::TestablePtr& constraint, + const Eigen::Isometry3d& goalPose, + double poseErrorTolerance, + double conversionRatioInGeodesicDistance, + double initialStepSize, + double jointLimitTolerance, + double constraintCheckResolution, + std::chrono::duration timelimit, + planner::PlanningResult* planningResult) +{ + // Save the current state of the space + auto saver = MetaSkeletonStateSpaceSaver(stateSpace); + DART_UNUSED(saver); - return planPathByVectorField( - _stateSpace, _constraint, _timestep, vectorfield, vectorfield); + auto vectorfield = std::make_shared( + stateSpace, + bn, + goalPose, + poseErrorTolerance, + conversionRatioInGeodesicDistance, + initialStepSize, + jointLimitTolerance); + + auto compoundConstraint + = std::make_shared(stateSpace); + compoundConstraint->addConstraint(constraint); + compoundConstraint->addConstraint( + aikido::constraint::createTestableBounds(stateSpace)); + + auto startState = stateSpace->getScopedStateFromMetaSkeleton(); + return followVectorField( + *vectorfield, + *startState, + *compoundConstraint, + timelimit, + initialStepSize, + constraintCheckResolution, + planningResult); } } // namespace vectorfield diff --git a/src/planner/vectorfield/VectorFieldPlannerExceptions.cpp b/src/planner/vectorfield/VectorFieldPlannerExceptions.cpp deleted file mode 100644 index 31da739834..0000000000 --- a/src/planner/vectorfield/VectorFieldPlannerExceptions.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include - -namespace aikido { -namespace planner { -namespace vectorfield { - -VectorFieldTerminated::VectorFieldTerminated(const std::string& _whatArg) - : std::runtime_error(_whatArg) -{ - // Do nothing -} - -//============================================================================== -DofLimitError::DofLimitError( - const dart::dynamics::DegreeOfFreedom* _dof, const std::string& _whatArg) - : VectorFieldTerminated(_whatArg), mDof(_dof) -{ - // Do nothing -} - -//============================================================================== -const dart::dynamics::DegreeOfFreedom* DofLimitError::dof() const -{ - return mDof; -} - -} // namespace vectorfield -} // namespace planner -} // namespace aikido diff --git a/src/planner/vectorfield/VectorFieldUtil.cpp b/src/planner/vectorfield/VectorFieldUtil.cpp index 23635d4a27..1cb8fe48fb 100644 --- a/src/planner/vectorfield/VectorFieldUtil.cpp +++ b/src/planner/vectorfield/VectorFieldUtil.cpp @@ -1,3 +1,6 @@ +#include +#include +#include #include #include #include @@ -6,83 +9,69 @@ namespace aikido { namespace planner { namespace vectorfield { -std::unique_ptr convertToSpline( - const std::vector& _knots, - int _cacheIndex, - aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace) -{ - using dart::common::make_unique; - - std::size_t numDof = _stateSpace->getMetaSkeleton()->getNumDofs(); - // Construct the output spline. - Eigen::VectorXd times(_cacheIndex); - std::transform( - _knots.begin(), - _knots.begin() + _cacheIndex, - times.data(), - [](const Knot& knot) { return knot.mT; }); - - auto _outputTrajectory = make_unique(_stateSpace); +namespace { - using CubicSplineProblem = aikido::common:: - SplineProblem; +/// A function class that defines an objective. The objective measures +/// the difference between a desired twist and Jacobian * joint velocities. +/// +class DesiredTwistFunction : public dart::optimizer::Function +{ +public: + using Twist = Eigen::Vector6d; + using Jacobian = dart::math::Jacobian; + + /// Constructor. + /// + /// \param[in] twist A desired twist. + /// \param[in] jacobian System Jacobian. + DesiredTwistFunction(const Twist& twist, const Jacobian& jacobian) + : dart::optimizer::Function("DesiredTwistFunction") + , mTwist(twist) + , mJacobian(jacobian) + { + // Do nothing + } - const Eigen::VectorXd zeroPosition = Eigen::VectorXd::Zero(numDof); - auto currState = _stateSpace->createState(); - for (int iknot = 0; iknot < _cacheIndex - 1; ++iknot) + /// Implementation inherited. + /// Evaluating an objective by a state value. + /// + /// \param[in] qd Joint velocities. + /// \return Objective value. + double eval(const Eigen::VectorXd& qd) override { - const double segmentDuration = _knots[iknot + 1].mT - _knots[iknot].mT; - Eigen::VectorXd currentPosition = _knots[iknot].mPositions; - Eigen::VectorXd currentVelocity = _knots[iknot].mVelocities; - Eigen::VectorXd nextPosition = _knots[iknot + 1].mPositions; - Eigen::VectorXd nextVelocity = _knots[iknot + 1].mVelocities; - - CubicSplineProblem problem(Eigen::Vector2d{0., segmentDuration}, 4, numDof); - problem.addConstantConstraint(0, 0, zeroPosition); - problem.addConstantConstraint(0, 1, currentVelocity); - problem.addConstantConstraint(1, 0, nextPosition - currentPosition); - problem.addConstantConstraint(1, 1, nextVelocity); - const auto solution = problem.fit(); - const auto coefficients = solution.getCoefficients().front(); - - _stateSpace->expMap(currentPosition, currState); - _outputTrajectory->addSegment(coefficients, segmentDuration, currState); + return 0.5 * (mJacobian * qd - mTwist).squaredNorm(); } - return _outputTrajectory; -} -//============================================================================== -DesiredTwistFunction::DesiredTwistFunction( - const Twist& _twist, const Jacobian& _jacobian) - : dart::optimizer::Function("DesiredTwistFunction") - , mTwist(_twist) - , mJacobian(_jacobian) -{ - // Do nothing -} + /// Implementation inherited. + /// Evaluating gradient of an objective by a state value. + /// \param[in] qd Joint velocities. + /// \param[out] grad Gradient of a defined objective. + void evalGradient( + const Eigen::VectorXd& qd, Eigen::Map grad) override + { + grad = mJacobian.transpose() * (mJacobian * qd - mTwist); + } -//============================================================================== -double DesiredTwistFunction::eval(const Eigen::VectorXd& _qd) -{ - return 0.5 * (mJacobian * _qd - mTwist).squaredNorm(); -} +private: + /// Twist. + Twist mTwist; -//============================================================================== -void DesiredTwistFunction::evalGradient( - const Eigen::VectorXd& _qd, Eigen::Map _grad) -{ - _grad = mJacobian.transpose() * (mJacobian * _qd - mTwist); + /// Jacobian of Meta Skeleton. + Jacobian mJacobian; +}; } //============================================================================== bool computeJointVelocityFromTwist( - const Eigen::Vector6d& _desiredTwist, - const aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace, - const dart::dynamics::BodyNodePtr _bodyNode, - double _optimizationTolerance, - double _timestep, - double _padding, - Eigen::VectorXd& _jointVelocity) + Eigen::VectorXd& jointVelocity, + const Eigen::Vector6d& desiredTwist, + const aikido::statespace::dart::MetaSkeletonStateSpacePtr stateSpace, + const dart::dynamics::BodyNodePtr bodyNode, + double jointLimitPadding, + const Eigen::VectorXd& jointVelocityLowerLimits, + const Eigen::VectorXd& jointVelocityUpperLimits, + bool enforceJointVelocityLimits, + double stepSize) { using dart::math::Jacobian; using dart::optimizer::Problem; @@ -90,73 +79,106 @@ bool computeJointVelocityFromTwist( using Eigen::VectorXd; const dart::dynamics::MetaSkeletonPtr skeleton - = _stateSpace->getMetaSkeleton(); + = stateSpace->getMetaSkeleton(); // Use LBFGS to find joint angles that won't violate the joint limits. - const Jacobian jacobian = skeleton->getWorldJacobian(_bodyNode); + const Jacobian jacobian = skeleton->getWorldJacobian(bodyNode); const std::size_t numDofs = skeleton->getNumDofs(); - const VectorXd positions = skeleton->getPositions(); + + jointVelocity = Eigen::VectorXd::Zero(numDofs); + VectorXd positions = skeleton->getPositions(); VectorXd initialGuess = skeleton->getVelocities(); - const VectorXd positionLowerLimits = skeleton->getPositionLowerLimits(); - const VectorXd positionUpperLimits = skeleton->getPositionUpperLimits(); - VectorXd velocityLowerLimits = skeleton->getVelocityLowerLimits(); - VectorXd velocityUpperLimits = skeleton->getVelocityUpperLimits(); + VectorXd positionLowerLimits = skeleton->getPositionLowerLimits(); + VectorXd positionUpperLimits = skeleton->getPositionUpperLimits(); + VectorXd velocityLowerLimits = jointVelocityLowerLimits; + VectorXd velocityUpperLimits = jointVelocityUpperLimits; - auto currentState = _stateSpace->createState(); - _stateSpace->convertPositionsToState(positions, currentState); + auto currentState = stateSpace->createState(); + stateSpace->convertPositionsToState(positions, currentState); - for (std::size_t i = 0; i < numDofs; ++i) + const auto problem = std::make_shared(numDofs); + if (enforceJointVelocityLimits) { - const double nextPositionLowerBound - = positions[i] + _timestep * velocityLowerLimits[i]; - const double nextPositionUpperBound - = positions[i] + _timestep * velocityUpperLimits[i]; - - const double paddedPositionLowerLimit = positionLowerLimits[i] + _padding; - const double paddedPositionUpperLimit = positionUpperLimits[i] - _padding; - - if (nextPositionLowerBound < paddedPositionLowerLimit) + for (std::size_t i = 0; i < numDofs; ++i) { - const double feasibleVelocityLowerLimit - = (paddedPositionLowerLimit - positions[i]) / _timestep; - velocityLowerLimits[i] - = std::max(velocityLowerLimits[i], feasibleVelocityLowerLimit); + const double position = positions[i]; + const double positionLowerLimit = positionLowerLimits[i]; + const double positionUpperLimit = positionUpperLimits[i]; + const double velocityLowerLimit = velocityLowerLimits[i]; + const double velocityUpperLimit = velocityUpperLimits[i]; + + if (position + stepSize * velocityLowerLimit + <= positionLowerLimit + jointLimitPadding) + { + velocityLowerLimits[i] = 0.0; + } + + if (position + stepSize * velocityUpperLimit + >= positionUpperLimit - jointLimitPadding) + { + velocityUpperLimits[i] = 0.0; + } + + initialGuess[i] = common::clamp( + initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]); } - - if (nextPositionUpperBound > paddedPositionUpperLimit) - { - const double feasibleVelocityUpperLimit - = (paddedPositionUpperLimit - positions[i]) / _timestep; - velocityUpperLimits[i] - = std::min(velocityUpperLimits[i], feasibleVelocityUpperLimit); - } - - initialGuess[i] = common::clamp( - initialGuess[i], velocityLowerLimits[i], velocityUpperLimits[i]); + problem->setLowerBounds(velocityLowerLimits); + problem->setUpperBounds(velocityUpperLimits); } - const auto problem = std::make_shared(numDofs); - problem->setLowerBounds(velocityLowerLimits); - problem->setUpperBounds(velocityUpperLimits); problem->setInitialGuess(initialGuess); problem->setObjective( - std::make_shared(_desiredTwist, jacobian)); + std::make_shared(desiredTwist, jacobian)); dart::optimizer::NloptSolver solver(problem, nlopt::LD_LBFGS); if (!solver.solve()) { return false; } - double optimalVal = problem->getOptimumValue(); - if (optimalVal > _optimizationTolerance) - { - return false; - } - - _jointVelocity = problem->getOptimalSolution(); + jointVelocity = problem->getOptimalSolution(); return true; } +//============================================================================== +Eigen::Vector6d computeGeodesicTwist( + const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans) +{ + using dart::math::logMap; + Eigen::Isometry3d relativeTrans = fromTrans.inverse() * toTrans; + Eigen::Vector3d relativeTranslation + = fromTrans.linear() * relativeTrans.translation(); + Eigen::Vector3d axisAngles = logMap(relativeTrans.linear()); + Eigen::Vector3d relativeAngles = fromTrans.linear() * axisAngles; + Eigen::Vector6d geodesicTwist; + geodesicTwist << relativeAngles, relativeTranslation; + return geodesicTwist; +} + +//============================================================================== +Eigen::Vector4d computeGeodesicError( + const Eigen::Isometry3d& fromTrans, const Eigen::Isometry3d& toTrans) +{ + using dart::math::logMap; + Eigen::Isometry3d relativeTrans = fromTrans.inverse() * toTrans; + Eigen::Vector3d relativeTranslation + = fromTrans.linear() * relativeTrans.translation(); + Eigen::Vector3d axisAngles = logMap(relativeTrans.linear()); + Eigen::Vector4d geodesicError; + geodesicError << axisAngles.norm(), relativeTranslation; + return geodesicError; +} + +//============================================================================== +double computeGeodesicDistance( + const Eigen::Isometry3d& fromTrans, + const Eigen::Isometry3d& toTrans, + double r) +{ + Eigen::Vector4d error = computeGeodesicError(fromTrans, toTrans); + error[0] = r * error[0]; + return error.norm(); +} + } // namespace vectorfield } // namespace planner } // namespace aikido diff --git a/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp b/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp new file mode 100644 index 0000000000..41bfab0d3c --- /dev/null +++ b/src/planner/vectorfield/detail/VectorFieldIntegrator.cpp @@ -0,0 +1,161 @@ +#include "VectorFieldIntegrator.hpp" +#include +#include +#include +#include +#include +#include "VectorFieldPlannerExceptions.hpp" + +namespace aikido { +namespace planner { +namespace vectorfield { +namespace detail { + +constexpr double defaultConstraintCheckResolution = 1e-3; + +//============================================================================== +std::unique_ptr convertToSpline( + const std::vector& knots, + aikido::statespace::ConstStateSpacePtr stateSpace) +{ + using dart::common::make_unique; + + std::size_t dimension = stateSpace->getDimension(); + + // TODO: const cast will be removed after the spline constructor updated. + auto nonConstStateSpace + = std::const_pointer_cast(stateSpace); + auto outputTrajectory + = make_unique(nonConstStateSpace); + + using CubicSplineProblem = aikido::common:: + SplineProblem; + + const Eigen::VectorXd zeroPosition = Eigen::VectorXd::Zero(dimension); + auto currState = stateSpace->createState(); + for (std::size_t iknot = 0; iknot < knots.size() - 1; ++iknot) + { + const double segmentDuration = knots[iknot + 1].mT - knots[iknot].mT; + Eigen::VectorXd currentPosition = knots[iknot].mPositions; + Eigen::VectorXd nextPosition = knots[iknot + 1].mPositions; + + CubicSplineProblem problem( + Eigen::Vector2d{0., segmentDuration}, 2, dimension); + problem.addConstantConstraint(0, 0, zeroPosition); + problem.addConstantConstraint(1, 0, nextPosition - currentPosition); + const auto solution = problem.fit(); + const auto coefficients = solution.getCoefficients().front(); + + stateSpace->expMap(currentPosition, currState); + outputTrajectory->addSegment(coefficients, segmentDuration, currState); + } + return outputTrajectory; +} + +//============================================================================== +VectorFieldIntegrator::VectorFieldIntegrator( + const VectorField* vectorField, + const aikido::constraint::Testable* collisionFreeConstraint, + double timelimit, + double checkConstraintResolution) + : mVectorField(vectorField) + , mConstraint(collisionFreeConstraint) + , mTimelimit(timelimit) + , mConstraintCheckResolution(checkConstraintResolution) +{ + mCacheIndex = -1; + mLastEvaluationTime = 0.0; + mDimension = mVectorField->getStateSpace()->getDimension(); + mState = mVectorField->getStateSpace()->createState(); +} + +//============================================================================== +void VectorFieldIntegrator::start() +{ + mTimer.start(); + mKnots.clear(); + mCacheIndex = -1; + mLastEvaluationTime = 0.0; +} + +//============================================================================== +int VectorFieldIntegrator::getCacheIndex() +{ + return mCacheIndex; +} + +//============================================================================== +std::vector& VectorFieldIntegrator::getKnots() +{ + return mKnots; +} + +double VectorFieldIntegrator::getLastEvaluationTime() +{ + return mLastEvaluationTime; +} + +//============================================================================== +void VectorFieldIntegrator::step( + const Eigen::VectorXd& q, Eigen::VectorXd& qd, double /*_t*/) +{ + mVectorField->getStateSpace()->expMap(q, mState); + + // compute joint velocities + bool success = mVectorField->evaluateVelocity(mState, qd); + if (!success) + { + throw IntegrationFailedError(); + } +} + +//============================================================================== +void VectorFieldIntegrator::check(const Eigen::VectorXd& q, double t) +{ + if (mTimer.getElapsedTime() > mTimelimit) + { + throw TimeLimitError(); + } + + mVectorField->getStateSpace()->expMap(q, mState); + + Knot knot; + knot.mT = t; + knot.mPositions = q; + mKnots.push_back(knot); + + if (mKnots.size() > 1) + { + auto trajSegment = convertToSpline(mKnots, mVectorField->getStateSpace()); + + if (!mVectorField->evaluateTrajectory( + *trajSegment, + mConstraint, + mConstraintCheckResolution, + mLastEvaluationTime, + true)) + { + throw ConstraintViolatedError(); + } + } + + VectorFieldPlannerStatus status = mVectorField->evaluateStatus(mState); + + if (status == VectorFieldPlannerStatus::CACHE_AND_CONTINUE + || status == VectorFieldPlannerStatus::CACHE_AND_TERMINATE) + { + mCacheIndex = mKnots.size(); + } + + if (status == VectorFieldPlannerStatus::TERMINATE + || status == VectorFieldPlannerStatus::CACHE_AND_TERMINATE) + { + throw VectorFieldTerminated( + "Planning was terminated by the StatusCallback."); + } +} + +} // namesapce detail +} // namespace vectorfield +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp b/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp new file mode 100644 index 0000000000..07881dbc39 --- /dev/null +++ b/src/planner/vectorfield/detail/VectorFieldIntegrator.hpp @@ -0,0 +1,130 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDINTEGRATOR_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDINTEGRATOR_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace planner { +namespace vectorfield { +namespace detail { + +struct Knot +{ + /// Timestamp. + double mT; + + /// Positions. + Eigen::VectorXd mPositions; +}; + +/// Convert a sequence of waypoint and time pairs into a trajectory. +/// +/// \param[in] A seqeunce of waypoint and time pairs. +/// \param[in] stateSpace State space of output trajectory. +/// \return A trajectory. +std::unique_ptr convertToSpline( + const std::vector& knots, + aikido::statespace::ConstStateSpacePtr stateSpace); + + + +/// VectorField Planner generates a trajectory by following a vector field +/// defined in joint space. +/// A planned trajectroy ends either by a predefined termination criterion or a +/// integral time. +/// +/// This class defines two callback functions for a integrator. +/// step() provides joint velocities the vector field planner should follow, +/// check() is called after each integration step to check planner status. +class VectorFieldIntegrator +{ +public: + /// Constructor. + /// + /// \param[in] vectorField Vector field in configuration space. + /// \param[in] constraint Constraint to be satisfied. + /// \param[in] timelimit Timelimit for integration calculation. + /// \param[in] checkConstraintResolution Resolution used in checking + /// constraint satisfaction in generated trajectory. + VectorFieldIntegrator( + const VectorField* vectorField, + const aikido::constraint::Testable* constraint, + double timelimit, + double checkConstraintResolution); + + /// Called before doing integration. + /// + void start(); + + /// Get cache index. + /// + /// \return Cache index. + int getCacheIndex(); + + /// Get a list of knots stored in the integration + /// + /// \return a list of knots. + std::vector& getKnots(); + + /// Get last evaluation time. + /// + /// \return last evaluation time. + double getLastEvaluationTime(); + + /// Vectorfield callback function that returns joint velocities for + /// integration. + /// + /// \param[in] q Position in configuration space. + /// \param[out] qd Joint velocities in configuration space. + /// \param[in] t Current time being planned. + void step(const Eigen::VectorXd& q, Eigen::VectorXd& qd, double t); + + /// Check status after every integration step. + /// + /// \param[in] q Position in configuration space. + /// \param[in] t Current time being planned. + void check(const Eigen::VectorXd& q, double t); + +protected: + /// Vector field + const VectorField* mVectorField; + + /// Constraint to be satisfied + const aikido::constraint::Testable* mConstraint; + + /// Cached index of knots. + int mCacheIndex; + + /// Dimension of state space. + std::size_t mDimension; + + /// Timer for timelimit. + dart::common::Timer mTimer; + + /// Planning timelimit. + double mTimelimit; + + std::vector mKnots; + + /// Resolution used in checking constraint satisfaction. + double mConstraintCheckResolution; + + /// Current state in integration + aikido::statespace::StateSpace::State* mState; + + /// Last evaluation time in checking trajectory + double mLastEvaluationTime; +}; + +} // namespace detail +} // namespace vectorfield +} // namespace planner +} // namespace aikido + +#endif // AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDINTEGRATOR_HPP_ diff --git a/src/planner/vectorfield/detail/VectorFieldPlannerExceptions.cpp b/src/planner/vectorfield/detail/VectorFieldPlannerExceptions.cpp new file mode 100644 index 0000000000..0e75feacd3 --- /dev/null +++ b/src/planner/vectorfield/detail/VectorFieldPlannerExceptions.cpp @@ -0,0 +1,51 @@ +#include "VectorFieldPlannerExceptions.hpp" + +namespace aikido { +namespace planner { +namespace vectorfield { +namespace detail { + +//============================================================================== +VectorFieldTerminated::VectorFieldTerminated(const std::string& whatArg) + : mWhatArg(whatArg) +{ + // Do nothing +} + +//============================================================================== +const char* VectorFieldTerminated::what() const noexcept +{ + return mWhatArg.c_str(); +} + +//============================================================================== +VectorFieldError::VectorFieldError(const std::string& whatArg) + : std::runtime_error(whatArg) +{ + // Do nothing +} + +//============================================================================== +ConstraintViolatedError::ConstraintViolatedError() + : VectorFieldTerminated("Constraint violated.") +{ + // Do nothing +} + +//============================================================================== +IntegrationFailedError::IntegrationFailedError() + : VectorFieldTerminated("Integation failed.") +{ + // Do nothing +} + +//============================================================================== +TimeLimitError::TimeLimitError() : VectorFieldTerminated("Reached time limit.") +{ + // DO nothing +} + +} // detail namespace +} // namespace vectorfield +} // namespace planner +} // namespace aikido diff --git a/src/planner/vectorfield/detail/VectorFieldPlannerExceptions.hpp b/src/planner/vectorfield/detail/VectorFieldPlannerExceptions.hpp new file mode 100644 index 0000000000..e0038b75f4 --- /dev/null +++ b/src/planner/vectorfield/detail/VectorFieldPlannerExceptions.hpp @@ -0,0 +1,74 @@ +#ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_ +#define AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_ + +#include +#include +#include + +namespace aikido { +namespace planner { +namespace vectorfield { +namespace detail { + +/// Define termination of vectorfield planner. +// When VectorFieldTerminated is throws, it means +// that planning is finished. +class VectorFieldTerminated : public std::exception +{ +public: + /// Constructor. + /// + /// \param[in] _whatArg Error string. + explicit VectorFieldTerminated(const std::string& whatArg); + + virtual const char* what() const noexcept; + +protected: + std::string mWhatArg; +}; + +/// Define runtime error of vector field planner. +// When VectorFieldError is thrown, it means that +// planning encounters an error. +class VectorFieldError : public std::runtime_error +{ +public: + /// Constructor. + /// + /// \param[in] whatArg Error string. + VectorFieldError(const std::string& whatArg); +}; + +/// Define state in collision for the termination of vector field planner. +class ConstraintViolatedError : public VectorFieldTerminated +{ +public: + /// Constructor. + /// + ConstraintViolatedError(); +}; + +/// Define integration failure for the termination of vectorfield planner. +class IntegrationFailedError : public VectorFieldTerminated +{ +public: + /// Constructor. + /// + IntegrationFailedError(); +}; + +/// Define time limit error for the termination of vectorfield planner. +class TimeLimitError : public VectorFieldTerminated +{ +public: + /// Constructor. + /// + TimeLimitError(); +}; + +} // namespace detail +} // namespace vectorfield +} // namespace aikido +} // namespace planner + +#endif // ifndef AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_ diff --git a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp index df5e2ad026..5ca2f4e695 100644 --- a/tests/planner/vectorfield/test_VectorFieldPlanner.cpp +++ b/tests/planner/vectorfield/test_VectorFieldPlanner.cpp @@ -4,7 +4,7 @@ #include #include #include -#include +#include #include #include #include @@ -62,16 +62,14 @@ class VectorFieldPlannerTest : public ::testing::Test VectorFieldPlannerTest() : mErrorTolerance(1e-3) { - SkeletonPtr skel - = createNLinkRobot(3, Eigen::Vector3d(0.2, 0.2, 1.0), BALL); + SkeletonPtr skel = createNLinkRobot(); mNumDof = skel->getNumDofs(); - mLinearVelocity = 0.2; mUpperPositionLimits = Eigen::VectorXd::Constant(mNumDof, constantsd::pi()); mLowerPositionLimits = Eigen::VectorXd::Constant(mNumDof, -constantsd::pi()); - mUpperVelocityLimits = Eigen::VectorXd::Constant(mNumDof, 2.0); - mLowerVelocityLimits = Eigen::VectorXd::Constant(mNumDof, -2.0); + mUpperVelocityLimits = Eigen::VectorXd::Constant(mNumDof, 4.0); + mLowerVelocityLimits = Eigen::VectorXd::Constant(mNumDof, -4.0); skel->setPositionUpperLimits(mUpperPositionLimits); skel->setPositionLowerLimits(mLowerPositionLimits); @@ -82,7 +80,7 @@ class VectorFieldPlannerTest : public ::testing::Test mStartConfig = Eigen::VectorXd::Zero(mNumDof); mStartConfig << 2.13746, -0.663612, 1.77876, 1.87515, 2.58646, -1.90034, - -1.03533, 1.68534, -1.39628; + -1.03533; mGoalConfig = Eigen::VectorXd(mNumDof); mPassingConstraint = std::make_shared(mStateSpace); @@ -141,67 +139,37 @@ class VectorFieldPlannerTest : public ::testing::Test return newComponent; } - /// Add an end-effector to the last link of the given robot - void addEndEffector(SkeletonPtr _robot, BodyNode* _parentNode, Vector3d _dim) - { - // Create the end-effector node with a random dimension - BodyNode::Properties node(BodyNode::AspectProperties("ee")); - std::shared_ptr shape - = std::make_shared(Vector3d(0.2, 0.2, 0.2)); - - Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); - T.translate(Eigen::Vector3d(0.0, 0.0, _dim(2))); - Joint::Properties joint("eeJoint", T); - - auto pair = _robot->createJointAndBodyNodePair( - _parentNode, joint, node); - auto bodyNode = pair.second; - bodyNode - ->createShapeNodeWith( - shape); - } - //============================================================================== /// Creates a N link manipulator with the given dimensions where each joint is /// the specified type - SkeletonPtr createNLinkRobot( - int _n, Vector3d _dim, TypeOfDOF _type, bool _finished = false) + SkeletonPtr createNLinkRobot() { - assert(_n > 0); + Vector3d dim(0.1, 0.1, 0.2); SkeletonPtr robot = Skeleton::create(); // robot->disableSelfCollision(); // Create the first link, the joint with the ground and its shape - BodyNode::Properties node(BodyNode::AspectProperties("link1")); + BodyNode::Properties node(BodyNode::AspectProperties("link0")); // node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); - std::shared_ptr shape(new BoxShape(_dim)); + std::shared_ptr shape(new BoxShape(dim)); std::pair pair1; - if (BALL == _type) - { - BallJoint::Base::Properties properties(std::string("joint1")); - pair1 = robot->createJointAndBodyNodePair( - nullptr, BallJoint::Properties(properties), node); - } - else - { - pair1 = add1DofJoint( - robot, - nullptr, - node, - "joint1", - 0.0, - -constantsd::pi(), - constantsd::pi(), - _type); - } + pair1 = add1DofJoint( + robot, + nullptr, + node, + "joint0", + 0.0, + -constantsd::pi(), + constantsd::pi(), + DOF_ROLL); Joint* joint = pair1.first; Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); // T.translation() = Eigen::Vector3d(0.0, 0.0, -0.5*dim(2)); // joint->setTransformFromParentBodyNode(T); - T.translation() = Eigen::Vector3d(0.0, 0.0, 0.5 * _dim(2)); + T.translation() = Eigen::Vector3d(0.0, 0.0, 0.5 * dim(2)); joint->setTransformFromChildBodyNode(T); // joint->setDampingCoefficient(0, 0.01); @@ -213,7 +181,7 @@ class VectorFieldPlannerTest : public ::testing::Test BodyNode* parentNode = currentNode; // Create links iteratively - for (int i = 1; i < _n; ++i) + for (int i = 1; i < 7; ++i) { std::ostringstream ssLink; std::ostringstream ssJoint; @@ -222,34 +190,30 @@ class VectorFieldPlannerTest : public ::testing::Test node = BodyNode::Properties(BodyNode::AspectProperties(ssLink.str())); // node.mInertia.setLocalCOM(Vector3d(0.0, 0.0, dim(2)/2.0)); - shape = std::shared_ptr(new BoxShape(_dim)); + shape = std::shared_ptr(new BoxShape(dim)); std::pair newPair; - if (BALL == _type) - { - BallJoint::Base::Properties properties(ssJoint.str()); - newPair = robot->createJointAndBodyNodePair( - parentNode, BallJoint::Properties(properties), node); - } + TypeOfDOF type; + if (i % 2 == 1) + type = DOF_PITCH; else - { - newPair = add1DofJoint( - robot, - parentNode, - node, - ssJoint.str(), - 0.0, - -constantsd::pi(), - constantsd::pi(), - _type); - } + type = DOF_ROLL; + newPair = add1DofJoint( + robot, + parentNode, + node, + ssJoint.str(), + 0.0, + -constantsd::pi(), + constantsd::pi(), + type); Joint* joint = newPair.first; Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); - T.translation() = Eigen::Vector3d(0.0, 0.0, -0.5 * _dim(2)); + T.translation() = Eigen::Vector3d(0.0, 0.0, -0.5 * dim(2)); joint->setTransformFromParentBodyNode(T); - T.translation() = Eigen::Vector3d(0.0, 0.0, 0.5 * _dim(2)); + T.translation() = Eigen::Vector3d(0.0, 0.0, 0.5 * dim(2)); joint->setTransformFromChildBodyNode(T); // joint->setDampingCoefficient(0, 0.01); @@ -262,8 +226,7 @@ class VectorFieldPlannerTest : public ::testing::Test } // If finished, initialize the skeleton - if (_finished) - addEndEffector(robot, parentNode, _dim); + // addEndEffector(robot, parentNode, dim); return robot; } @@ -294,59 +257,74 @@ TEST_F(VectorFieldPlannerTest, ComputeJointVelocityFromTwistTest) { using aikido::planner::vectorfield::computeJointVelocityFromTwist; - Eigen::VectorXd currentConfig = Eigen::VectorXd::Random(mNumDof); - mStateSpace->getMetaSkeleton()->setPositions(currentConfig); + Eigen::VectorXd jointVelocityUpperLimits + = mStateSpace->getMetaSkeleton()->getVelocityUpperLimits(); + Eigen::VectorXd jointVelocityLowerLimits + = mStateSpace->getMetaSkeleton()->getVelocityLowerLimits(); - Eigen::Isometry3d currentTrans = mBodynode->getTransform(); - Eigen::Vector3d currentTranslation = currentTrans.translation(); + double maxStepSize = 0.01; + double padding = 2e-3; - Eigen::Vector6d desiredTwist; - Eigen::Vector3d linearVel = Eigen::Vector3d::Zero(); - Eigen::Vector3d angularVel = Eigen::Vector3d::Zero(); - linearVel << 0.5, 0.5, 0.5; - desiredTwist.head<3>() = angularVel; - desiredTwist.tail<3>() = linearVel; + // Linear only + Eigen::Vector6d desiredTwist1 = Eigen::Vector6d::Zero(); + desiredTwist1.tail<3>() << 1.0, 1.0, 1.0; - double timestep = 0.01; - double padding = 1e-3; - double optimizationTolerance = 1e-10; Eigen::VectorXd qd = Eigen::VectorXd::Zero(mNumDof); EXPECT_TRUE( computeJointVelocityFromTwist( - desiredTwist, + qd, + desiredTwist1, + mStateSpace, + mBodynode, + padding, + jointVelocityLowerLimits, + jointVelocityUpperLimits, + true, + maxStepSize)); + + // Angular only + Eigen::Vector6d desiredTwist2 = Eigen::Vector6d::Zero(); + desiredTwist2.head<3>() << 1.0, 1.0, 1.0; + + EXPECT_TRUE( + computeJointVelocityFromTwist( + qd, + desiredTwist2, mStateSpace, mBodynode, - optimizationTolerance, - timestep, padding, - qd)); - - Eigen::VectorXd nextConfig(mNumDof); - auto currentState = mStateSpace->createState(); - mStateSpace->convertPositionsToState(currentConfig, currentState); - auto deltaState = mStateSpace->createState(); - mStateSpace->convertPositionsToState(timestep * qd, deltaState); - auto nextState = mStateSpace->createState(); - mStateSpace->compose(currentState, deltaState, nextState); - mStateSpace->convertStateToPositions(nextState, nextConfig); - mStateSpace->getMetaSkeleton()->setPositions(nextConfig); - - Eigen::Isometry3d nextTrans = mBodynode->getTransform(); - Eigen::Vector3d nextTranslation = nextTrans.translation(); - - Eigen::Vector3d linearDelta = nextTranslation - currentTranslation; - Eigen::Vector3d expectedLinearDelta = linearVel * timestep; - - double tolerance = 1e-3; - EXPECT_TRUE((linearDelta - expectedLinearDelta).norm() < tolerance); + jointVelocityLowerLimits, + jointVelocityUpperLimits, + true, + maxStepSize)); + + // Both linear and angular + Eigen::Vector6d desiredTwist3 = Eigen::Vector6d::Zero(); + desiredTwist3.head<3>() << 1.0, 1.0, 1.0; + desiredTwist3.tail<3>() << 1.0, 1.0, 1.0; + + EXPECT_TRUE( + computeJointVelocityFromTwist( + qd, + desiredTwist2, + mStateSpace, + mBodynode, + padding, + jointVelocityLowerLimits, + jointVelocityUpperLimits, + true, + maxStepSize)); } TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) { + using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField; + Eigen::Vector3d direction; direction << 1., 1., 0.; direction.normalize(); - double distance = 0.5; + double minDistance = 0.4; + double maxDistance = 0.42; mStateSpace->getMetaSkeleton()->setPositions(mStartConfig); auto startState = mStateSpace->createState(); @@ -356,22 +334,24 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) double positionTolerance = 0.01; double angularTolerance = 0.15; - double timestep = 0.02; - double linearGain = 1. / timestep; - double angularGain = 0.; // not trying to correct angular deviation + double initialStepSize = 0.001; + double jointLimitTolerance = 1e-3; + double constraintCheckResolution = 1e-3; + std::chrono::duration timelimit(5.); auto traj = aikido::planner::vectorfield::planToEndEffectorOffset( mStateSpace, mBodynode, mPassingConstraint, direction, - distance, - mLinearVelocity, + minDistance, + maxDistance, positionTolerance, angularTolerance, - linearGain, - angularGain, - timestep); + initialStepSize, + jointLimitTolerance, + constraintCheckResolution, + timelimit); EXPECT_FALSE(traj == nullptr) << "Trajectory not found"; @@ -406,7 +386,7 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) Eigen::Vector3d waypointVec = waypointTrans.translation(); Eigen::Vector3d linear_error - = startVec + direction * distance - waypointVec; + = startVec + direction * minDistance - waypointVec; Eigen::Vector3d const linear_orthogonal_error = linear_error - linear_error.dot(direction) * direction; double const linear_orthogonal_magnitude = linear_orthogonal_error.norm(); @@ -421,13 +401,23 @@ TEST_F(VectorFieldPlannerTest, PlanToEndEffectorOffsetTest) = (endTrans.translation() - startTrans.translation()).norm(); // Verify the moving distance - EXPECT_NEAR(movedDistance, distance, positionTolerance); + EXPECT_TRUE(movedDistance >= minDistance); + EXPECT_TRUE(movedDistance <= maxDistance); } TEST_F(VectorFieldPlannerTest, DirectionZeroVector) { + using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField; + 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; mStateSpace->getMetaSkeleton()->setPositions(mStartConfig); @@ -438,6 +428,70 @@ TEST_F(VectorFieldPlannerTest, DirectionZeroVector) mPassingConstraint, direction, distance, - mLinearVelocity), + maxDistance, + positionTolerance, + angularTolerance, + initialStepSize, + jointLimitTolerance, + constraintCheckResolution, + timelimit), std::runtime_error); } + +TEST_F(VectorFieldPlannerTest, PlanToEndEffectorPoseTest) +{ + using dart::math::eulerXYXToMatrix; + using aikido::planner::vectorfield::computeGeodesicDistance; + using aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField; + + Eigen::VectorXd goalConfig = Eigen::VectorXd::Zero(mNumDof); + goalConfig << 1.13746, -0.363612, 0.77876, 1.07515, 1.58646, -1.00034, + -0.03533; + mStateSpace->getMetaSkeleton()->setPositions(goalConfig); + Eigen::Isometry3d targetPose = mBodynode->getTransform(); + + double poseErrorTolerance = 0.01; + double initialStepSize = 0.05; + double r = 1.0; + double jointLimitTolerance = 1e-3; + double constraintCheckResolution = 1e-3; + std::chrono::duration timelimit(5.); + + mStateSpace->getMetaSkeleton()->setPositions(mStartConfig); + + auto traj1 = aikido::planner::vectorfield::planToEndEffectorPose( + mStateSpace, + mBodynode, + mPassingConstraint, + targetPose, + poseErrorTolerance, + r, + initialStepSize, + jointLimitTolerance, + constraintCheckResolution, + timelimit); + + EXPECT_FALSE(traj1 == nullptr) << "Trajectory not found"; + + if (traj1 == nullptr) + { + return; + } + + // Extract and evalute first waypoint + auto firstWayPoint = mStateSpace->createState(); + traj1->evaluate(0.0, firstWayPoint); + Eigen::VectorXd testStart(mNumDof); + mStateSpace->convertStateToPositions(firstWayPoint, testStart); + + double errorTolerance = 1e-3; + EXPECT_TRUE((testStart - mStartConfig).norm() <= errorTolerance); + + auto endpoint = mStateSpace->createState(); + traj1->evaluate(traj1->getEndTime(), endpoint); + mStateSpace->setState(endpoint); + Eigen::Isometry3d endTrans = mBodynode->getTransform(); + + double poseError = computeGeodesicDistance(endTrans, targetPose, r); + EXPECT_TRUE(poseError <= poseErrorTolerance); +}