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. More...

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

Inheritance diagram for aikido::planner::vectorfield::BodyNodePoseVectorField:

Public Member Functions
	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...

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

virtual VectorFieldPlannerStatus	evaluateCartesianStatus (const Eigen::Isometry3d &pose) const =0
	Evaluate current pose to determine the status of planning. 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
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

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.

Constructor & Destructor Documentation

◆ BodyNodePoseVectorField()

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.

Parameters

[in]	metaSkeletonStateSpace	MetaSkeleton state space.
[in]	metaSkeleton	MetaSkeleton to plan with
[in]	bodyNode	Body node of end-effector.
[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.
[in]	enforceJointVelocityLimits	Whether joint velocity limits are considered in computation.

Member Function Documentation

◆ evaluateCartesianStatus()

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

pure virtual

Evaluate current pose to determine the status of planning.

Parameters

[in] pose Current pose of body node.

Returns: Status of planner.

Implemented in aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField, and aikido::planner::vectorfield::MoveEndEffectorPoseVectorField.

◆ evaluateCartesianVelocity()

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

pure virtual

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).

Implemented in aikido::planner::vectorfield::MoveEndEffectorOffsetVectorField, and aikido::planner::vectorfield::MoveEndEffectorPoseVectorField.

◆ evaluateStatus()

VectorFieldPlannerStatus aikido::planner::vectorfield::BodyNodePoseVectorField::evaluateStatus ( const aikido::statespace::StateSpace::State * state ) const

overridevirtual

Vectorfield planning status callback function.

Parameters

[in] state State to evaluate.

Returns: Status of planning.

Implements aikido::planner::vectorfield::VectorField.

◆ evaluateTrajectory()

bool aikido::planner::vectorfield::BodyNodePoseVectorField::evaluateTrajectory	(	const aikido::trajectory::Trajectory &	trajectory,
		const aikido::constraint::Testable *	constraint,
		double	evalStepSize,
		double &	evalTimePivot,
		bool	includeEndTime
	)		const

overridevirtual

Evaludate whether a trajectory satisfies a constraint.

It is checked by a user-defined evaluation step size.

Parameters

[in]	trajectory	Trajectory to be evaluated.
[in]	constraint	Constraint to be satisfied.
[in]	evalStepSize	The step size used in evaluating constraint.
[in,out]	evalTimePivot	Input provides the start time of the trajectory to evaluate; output returns the end time of the trajectory evaluate.
[in]	includeEndTime	Whether end time is included in evaluation. evaluate. satisfaction.

Implements aikido::planner::vectorfield::VectorField.

◆ evaluateVelocity()

bool aikido::planner::vectorfield::BodyNodePoseVectorField::evaluateVelocity	(	const aikido::statespace::StateSpace::State *	state,
		Eigen::VectorXd &	qd
	)		const

overridevirtual

Vectorfield callback function.

Parameters

[in]	state	Statespace state.
[out]	qd	Joint velocities.

Returns: Whether joint velocities are successfully computed.

Implements aikido::planner::vectorfield::VectorField.

◆ getBodyNode()

::dart::dynamics::ConstBodyNodePtr aikido::planner::vectorfield::BodyNodePoseVectorField::getBodyNode ( ) const

Returns const body node of end-effector.

◆ getMetaSkeleton() [1/2]

::dart::dynamics::MetaSkeletonPtr aikido::planner::vectorfield::BodyNodePoseVectorField::getMetaSkeleton ( )

Returns meta skeleton.

◆ getMetaSkeleton() [2/2]

::dart::dynamics::ConstMetaSkeletonPtr aikido::planner::vectorfield::BodyNodePoseVectorField::getMetaSkeleton ( ) const

Returns const meta skeleton.

◆ getMetaSkeletonStateSpace()

aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr aikido::planner::vectorfield::BodyNodePoseVectorField::getMetaSkeletonStateSpace ( ) const

Return const meta skeleton state space.

Member Data Documentation

◆ mBodyNode

::dart::dynamics::ConstBodyNodePtr aikido::planner::vectorfield::BodyNodePoseVectorField::mBodyNode

protected

BodyNode.

◆ mEnforceJointVelocityLimits

bool aikido::planner::vectorfield::BodyNodePoseVectorField::mEnforceJointVelocityLimits

protected

Enfoce joint velocity limits.

◆ mJointLimitPadding

double aikido::planner::vectorfield::BodyNodePoseVectorField::mJointLimitPadding

protected

Padding of joint limits.

◆ mMaxStepSize

double aikido::planner::vectorfield::BodyNodePoseVectorField::mMaxStepSize

protected

Maximum step size of integrator.

◆ mMetaSkeleton

::dart::dynamics::MetaSkeletonPtr aikido::planner::vectorfield::BodyNodePoseVectorField::mMetaSkeleton

protected

Meta Skeleton.

◆ mMetaSkeletonStateSpace

aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr aikido::planner::vectorfield::BodyNodePoseVectorField::mMetaSkeletonStateSpace

protected

Meta skeleton state space.

◆ mVelocityLowerLimits

Eigen::VectorXd aikido::planner::vectorfield::BodyNodePoseVectorField::mVelocityLowerLimits

protected

Joint velocities lower limits.

◆ mVelocityUpperLimits

Eigen::VectorXd aikido::planner::vectorfield::BodyNodePoseVectorField::mVelocityUpperLimits

protected

Joint velocities upper limits.

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ BodyNodePoseVectorField()

Member Function Documentation

◆ evaluateCartesianStatus()

◆ evaluateCartesianVelocity()

◆ evaluateStatus()

◆ evaluateTrajectory()

◆ evaluateVelocity()

◆ getBodyNode()

◆ getMetaSkeleton() [1/2]

◆ getMetaSkeleton() [2/2]

◆ getMetaSkeletonStateSpace()

Member Data Documentation

◆ mBodyNode

◆ mEnforceJointVelocityLimits

◆ mJointLimitPadding

◆ mMaxStepSize

◆ mMetaSkeleton

◆ mMetaSkeletonStateSpace

◆ mVelocityLowerLimits

◆ mVelocityUpperLimits