Implements a constrained RRT planner. More...

#include <aikido/planner/ompl/CRRT.hpp>

Inheritance diagram for aikido::planner::ompl::CRRT:

Classes
class	Motion
	Representation of a node in the tree. More...

Public Member Functions
	CRRT (const ::ompl::base::SpaceInformationPtr &_si)
	Constructor. More...

	CRRT (const ::ompl::base::SpaceInformationPtr &_si, const std::string &_name)
	Constructor. More...

virtual	~CRRT ()
	Destructor. More...

void	getPlannerData (::ompl::base::PlannerData &_data) const override
	Get information about the current run of the motion planner. More...

::ompl::base::PlannerStatus	solve (const ::ompl::base::PlannerTerminationCondition &_ptc) override
	Function that can solve the motion planning problem. More...

::ompl::base::PlannerStatus	solve (double _solveTime)
	Solve the motion planning problem in the given time. More...

void	clear () override
	Clear all internal datastructures. More...

void	setGoalBias (double _goalBias)
	Set the goal bias. More...

double	getGoalBias () const
	Get the goal bias the planner is using. More...

void	setRange (double _distance)
	Set the range the planner is supposed to use. More...

double	getRange () const
	Get the range the planner is using. More...

void	setPathConstraint (constraint::ProjectablePtr _projectable)
	Set a projectable constraint to be applied throughout the trajectory. More...

void	setProjectionResolution (double _resolution)
	Set the resolution for the projection. More...

double	getProjectionResolution () const
	Get the resolution for the projection. More...

void	setMaxProjectedStepsizeSlackFactor (double _slackFactor)
	Set the slack factor for the resolution of the final path after projection. More...

double	getMaxProjectedStepsizeSlackFactor () const
	Get the slack factor for the resolution of the final path after projection. More...

void	setMinStateDifference (double _mindist)
	Set the minimum distance between two states for them to be considered "equivalent". More...

double	getMinStateDifference () const
	Get the minimum distance between two states for them to be considered "equivalent". More...

template<template< typename T > class NN>
void	setNearestNeighbors ()
	Set a nearest neighbors data structure. More...

void	setup () override
	Perform extra configuration steps, if needed. More...

Protected Types
using	TreeData = ompl_shared_ptr<::ompl::NearestNeighbors< Motion * > >
	A nearest-neighbor datastructure representing a tree of motions *‍/. More...

Protected Member Functions
virtual void	freeMemory ()
	Free the memory allocated by this planner. More...

