Classes
struct	CRRTPlannerParameters

struct	VectorFieldPlannerParameters

Functions
trajectory::TrajectoryPtr	planToConfiguration (const statespace::dart::MetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const statespace::StateSpace::State goalState, const constraint::TestablePtr &collisionTestable, common::RNG rng, double timelimit)
	Plan the robot to a specific configuration. More...

trajectory::TrajectoryPtr	planToConfigurations (const statespace::dart::MetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const std::vector< statespace::StateSpace::State * > &goalStates, const constraint::TestablePtr &collisionTestable, common::RNG *rng, double timelimit)
	Plan the robot to a set of configurations. More...

trajectory::TrajectoryPtr	planToTSR (const statespace::dart::MetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const dart::dynamics::BodyNodePtr &bodyNode, const constraint::dart::TSRPtr &tsr, const constraint::TestablePtr &collisionTestable, common::RNG *rng, double timelimit, std::size_t maxNumTrials, const distance::ConstConfigurationRankerPtr &ranker=nullptr)
	Plan the configuration of the metakeleton such that the specified bodynode is set to a sample in TSR. More...

trajectory::InterpolatedPtr	planToTSRwithTrajectoryConstraint (const statespace::dart::MetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const dart::dynamics::BodyNodePtr &bodyNode, const constraint::dart::TSRPtr &goalTsr, const constraint::dart::TSRPtr &constraintTsr, const constraint::TestablePtr &collisionTestable, double timelimit, const CRRTPlannerParameters &crrtParameters=CRRTPlannerParameters())
	Returns a Trajectory that moves the configuration of the metakeleton such that the specified bodynode is set to a sample in a goal TSR and the trajectory is constrained to a constraint TSR Uses CRRTPlanner. More...

trajectory::TrajectoryPtr	planToEndEffectorOffset (const statespace::dart::MetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const dart::dynamics::BodyNodePtr &body, const Eigen::Vector3d &direction, const constraint::TestablePtr &collisionTestable, double distance, double timelimit, double positionTolerance=1e-3, double angularTolerance=1e-3, const VectorFieldPlannerParameters &vfParameters=VectorFieldPlannerParameters(), const CRRTPlannerParameters &crrtParameters=CRRTPlannerParameters())
	Plan to a desired end-effector offset with fixed orientation. More...

trajectory::InterpolatedPtr	planToEndEffectorOffsetByCRRT (const statespace::dart::MetaSkeletonStateSpacePtr &space, const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const dart::dynamics::BodyNodePtr &bodyNode, const constraint::TestablePtr &collisionTestable, const Eigen::Vector3d &direction, double distance, double timelimit, double positionTolerance=1e-3, double angularTolerance=1e-3, const CRRTPlannerParameters &crrtParameters=CRRTPlannerParameters())
	Plan to a desired end-effector offset with fixed orientation using CRRT. More...

std::unordered_map< std::string, const Eigen::VectorXd >	parseYAMLToNamedConfigurations (const YAML::Node &node)
	Parses YAML node for named configurtaions. More...

bool	getGoalAndConstraintTSRForEndEffectorOffset (const dart::dynamics::BodyNodePtr &bodyNode, const Eigen::Vector3d &direction, double distance, const constraint::dart::TSRPtr &goal, const constraint::dart::TSRPtr &constraint, double positionTolerance=1e-3, double angularTolerance=1e-3)
	Gets Goal and Constraint TSR for end-effector. More...

Eigen::Isometry3d	getLookAtIsometry (const Eigen::Vector3d &positionFrom, const Eigen::Vector3d &positionTo)
	Get a pose at a point looking at another point. More...

const dart::dynamics::BodyNode *	getBodyNodeOrThrow (const dart::dynamics::MetaSkeleton &skeleton, const std::string &bodyNodeName)
	Get a specific BodyNode of a MetaSkeleton or throw an execption if it doesn't exist. More...

dart::dynamics::BodyNode *	getBodyNodeOrThrow (dart::dynamics::MetaSkeleton &skeleton, const std::string &bodyNodeName)
	Get a specific BodyNode of a MetaSkeleton or throw an execption if it doesn't exist. More...

Function Documentation

◆ getBodyNodeOrThrow() [1/2]

const dart::dynamics::BodyNode* aikido::robot::util::getBodyNodeOrThrow	(	const dart::dynamics::MetaSkeleton &	skeleton,
		const std::string &	bodyNodeName
	)

Get a specific BodyNode of a MetaSkeleton or throw an execption if it doesn't exist.

Parameters

[in]	skeleton	MetaSkeleton that should contain the BodyNode
[in]	bodyNodeName	Name of the BodyNode we are looking for

Returns: The BodyNode

◆ getBodyNodeOrThrow() [2/2]

dart::dynamics::BodyNode* aikido::robot::util::getBodyNodeOrThrow	(	dart::dynamics::MetaSkeleton &	skeleton,
		const std::string &	bodyNodeName
	)

