-
Notifications
You must be signed in to change notification settings - Fork 30
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
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
- Loading branch information
Showing
24 changed files
with
1,730 additions
and
827 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
126 changes: 126 additions & 0 deletions
126
include/aikido/planner/vectorfield/BodyNodePoseVectorField.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,126 @@ | ||
| #ifndef AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_ | ||
| #define AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_ | ||
|
|
||
| #include <dart/dynamics/BodyNode.hpp> | ||
| #include <aikido/planner/vectorfield/VectorField.hpp> | ||
| #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp> | ||
|
|
||
| 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<BodyNodePoseVectorField>; | ||
|
|
||
| } // namespace vectorfield | ||
| } // namespace planner | ||
| } // namespace aikido | ||
|
|
||
| #endif // AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_ |
120 changes: 65 additions & 55 deletions
120
include/aikido/planner/vectorfield/MoveEndEffectorOffsetVectorField.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
70 changes: 70 additions & 0 deletions
70
include/aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,70 @@ | ||
| #ifndef AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_ | ||
| #define AIKIDO_PLANNER_VECTORFIELD_MOVEENDEFFECTORPOSEVECTORFIELD_HPP_ | ||
|
|
||
| #include <aikido/planner/vectorfield/BodyNodePoseVectorField.hpp> | ||
|
|
||
| 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_ |
Oops, something went wrong.