double	distanceFunction (const Motion a, const Motion b) const
	Compute distance between motions (actually distance between contained states. More...

Motion *	constrainedExtend (const ::ompl::base::PlannerTerminationCondition &ptc, TreeData &tree, Motion nmotion, ::ompl::base::State gstate, ::ompl::base::State xstate, ::ompl::base::Goal goal, bool returnlast, double &dist, bool &foundgoal)
	Perform an extension that projects to a constraint. More...

Protected Attributes
TreeData	mStartTree
	A nearest-neighbors datastructure containing the tree of motions. More...

::ompl::base::StateSamplerPtr	mSampler
	State sampler. More...

double	mGoalBias
	The fraction of time the goal is picked as the state to expand towards (if such a state is available) More...

double	mMaxDistance
	The maximum length of a motion to be added to a tree. More...

::ompl::RNG	mRng
	The random number generator used to determine whether to sample a goal state or a state uniformly from free space. More...

Motion *	mLastGoalMotion
	The most recent goal motion. Used for PlannerData computation. More...

constraint::ProjectablePtr	mCons
	The constraint that must be satisfied throughout the trajectory. More...

double	mMaxStepsize
	The maximum length of a step before projecting. More...

double	mMaxProjectedStepsizeSlackFactor
	We multiply this with `mMaxStepsize` to get the maximum length of a step after projecting: in other words, the resolution of the final path on the constraint manifold. More...

double	mMinStepsize
	The minumum step size along the constraint. More...

Detailed Description

Implements a constrained RRT planner.

Member Typedef Documentation

◆ TreeData

using aikido::planner::ompl::CRRT::TreeData = ompl_shared_ptr<::ompl::NearestNeighbors<Motion*> >

protected

A nearest-neighbor datastructure representing a tree of motions *‍/.

Constructor & Destructor Documentation

◆ CRRT() [1/2]

aikido::planner::ompl::CRRT::CRRT ( const ::ompl::base::SpaceInformationPtr & _si )

explicit

Constructor.

Parameters

_si	Information about the planning space

◆ CRRT() [2/2]

aikido::planner::ompl::CRRT::CRRT	(	const ::ompl::base::SpaceInformationPtr &	_si,
		const std::string &	_name
	)

Constructor.

Parameters

_si	Information about the planning space
_name	A name for this planner

◆ ~CRRT()

virtual aikido::planner::ompl::CRRT::~CRRT ( )

virtual

Destructor.

Member Function Documentation

◆ clear()

void aikido::planner::ompl::CRRT::clear ( )

override

Clear all internal datastructures.

Planner settings are not affected. Subsequent calls to solve() will ignore all previous work.

◆ constrainedExtend()

Motion* aikido::planner::ompl::CRRT::constrainedExtend	(	const ::ompl::base::PlannerTerminationCondition &	ptc,
		TreeData &	tree,
		Motion *	nmotion,
		::ompl::base::State *	gstate,
		::ompl::base::State *	xstate,
		::ompl::base::Goal *	goal,
		bool	returnlast,
		double &	dist,
		bool &	foundgoal
	)

protected

Perform an extension that projects to a constraint.

Parameters

	ptc	Planner termination conditions. Used to stop extending if planning time expires.
	tree	The tree to extend
	nmotion	The node in the tree to extend from
	gstate	The state the extension aims to reach
	xstate	A temporary state that can be used during extension
	goal	The goal of the planning instance
	returnlast	If true, return the last node added to the tree, otherwise return the node added that was nearest the goal
[out]	dist	The closest distance this extension got to the goal
[out]	foundgoal	True if the extension reached the goal.

Returns: fmotion If returnlast is true, the last node on the extension, otherwise the closest node along the extension to the goal

◆ distanceFunction()

double aikido::planner::ompl::CRRT::distanceFunction	(	const Motion *	a,
		const Motion *	b
	)		const

protected

Compute distance between motions (actually distance between contained states.

◆ freeMemory()

virtual void aikido::planner::ompl::CRRT::freeMemory ( )

protectedvirtual

Free the memory allocated by this planner.

Reimplemented in aikido::planner::ompl::CRRTConnect.

◆ getGoalBias()

double aikido::planner::ompl::CRRT::getGoalBias ( ) const

Get the goal bias the planner is using.

◆ getMaxProjectedStepsizeSlackFactor()

double aikido::planner::ompl::CRRT::getMaxProjectedStepsizeSlackFactor ( ) const

Get the slack factor for the resolution of the final path after projection.

During tree extension, any projection that would cause the final resolution of the path to exceed this factor times mMaxStepsize is rejected.

◆ getMinStateDifference()

double aikido::planner::ompl::CRRT::getMinStateDifference ( ) const

Get the minimum distance between two states for them to be considered "equivalent".

This is used during extension to determine if a projection is near enough the previous projection to say progress is no longer being made and quit extending.

◆ getPlannerData()

void aikido::planner::ompl::CRRT::getPlannerData ( ::ompl::base::PlannerData & _data ) const

override

Get information about the current run of the motion planner.

Repeated calls to this function will update data (only additions are made). This is useful to see what changed in the exploration datastructure, between calls to solve(), for example (without calling clear() in between).

Parameters

[out] _data Data about the current run of the motion planner

◆ getProjectionResolution()

double aikido::planner::ompl::CRRT::getProjectionResolution ( ) const

Get the resolution for the projection.

During tree extension, a projection back to the constraint will be performed after any step larger than this distance.

◆ getRange()

double aikido::planner::ompl::CRRT::getRange ( ) const

Get the range the planner is using.

◆ setGoalBias()

void aikido::planner::ompl::CRRT::setGoalBias ( double _goalBias )

Set the goal bias.

In the process of randomly selecting states in the state space to attempt to go towards, the algorithm may in fact choose the actual goal state, if it knows it, with some probability. This probability is a real number between 0.0 and 1.0; its value should usually be around 0.05 and should not be too large. It is probably a good idea to use the default value.

◆ setMaxProjectedStepsizeSlackFactor()

void aikido::planner::ompl::CRRT::setMaxProjectedStepsizeSlackFactor ( double _slackFactor )

Set the slack factor for the resolution of the final path after projection.

During tree extension, any projection that would cause the final resolution of the path to exceed this factor times mMaxStepsize is rejected.

Parameters

_slackFactor The slack factor enforced for the resolution of the final path.

◆ setMinStateDifference()

void aikido::planner::ompl::CRRT::setMinStateDifference ( double _mindist )

Set the minimum distance between two states for them to be considered "equivalent".

This is used during extension to determine if a projection is near enough the previous projection to say progress is no longer being made and quit extending.

◆ setNearestNeighbors()

template<template< typename T > class NN>

void aikido::planner::ompl::CRRT::setNearestNeighbors

Set a nearest neighbors data structure.

◆ setPathConstraint()

void aikido::planner::ompl::CRRT::setPathConstraint ( constraint::ProjectablePtr _projectable )

Set a projectable constraint to be applied throughout the trajectory.

The projection is applied at the resolution set via setProjectionResolution

Parameters

_projectable The constraint

◆ setProjectionResolution()

void aikido::planner::ompl::CRRT::setProjectionResolution ( double _resolution )

Set the resolution for the projection.

During tree extension, a projection back to the constraint will be performed after any step larger than this distance.

Parameters

_resolution The max step on an extension before projecting back to constraint

◆ setRange()

void aikido::planner::ompl::CRRT::setRange ( double _distance )

Set the range the planner is supposed to use.

This parameter greatly influences the runtime of the algorithm. It represents the maximum length of a motion to be added in the tree of motions.

Parameters

_distance The maximum length of a motion to be added in the tree of motions

◆ setup()

void aikido::planner::ompl::CRRT::setup ( )

override

Perform extra configuration steps, if needed.

This call will also issue a call to ompl::base::SpaceInformation::setup() if needed. This must be called before solving.

◆ solve() [1/2]

::ompl::base::PlannerStatus aikido::planner::ompl::CRRT::solve ( const ::ompl::base::PlannerTerminationCondition & _ptc )

override

Function that can solve the motion planning problem.

This function can be called multiple times on the same problem, without calling clear() in between. This allows the planner to continue work for more time on an unsolved problem, for example. If this option is used, it is assumed the problem definition is not changed (unpredictable results otherwise). The only change in the problem definition that is accounted for is the addition of starting or goal states (but not changing previously added start/goal states). The function terminates if the call to ptc returns true.

Parameters

_ptc	Conditions for terminating planning before a solution is found

◆ solve() [2/2]

::ompl::base::PlannerStatus aikido::planner::ompl::CRRT::solve ( double _solveTime )

Solve the motion planning problem in the given time.

Parameters

_solveTime The maximum allowable time to solve the planning problem

Member Data Documentation

◆ mCons

constraint::ProjectablePtr aikido::planner::ompl::CRRT::mCons

protected

The constraint that must be satisfied throughout the trajectory.

◆ mGoalBias

double aikido::planner::ompl::CRRT::mGoalBias

protected

The fraction of time the goal is picked as the state to expand towards (if such a state is available)

◆ mLastGoalMotion

Motion* aikido::planner::ompl::CRRT::mLastGoalMotion

protected

The most recent goal motion. Used for PlannerData computation.

◆ mMaxDistance

double aikido::planner::ompl::CRRT::mMaxDistance

protected

The maximum length of a motion to be added to a tree.

◆ mMaxProjectedStepsizeSlackFactor

double aikido::planner::ompl::CRRT::mMaxProjectedStepsizeSlackFactor

protected

We multiply this with mMaxStepsize to get the maximum length of a step after projecting: in other words, the resolution of the final path on the constraint manifold.

◆ mMaxStepsize

double aikido::planner::ompl::CRRT::mMaxStepsize

protected

The maximum length of a step before projecting.

◆ mMinStepsize

double aikido::planner::ompl::CRRT::mMinStepsize

protected

The minumum step size along the constraint.

Used to determine when projection is no longer making progress during an extension.

◆ mRng

::ompl::RNG aikido::planner::ompl::CRRT::mRng

protected

The random number generator used to determine whether to sample a goal state or a state uniformly from free space.

◆ mSampler

::ompl::base::StateSamplerPtr aikido::planner::ompl::CRRT::mSampler

protected

State sampler.

◆ mStartTree

TreeData aikido::planner::ompl::CRRT::mStartTree

protected

A nearest-neighbors datastructure containing the tree of motions.

Classes

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ TreeData

Constructor & Destructor Documentation

◆ CRRT() [1/2]

◆ CRRT() [2/2]

◆ ~CRRT()

Member Function Documentation

◆ clear()

◆ constrainedExtend()

◆ distanceFunction()

◆ freeMemory()

◆ getGoalBias()

◆ getMaxProjectedStepsizeSlackFactor()

◆ getMinStateDifference()

◆ getPlannerData()

◆ getProjectionResolution()

◆ getRange()

◆ setGoalBias()

◆ setMaxProjectedStepsizeSlackFactor()

◆ setMinStateDifference()

◆ setNearestNeighbors()

◆ setPathConstraint()

◆ setProjectionResolution()

◆ setRange()

◆ setup()

◆ solve() [1/2]

◆ solve() [2/2]

Member Data Documentation

◆ mCons

◆ mGoalBias

◆ mLastGoalMotion

◆ mMaxDistance

◆ mMaxProjectedStepsizeSlackFactor

◆ mMaxStepsize

◆ mMinStepsize

◆ mRng

◆ mSampler

◆ mStartTree