Get a specific BodyNode of a MetaSkeleton or throw an execption if it doesn't exist.

Parameters

[in]	skeleton	MetaSkeleton that should contain the BodyNode
[in]	bodyNodeName	Name of the BodyNode we are looking for

Returns: The BodyNode

◆ getGoalAndConstraintTSRForEndEffectorOffset()

bool aikido::robot::util::getGoalAndConstraintTSRForEndEffectorOffset	(	const dart::dynamics::BodyNodePtr &	bodyNode,
		const Eigen::Vector3d &	direction,
		double	distance,
		const constraint::dart::TSRPtr &	goal,
		const constraint::dart::TSRPtr &	constraint,
		double	positionTolerance = `1e-3`,
		double	angularTolerance = `1e-3`
	)

Gets Goal and Constraint TSR for end-effector.

Parameters

[in]	bodyNode	End-effector body node
[in]	direction	End-effector direction
[in]	distance	Offset to move
[out]	goal	Goal TSR
[out]	constraint	Constraint TSR
[in]	positionTolerance	Tolerance in position, used in TSR
[in]	angularTolerance	Tolerance in angle, used in TSR

Returns: True if all outputs are successfully made.

◆ getLookAtIsometry()

Eigen::Isometry3d aikido::robot::util::getLookAtIsometry	(	const Eigen::Vector3d &	positionFrom,
		const Eigen::Vector3d &	positionTo
	)

Get a pose at a point looking at another point.

Parameters

[in]	positionFrom	Position to look from
[in]	positionTo	Position to look to

Returns: Pose at positionFrom, looking at positionTo.

◆ parseYAMLToNamedConfigurations()

std::unordered_map<std::string, const Eigen::VectorXd> aikido::robot::util::parseYAMLToNamedConfigurations ( const YAML::Node & node )

Parses YAML node for named configurtaions.

Parameters

[in] node YAML node containing named configurations

Returns: Unordered map of (name, configuration) pairs.

◆ planToConfiguration()

trajectory::TrajectoryPtr aikido::robot::util::planToConfiguration	(	const statespace::dart::MetaSkeletonStateSpacePtr &	space,
		const dart::dynamics::MetaSkeletonPtr &	metaSkeleton,
		const statespace::StateSpace::State *	goalState,
		const constraint::TestablePtr &	collisionTestable,
		common::RNG *	rng,
		double	timelimit
	)

Plan the robot to a specific configuration.

Restores the robot to its initial configuration after planning.

Parameters

[in]	space	The StateSpace for the metaskeleton
[in]	metaSkeleton	MetaSkeleton to plan with.
[in]	goalState	Goal state
[in]	collisionTestable	Testable constraint to check for collision.
[in]	rng	Random number generator
[in]	timelimit	Max time to spend per planning to each IK

◆ planToConfigurations()

trajectory::TrajectoryPtr aikido::robot::util::planToConfigurations	(	const statespace::dart::MetaSkeletonStateSpacePtr &	space,
		const dart::dynamics::MetaSkeletonPtr &	metaSkeleton,
		const std::vector< statespace::StateSpace::State * > &	goalStates,
		const constraint::TestablePtr &	collisionTestable,
		common::RNG *	rng,
		double	timelimit
	)

Plan the robot to a set of configurations.

Restores the robot to its initial configuration after planning.

Parameters

[in]	space	The StateSpace for the metaskeleton
[in]	metaSkeleton	MetaSkeleton to plan with.
[in]	goalStates	Goal states
[in]	collisionTestable	Testable constraint to check for collision.
[in]	rng	Random number generator
[in]	timelimit	Max time to spend per planning to each IK

◆ planToEndEffectorOffset()

trajectory::TrajectoryPtr aikido::robot::util::planToEndEffectorOffset	(	const statespace::dart::MetaSkeletonStateSpacePtr &	space,
		const dart::dynamics::MetaSkeletonPtr &	metaSkeleton,
		const dart::dynamics::BodyNodePtr &	body,
		const Eigen::Vector3d &	direction,
		const constraint::TestablePtr &	collisionTestable,
		double	distance,
		double	timelimit,
		double	positionTolerance = `1e-3`,
		double	angularTolerance = `1e-3`,
		const VectorFieldPlannerParameters &	vfParameters = `VectorFieldPlannerParameters()`,
		const CRRTPlannerParameters &	crrtParameters = `CRRTPlannerParameters()`
	)

Plan to a desired end-effector offset with fixed orientation.

Parameters

[in]	space	StateSpace for the metaskeleton
[in]	metaSkeleton	Metaskeleton to plan with
[in]	body	Bodynode for the end-effector
[in]	direction	Direction unit vector in the world frame
[in]	collisionTestable	Collision constraint to check. Self-collision is checked by default.
[in]	distance	Distance distance to move, in meters
[in]	timelimit	Timelimit for planning
[in]	positionTolerance	Tolerance in position
[in]	angularTolerance	Tolerance in angle
[in]	vfParameters	VectorFieldPlanenr parameters
[in]	crrtParameters	CRRTPlanner parameters

