The rapidly-expanding random tree implementation. More...

#include <RRT.hpp>

Public Types
enum	StepResult { STEP_COLLISION , STEP_REACHED , STEP_PROGRESS }
	To get byte-aligned Eigen vectors. More...

Public Member Functions
	RRT (dart::simulation::WorldPtr world, dart::dynamics::SkeletonPtr robot, const std::vector< std::size_t > &dofs, const Eigen::VectorXd &root, double stepSize=0.02)

	RRT (simulation::WorldPtr world, dynamics::SkeletonPtr robot, const std::vector< std::size_t > &dofs, const std::vector< Eigen::VectorXd > &roots, double stepSize=0.02)
	Constructor with multiple roots (so, multiple trees) More...

virtual	~RRT ()
	Destructor. More...

bool	connect ()
	Reach for a random node by repeatedly extending nodes from the nearest neighbor in the tree. More...

bool	connect (const Eigen::VectorXd &target)
	Reach for a target by repeatedly extending nodes from the nearest neighbor. Stop if collide. More...

StepResult	tryStep ()
	Try a single step with the given "stepSize" to a random configuration. Fail if collide. More...

StepResult	tryStep (const Eigen::VectorXd &qtry)
	Try a single step with the given "stepSize" to the given configuration. Fail if collide. More...

virtual StepResult	tryStepFromNode (const Eigen::VectorXd &qtry, int NNidx)
	Tries to extend tree towards provided sample. More...

virtual bool	newConfig (std::list< Eigen::VectorXd > &intermediatePoints, Eigen::VectorXd &qnew, const Eigen::VectorXd &qnear, const Eigen::VectorXd &qtarget)
	Checks if the given new configuration is in collision with an obstacle. More...

double	getGap (const Eigen::VectorXd &target)
	Returns the distance between the current active node and the given node. More...

void	tracePath (int node, std::list< Eigen::VectorXd > &path, bool reverse=false)
	Traces the path from some node to the initConfig node - useful in creating the full path after the goal is reached. More...

std::size_t	getSize ()
	Returns the number of nodes in the tree. More...

virtual bool	checkCollisions (const Eigen::VectorXd &c)
	Implementation-specific function for checking collisions. More...

virtual Eigen::VectorXd	getRandomConfig ()
	Returns a random configuration with the specified node IDs. More...

