Vector field for moving end-effector to a goal pose in SE(3). More...

#include <aikido/planner/vectorfield/MoveEndEffectorPoseVectorField.hpp>

Inheritance diagram for aikido::planner::vectorfield::MoveEndEffectorPoseVectorField:

Public Member Functions
	MoveEndEffectorPoseVectorField (aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr stateSpace, ::dart::dynamics::MetaSkeletonPtr metaskeleton, ::dart::dynamics::ConstBodyNodePtr bn, const Eigen::Isometry3d &goalPose, double goalTolerance, double r, double positionTolerance, double angularTolerance, double maxStepSize, double jointLimitPadding)
	Constructor. More...

bool	evaluateCartesianVelocity (const Eigen::Isometry3d &pose, Eigen::Vector6d &cartesianVelocity) const override
	Evaluate Cartesian velocity by current pose of body node. More...

VectorFieldPlannerStatus	evaluateCartesianStatus (const Eigen::Isometry3d &pose) const override
	Evaluate current pose to determine the status of planning. More...

Public Member Functions inherited from aikido::planner::vectorfield::BodyNodePoseVectorField
	BodyNodePoseVectorField (aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr metaSkeletonStateSpace, ::dart::dynamics::MetaSkeletonPtr metaSkeleton, ::dart::dynamics::ConstBodyNodePtr bodyNode, double maxStepSize, double jointLimitPadding, bool enforceJointVelocityLimits=false)
	Constructor. More...

bool	evaluateVelocity (const aikido::statespace::StateSpace::State *state, Eigen::VectorXd &qd) const override
	Vectorfield callback function. More...

VectorFieldPlannerStatus	evaluateStatus (const aikido::statespace::StateSpace::State *state) const override
	Vectorfield planning status callback function. More...

bool	evaluateTrajectory (const aikido::trajectory::Trajectory &trajectory, const aikido::constraint::Testable *constraint, double evalStepSize, double &evalTimePivot, bool includeEndTime) const override
	Evaludate whether a trajectory satisfies a constraint. More...

aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr	getMetaSkeletonStateSpace () const
	Return const meta skeleton state space. More...

::dart::dynamics::MetaSkeletonPtr	getMetaSkeleton ()
	Returns meta skeleton. More...

::dart::dynamics::ConstMetaSkeletonPtr	getMetaSkeleton () const
	Returns const meta skeleton. More...

::dart::dynamics::ConstBodyNodePtr	getBodyNode () const
	Returns const body node of end-effector. More...

Public Member Functions inherited from aikido::planner::vectorfield::VectorField
	VectorField (aikido::statespace::ConstStateSpacePtr stateSpace)
	Constructor. More...

virtual	~VectorField ()=default
	Destructor. More...

aikido::statespace::ConstStateSpacePtr	getStateSpace ()
	Returns state space. More...

aikido::statespace::ConstStateSpacePtr	getStateSpace () const
	Returns const state space. More...

Protected Attributes
Eigen::Isometry3d	mGoalPose
	Goal pose. More...

double	mGoalTolerance
	Tolerance of pose error. More...

double	mConversionRatioFromRadianToMeter
	Conversion ratio from radius to meter. More...

double	mPositionTolerance
	Tolerance of linear deviation error. More...

double	mAngularTolerance
	Tolerance of angular error. More...

Eigen::Isometry3d	mStartPose
	Start pose of the end-effector. More...

Eigen::Vector6d	mDesiredTwist
	Cache for desired twist between start and goal. More...

Eigen::Vector3d	mDirection

Eigen::Vector3d	mRotation

std::shared_ptr< double >	mLastPoseError
	Cache of last pose error. More...

Protected Attributes inherited from aikido::planner::vectorfield::BodyNodePoseVectorField
aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr	mMetaSkeletonStateSpace
	Meta skeleton state space. More...

::dart::dynamics::MetaSkeletonPtr	mMetaSkeleton
	Meta Skeleton. More...

::dart::dynamics::ConstBodyNodePtr	mBodyNode
	BodyNode. More...

double	mMaxStepSize
	Maximum step size of integrator. More...

double	mJointLimitPadding
	Padding of joint limits. More...