Returns: Output trajectory

◆ planToEndEffectorOffsetByCRRT()

trajectory::InterpolatedPtr aikido::robot::util::planToEndEffectorOffsetByCRRT	(	const statespace::dart::MetaSkeletonStateSpacePtr &	space,
		const dart::dynamics::MetaSkeletonPtr &	metaSkeleton,
		const dart::dynamics::BodyNodePtr &	bodyNode,
		const constraint::TestablePtr &	collisionTestable,
		const Eigen::Vector3d &	direction,
		double	distance,
		double	timelimit,
		double	positionTolerance = `1e-3`,
		double	angularTolerance = `1e-3`,
		const CRRTPlannerParameters &	crrtParameters = `CRRTPlannerParameters()`
	)

Plan to a desired end-effector offset with fixed orientation using CRRT.

Parameters

[in]	space	StateSpace for the metaskeleton
[in]	metaSkeleton	Metaskeleton to plan with
[in]	bodyNode	Bodynode for the end-effector
[in]	direction	Direction unit vector in the world frame
[in]	collisionTestable	Collision constraint to check. Self-collision is checked by default.
[in]	distance	Distance distance to move, in meters
[in]	timelimit	Timelimit for planning
[in]	positionTolerance	Tolerance in position
[in]	angularTolerance	Tolerance in angle
[in]	crrtParameters	CRRTPlanner parameters

Returns: Output trajectory

◆ planToTSR()

trajectory::TrajectoryPtr aikido::robot::util::planToTSR	(	const statespace::dart::MetaSkeletonStateSpacePtr &	space,
		const dart::dynamics::MetaSkeletonPtr &	metaSkeleton,
		const dart::dynamics::BodyNodePtr &	bodyNode,
		const constraint::dart::TSRPtr &	tsr,
		const constraint::TestablePtr &	collisionTestable,
		common::RNG *	rng,
		double	timelimit,
		std::size_t	maxNumTrials,
		const distance::ConstConfigurationRankerPtr &	ranker = `nullptr`
	)

Plan the configuration of the metakeleton such that the specified bodynode is set to a sample in TSR.

Parameters

[in]	space	The StateSpace for the metaskeleton.
[in]	metaSkeleton	MetaSkeleton to plan with.
[in]	bodyNode	Bodynode whose frame for which TSR is constructed.
[in]	tsr	TSR to plan to.
[in]	collisionTestable	Testable constraint to check for collision.
[in]	rng	Random number generator
[in]	timelimit	Max time (seconds) to spend per planning to each IK
[in]	maxNumTrials	Number of retries before failure.
[in]	ranker	Ranker to rank the sampled configurations. If nullptr, NominalConfigurationRanker is used with the current metaSkeleton pose.

Returns: Trajectory to a sample in TSR, or nullptr if planning fails.

◆ planToTSRwithTrajectoryConstraint()

trajectory::InterpolatedPtr aikido::robot::util::planToTSRwithTrajectoryConstraint	(	const statespace::dart::MetaSkeletonStateSpacePtr &	space,
		const dart::dynamics::MetaSkeletonPtr &	metaSkeleton,
		const dart::dynamics::BodyNodePtr &	bodyNode,
		const constraint::dart::TSRPtr &	goalTsr,
		const constraint::dart::TSRPtr &	constraintTsr,
		const constraint::TestablePtr &	collisionTestable,
		double	timelimit,
		const CRRTPlannerParameters &	crrtParameters = `CRRTPlannerParameters()`
	)

Returns a Trajectory that moves the configuration of the metakeleton such that the specified bodynode is set to a sample in a goal TSR and the trajectory is constrained to a constraint TSR Uses CRRTPlanner.

Parameters

[in]	space	The StateSpace for the metaskeleton
[in]	metaSkeleton	MetaSkeleton to plan with.
[in]	bodyNode	Bodynode whose frame is meant for TSR
[in]	goalTsr	The goal TSR to move to
[in]	constraintTsr	The constraint TSR for the trajectory
[in]	collisionTestable	Testable constraint to check for collision.
[in]	timelimit	Timelimit for planning
[in]	crrtParameters	Parameters to use in planning.

Returns: Trajectory to a sample in TSR, or nullptr if planning fails.

Classes

Functions

Function Documentation

◆ getBodyNodeOrThrow() [1/2]

◆ getBodyNodeOrThrow() [2/2]

◆ getGoalAndConstraintTSRForEndEffectorOffset()

◆ getLookAtIsometry()

◆ parseYAMLToNamedConfigurations()

◆ planToConfiguration()

◆ planToConfigurations()

◆ planToEndEffectorOffset()

◆ planToEndEffectorOffsetByCRRT()

◆ planToTSR()

◆ planToTSRwithTrajectoryConstraint()