Public Attributes
const int	ndim
	Number of dof we can manipulate (may be less than robot's) More...

const double	stepSize
	Step size at each node creation. More...

int	activeNode
	Last added node or the nearest node found after a search. More...

std::vector< int >	parentVector
	The ith node in configVector has parent with index pV[i]. More...

std::vector< const Eigen::VectorXd * >	configVector
	All visited configs. More...

Protected Member Functions
double	randomInRange (double min, double max)
	Returns a random value between the given minimum and maximum value. More...

virtual int	getNearestNeighbor (const Eigen::VectorXd &qsamp)
	Returns the nearest neighbor to query point. More...

virtual int	addNode (const Eigen::VectorXd &qnew, int parentId)
	Adds a new node to the tree. More...

Protected Attributes
simulation::WorldPtr	world
	The world that the robot is in. More...

dynamics::SkeletonPtr	robot
	The ID of the robot for which a plan is generated. More...

std::vector< std::size_t >	dofs
	The dofs of the robot the planner can manipulate. More...

flann::Index< flann::L2< double > > *	index
	The underlying flann data structure for fast nearest neighbor searches. More...

Detailed Description

The rapidly-expanding random tree implementation.

Member Enumeration Documentation

◆ StepResult

enum dart::planning::RRT::StepResult

To get byte-aligned Eigen vectors.

The result of attempting to create a new node to reach a target node

Enumerator
STEP_COLLISION
STEP_REACHED
STEP_PROGRESS

Constructor & Destructor Documentation

◆ RRT() [1/2]

dart::planning::RRT::RRT	(	dart::simulation::WorldPtr	world,
		dart::dynamics::SkeletonPtr	robot,
		const std::vector< std::size_t > &	dofs,
		const Eigen::VectorXd &	root,
		double	stepSize = `0.02`
	)

◆ RRT() [2/2]

dart::planning::RRT::RRT	(	simulation::WorldPtr	world,
		dynamics::SkeletonPtr	robot,
		const std::vector< std::size_t > &	dofs,
		const std::vector< Eigen::VectorXd > &	roots,
		double	stepSize = `0.02`
	)

Constructor with multiple roots (so, multiple trees)

◆ ~RRT()

virtual dart::planning::RRT::~RRT ( )

inlinevirtual

Destructor.

Member Function Documentation

◆ addNode()

int dart::planning::RRT::addNode	(	const Eigen::VectorXd &	qnew,
		int	parentId
	)

protectedvirtual

Adds a new node to the tree.

◆ checkCollisions()

bool dart::planning::RRT::checkCollisions ( const Eigen::VectorXd & c )

virtual

Implementation-specific function for checking collisions.

◆ connect() [1/2]

bool dart::planning::RRT::connect ( )

Reach for a random node by repeatedly extending nodes from the nearest neighbor in the tree.

Stop if there is a collision.

◆ connect() [2/2]

bool dart::planning::RRT::connect ( const Eigen::VectorXd & target )

Reach for a target by repeatedly extending nodes from the nearest neighbor. Stop if collide.

◆ getGap()

double dart::planning::RRT::getGap ( const Eigen::VectorXd & target )

Returns the distance between the current active node and the given node.

TODO This might mislead the users to thinking returning the distance between the given target and the nearest neighbor.

◆ getNearestNeighbor()

int dart::planning::RRT::getNearestNeighbor ( const Eigen::VectorXd & qsamp )

inlineprotectedvirtual

Returns the nearest neighbor to query point.

◆ getRandomConfig()

VectorXd dart::planning::RRT::getRandomConfig ( )

virtual

Returns a random configuration with the specified node IDs.

◆ getSize()

std::size_t dart::planning::RRT::getSize ( )

Returns the number of nodes in the tree.

◆ newConfig()

bool dart::planning::RRT::newConfig	(	std::list< Eigen::VectorXd > &	intermediatePoints,
		Eigen::VectorXd &	qnew,
		const Eigen::VectorXd &	qnear,
		const Eigen::VectorXd &	qtarget
	)

virtual

Checks if the given new configuration is in collision with an obstacle.

Moreover, it is a an opportunity for child classes to change the new configuration if there is a need. For instance, task constrained planners might want to sample around this point and replace it with a better (less erroroneous due to constraint) node.

◆ randomInRange()

double dart::planning::RRT::randomInRange	(	double	min,
		double	max
	)

inlineprotected

Returns a random value between the given minimum and maximum value.

◆ tracePath()

void dart::planning::RRT::tracePath	(	int	node,
		std::list< Eigen::VectorXd > &	path,
		bool	reverse = `false`
	)

Traces the path from some node to the initConfig node - useful in creating the full path after the goal is reached.

◆ tryStep() [1/2]

RRT::StepResult dart::planning::RRT::tryStep ( )

Try a single step with the given "stepSize" to a random configuration. Fail if collide.

◆ tryStep() [2/2]

StepResult dart::planning::RRT::tryStep ( const Eigen::VectorXd & qtry )

Try a single step with the given "stepSize" to the given configuration. Fail if collide.

◆ tryStepFromNode()

RRT::StepResult dart::planning::RRT::tryStepFromNode	(	const Eigen::VectorXd &	qtry,
		int	NNidx
	)

virtual

Tries to extend tree towards provided sample.

Member Data Documentation

◆ activeNode

int dart::planning::RRT::activeNode

Last added node or the nearest node found after a search.

◆ configVector

std::vector<const Eigen::VectorXd*> dart::planning::RRT::configVector

All visited configs.

◆ dofs

std::vector<std::size_t> dart::planning::RRT::dofs

protected

The dofs of the robot the planner can manipulate.

◆ index

flann::Index<flann::L2<double> >* dart::planning::RRT::index

protected

The underlying flann data structure for fast nearest neighbor searches.

◆ ndim

const int dart::planning::RRT::ndim

Number of dof we can manipulate (may be less than robot's)

◆ parentVector

std::vector<int> dart::planning::RRT::parentVector

The ith node in configVector has parent with index pV[i].

◆ robot

dynamics::SkeletonPtr dart::planning::RRT::robot

protected

The ID of the robot for which a plan is generated.

◆ stepSize

const double dart::planning::RRT::stepSize

Step size at each node creation.

◆ world

simulation::WorldPtr dart::planning::RRT::world

protected

The world that the robot is in.

Public Types

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Member Enumeration Documentation

◆ StepResult

Constructor & Destructor Documentation

◆ RRT() [1/2]

◆ RRT() [2/2]

◆ ~RRT()

Member Function Documentation

◆ addNode()

◆ checkCollisions()

◆ connect() [1/2]

◆ connect() [2/2]

◆ getGap()

◆ getNearestNeighbor()

◆ getRandomConfig()

◆ getSize()

◆ newConfig()

◆ randomInRange()

◆ tracePath()

◆ tryStep() [1/2]

◆ tryStep() [2/2]

◆ tryStepFromNode()

Member Data Documentation

◆ activeNode

◆ configVector

◆ dofs

◆ index

◆ ndim

◆ parentVector

◆ robot

◆ stepSize

◆ world