Eigen::VectorXd	mVelocityLowerLimits
	Joint velocities lower limits. More...

Eigen::VectorXd	mVelocityUpperLimits
	Joint velocities upper limits. More...

bool	mEnforceJointVelocityLimits
	Enfoce joint velocity limits. More...

Protected Attributes inherited from aikido::planner::vectorfield::VectorField
aikido::statespace::ConstStateSpacePtr	mStateSpace
	State space. More...

Detailed Description

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.

Constructor & Destructor Documentation

◆ MoveEndEffectorPoseVectorField()

aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::MoveEndEffectorPoseVectorField	(	aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr	stateSpace,
		::dart::dynamics::MetaSkeletonPtr	metaskeleton,
		::dart::dynamics::ConstBodyNodePtr	bn,
		const Eigen::Isometry3d &	goalPose,
		double	goalTolerance,
		double	r,
		double	positionTolerance,
		double	angularTolerance,
		double	maxStepSize,
		double	jointLimitPadding
	)

Constructor.

Parameters

[in]	stateSpace	MetaSkeleton state space.
[in]	metaskeleton	MetaSkeleton to plan with
[in]	bn	Body node of end-effector.
[in]	goalPose	Desired end-effector pose.
[in]	goalTolerance	Geodesic Distance from goal to stop planning.
[in]	r	Conversion of radius to meters in computing goalTolerance.
[in]	positionTolerance	Straight-line constraint tolerance in meters.
[in]	angularTolerance	Rotation deviation tolerance in radians.
[in]	maxStepSize	The maximum step size used to guarantee that the integrator does not step out of joint limits.
[in]	jointLimitPadding	If less then this distance to joint limit, velocity is bounded in that direction to 0.

Member Function Documentation

◆ evaluateCartesianStatus()

VectorFieldPlannerStatus aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::evaluateCartesianStatus ( const Eigen::Isometry3d & pose ) const

overridevirtual

Evaluate current pose to determine the status of planning.

Parameters

[in] pose Current pose of body node.

Returns: Status of planner.

Implements aikido::planner::vectorfield::BodyNodePoseVectorField.

◆ evaluateCartesianVelocity()

bool aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::evaluateCartesianVelocity	(	const Eigen::Isometry3d &	pose,
		Eigen::Vector6d &	cartesianVelocity
	)		const

overridevirtual

Evaluate Cartesian velocity by current pose of body node.

Parameters

[in]	pose	Current pose of body node.
[out]	cartesianVelocity	Cartesian velocity defined in se(3).

Implements aikido::planner::vectorfield::BodyNodePoseVectorField.

Member Data Documentation

◆ mAngularTolerance

double aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mAngularTolerance

protected

Tolerance of angular error.

◆ mConversionRatioFromRadianToMeter

double aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mConversionRatioFromRadianToMeter

protected

Conversion ratio from radius to meter.

◆ mDesiredTwist

Eigen::Vector6d aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mDesiredTwist

protected

Cache for desired twist between start and goal.

◆ mDirection

Eigen::Vector3d aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mDirection

protected

◆ mGoalPose

Eigen::Isometry3d aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mGoalPose

protected

Goal pose.

◆ mGoalTolerance

double aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mGoalTolerance

protected

Tolerance of pose error.

◆ mLastPoseError

std::shared_ptr<double> aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mLastPoseError

protected

Cache of last pose error.

◆ mPositionTolerance

double aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mPositionTolerance

protected

Tolerance of linear deviation error.

◆ mRotation

Eigen::Vector3d aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mRotation

protected

◆ mStartPose

Eigen::Isometry3d aikido::planner::vectorfield::MoveEndEffectorPoseVectorField::mStartPose

protected

Start pose of the end-effector.

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ MoveEndEffectorPoseVectorField()

Member Function Documentation

◆ evaluateCartesianStatus()

◆ evaluateCartesianVelocity()

Member Data Documentation

◆ mAngularTolerance

◆ mConversionRatioFromRadianToMeter

◆ mDesiredTwist

◆ mDirection

◆ mGoalPose

◆ mGoalTolerance

◆ mLastPoseError

◆ mPositionTolerance

◆ mRotation

◆ mStartPose