Helper for common operations during planning and evaluation. More...

#include <PlannerUtils.hpp>

Public Member Functions
	PlannerUtils ()=delete

Static Public Member Functions
template<typename N >
static double	slope (const N &x1, const N &y1, const N &x2, const N &y2)
	Compute the angular slope between two points. More...

static double	slope (const Point &a, const Point &b)
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

static double	slope (const ompl::base::State a, const ompl::base::State b)
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

static bool	equals (const ompl::base::State a, const ompl::base::State b)
	Check whether two states are equal (up to some tolerance). More...

static std::vector< Point >	toSteeredPoints (const ob::State a, const ob::State b)

static bool	collides (const std::vector< Point > &path)
	Check whether points on a path collide with the environment. More...

static bool	collides (const ompl::geometric::PathGeometric &path)
	Collision check that respects the collision model. More...

static bool	collides (const ompl::base::State a, const ompl::base::State b)
	Collision check of segment between two states. More...

static bool	collides (const ompl::base::State a, const ompl::base::State b, og::PathGeometric &path)
	Collision check that respects the collision model. More...

static bool	collides (const std::vector< Point > &path, std::vector< Point > &collisions)
	Check whether points on a path collide with the environment. More...

static bool	collides (const ompl::base::State a, const ompl::base::State b, std::vector< Point > &collisions)
	Collision check of segment between two states. More...

static ompl::geometric::PathGeometric	interpolated (ompl::geometric::PathGeometric path)
	Interpolate path based on its associated state space. More...

static ompl::control::PathControl	interpolated (ompl::control::PathControl path)
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More...

static void	updateAngles (ompl::geometric::PathGeometric &path, bool AverageAngles=true, bool preventCollisions=true)

static void	gradientDescent (ompl::geometric::PathGeometric &path, unsigned int rounds, double eta, double discount=1.)

static void	gradientDescent (std::vector< Point > &path, unsigned int rounds, double eta, double discount=1.)

static std::vector< Point >	linearInterpolate (const Point &a, const Point &b, double dt=0.1)
	Linearly interpolate between two points. More...

static std::vector< Point >	linearInterpolate (ompl::base::State a, ompl::base::State b, double dt=0.1)
	This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.This does not take into account yaw and returns a list of xy points. More...

static ompl::base::State *	closestPoint (Point x, const std::vector< Point > &points)
	Find closest collision-free point in an ordered set of points. More...

static double	totalLength (const std::vector< Point > &path)
	Compute total length of a path. More...

static std::vector< Point >	equidistantSampling (const std::vector< Point > &path, size_t targetSize)
	Compute equidistance samples along path. More...

template<typename N >
static std::string	num2str (const N &v, unsigned int precision=3)
	Convert a number to a string with the given precision. More...

static double	normalizeAngle (double angle)
	Normalize angle in radians to the interval \((-\pi, \pi]\). More...

Detailed Description

Helper for common operations during planning and evaluation.

Contains static functions for collision checking, and other path evaluations.

Constructor & Destructor Documentation

◆ PlannerUtils()

PlannerUtils::PlannerUtils ( )

delete

Member Function Documentation

◆ closestPoint()

static ompl::base::State * PlannerUtils::closestPoint	(	Point	x,
		const std::vector< Point > &	points
	)

inlinestatic

Find closest collision-free point in an ordered set of points.

The closest point will be converted to a state, whose yaw is computed based on neighboring points.

Parameters

x	The reference point.
points	The collection of points to check.

Returns: The state closest to x.

◆ collides() [1/6]

static bool PlannerUtils::collides	(	const ompl::base::State *	a,
		const ompl::base::State *	b
	)

inlinestatic

Collision check of segment between two states.

The collision model is set in PlannerSettings::GlobalSettings::EnvironmentSettings::CollisionSettings::collision_model. The environment is set through PlannerSettings::GlobalSettings::environment. The steering function used to compute the path between the two state is defined by PlannerSettings::GlobalSettings::OmplSettings::state_space.

Parameters

a	The first state of the path segment to check.
b	The last state of the path segment to check.

Returns: True if the segment collides with the environment, otherwise false.

◆ collides() [2/6]

static bool PlannerUtils::collides	(	const ompl::base::State *	a,
		const ompl::base::State *	b,
		og::PathGeometric &	path
	)

inlinestatic

Collision check that respects the collision model.

The path is interpolated prior to collision checking.

The collision model is set in PlannerSettings::GlobalSettings::EnvironmentSettings::CollisionSettings::collision_model. The environment is set through PlannerSettings::GlobalSettings::environment. Interpolation is based on the state space of the path.

Parameters

a	Deprecated.
b	Deprecated.
The	OMPL path to check.

Returns: True if the path collides with the environment, otherwise false.

◆ collides() [3/6]

static bool PlannerUtils::collides	(	const ompl::base::State *	a,
		const ompl::base::State *	b,
		std::vector< Point > &	collisions
	)

inlinestatic

Collision check of segment between two states.

The collision model is set in PlannerSettings::GlobalSettings::EnvironmentSettings::CollisionSettings::collision_model. The environment is set through PlannerSettings::GlobalSettings::environment. The steering function used to compute the path between the two state is defined by PlannerSettings::GlobalSettings::OmplSettings::state_space.

Parameters

a	The first state of the path segment to check.
b	The last state of the path segment to check.
collisions	Points that collide with the environment.

Returns: True if the segment collides with the environment, otherwise false.

◆ collides() [4/6]

static bool PlannerUtils::collides ( const ompl::geometric::PathGeometric & path )

inlinestatic

Collision check that respects the collision model.

No additional interpolation along the path is performed prior to collision checking.

The collision model is set in PlannerSettings::GlobalSettings::EnvironmentSettings::CollisionSettings::collision_model. The environment is set through PlannerSettings::GlobalSettings::environment.

Parameters

The	OMPL path to check.

Returns: True if the path collides with the environment, otherwise false.

◆ collides() [5/6]

static bool PlannerUtils::collides ( const std::vector< Point > & path )

inlinestatic

Check whether points on a path collide with the environment.

This function does not consider the collision model, since points only contain x and y coordinates. The environment is set through PlannerSettings::GlobalSettings::environment.

Parameters

path	The points on the path.

Returns: True if the path collides with the environment, false otherwise.

◆ collides() [6/6]

static bool PlannerUtils::collides	(	const std::vector< Point > &	path,
		std::vector< Point > &	collisions
	)

inlinestatic

Check whether points on a path collide with the environment.

This function does not consider the collision model, since points only contain x and y coordinates. The environment is set through PlannerSettings::GlobalSettings::environment.

Parameters

path	The points on the path.
collisions	Points that collide with the environment.

Returns: True if the path collides with the environment, false otherwise.

◆ equals()

static bool PlannerUtils::equals	(	const ompl::base::State *	a,
		const ompl::base::State *	b
	)

inlinestatic

Check whether two states are equal (up to some tolerance).

Check compares x, y, and yaw difference. All have to be within tolerance for states to be considered equal. Tolerance is set in PlannerSettings::GlobalSettings::OmplSettings::state_equality_tolerance.

Parameters

a	The first state.
b	The second state.

Returns: True if the states are equal, false otherwise.

◆ equidistantSampling()

static std::vector< Point > PlannerUtils::equidistantSampling	(	const std::vector< Point > &	path,
		size_t	targetSize
	)

inlinestatic

Compute equidistance samples along path.

Parameters

path	The input path.
targetSize	The number of equdistant samples on the output path.

Returns: The path of size targetSize with equidistant samples.

◆ gradientDescent() [1/2]

static void PlannerUtils::gradientDescent	(	ompl::geometric::PathGeometric &	path,
		unsigned int	rounds,
		double	eta,
		double	discount = `1.`
	)

inlinestatic

◆ gradientDescent() [2/2]

static void PlannerUtils::gradientDescent	(	std::vector< Point > &	path,
		unsigned int	rounds,
		double	eta,
		double	discount = `1.`
	)

inlinestatic

◆ interpolated() [1/2]

static ompl::control::PathControl PlannerUtils::interpolated ( ompl::control::PathControl path )

inlinestatic

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Parameters

The	control path to interpolate.

◆ interpolated() [2/2]

static ompl::geometric::PathGeometric PlannerUtils::interpolated ( ompl::geometric::PathGeometric path )

inlinestatic

Interpolate path based on its associated state space.

Parameters

The	geometric path to interpolate.

Returns: The interpolated path.

◆ linearInterpolate() [1/2]

static std::vector< Point > PlannerUtils::linearInterpolate	(	const Point &	a,
		const Point &	b,
		double	dt = `0.1`
	)

inlinestatic

Linearly interpolate between two points.

Parameters

a	The first point.
b	The last point.
dt	The step to take (where \(t\) parametrizes the line, i.e., \(t\in[0,1]\)).

Returns: The points on the line from a to b, including both of these points.

◆ linearInterpolate() [2/2]

static std::vector< Point > PlannerUtils::linearInterpolate	(	ompl::base::State *	a,
		ompl::base::State *	b,
		double	dt = `0.1`
	)

inlinestatic

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.This does not take into account yaw and returns a list of xy points.

Parameters

a	The first state.
b	The last state.
dt	The step to take (where \(t\) parametrizes the line, i.e., \(t\in[0,1]\)).

Returns: The points on the line from a to b, including both of these points.

◆ normalizeAngle()

static double PlannerUtils::normalizeAngle ( double angle )

inlinestatic

Normalize angle in radians to the interval \((-\pi, \pi]\).

Parameters

angle The angle to normalize.

Returns: The normalized angle.

◆ num2str()

template<typename N >

static std::string PlannerUtils::num2str	(	const N &	v,
		unsigned int	precision = `3`
	)

inlinestatic

Convert a number to a string with the given precision.

Parameters

v	The number to convert to a string.
precision	The precision used when converting the number to a string.

Returns: The string containing the number.

◆ slope() [1/3]

template<typename N >

static double PlannerUtils::slope	(	const N &	x1,
		const N &	y1,
		const N &	x2,
		const N &	y2
	)

inlinestatic

Compute the angular slope between two points.

The slope will be zero, for example, for a line from \((0,0)\) to \((1,0)\). The slope will be \(\frac{\pi}{2}\), for example, for a line from \((0,0)\) to \((0,1)\).

Parameters

x1	X-coordinate of first point.
y1	Y-coordinate of first point.
x2	X-coorindate of second point.
y2	Y-coordinate of second point.

Returns: Angular slope of the line in the interval \((-\pi,\pi]\)

◆ slope() [2/3]

static double PlannerUtils::slope	(	const ompl::base::State *	a,
		const ompl::base::State *	b
	)

inlinestatic

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Parameters

a	The state containing the first point.
b	The state containing the second point.

◆ slope() [3/3]

static double PlannerUtils::slope	(	const Point &	a,
		const Point &	b
	)

inlinestatic

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

Parameters

a	The first point.
b	The second point.

◆ toSteeredPoints()

static std::vector< Point > PlannerUtils::toSteeredPoints	(	const ob::State *	a,
		const ob::State *	b
	)

inlinestatic

◆ totalLength()

static double PlannerUtils::totalLength ( const std::vector< Point > & path )

inlinestatic

Compute total length of a path.

The length between two points is calculated as Euclidean distance. The total length is computed as the sum of pairwise distances.

Parameters

path	The ordered points along the path.

Returns: The total length of the path as the sum of pairwise Euclidean distances.

◆ updateAngles()

static void PlannerUtils::updateAngles	(	ompl::geometric::PathGeometric &	path,
		bool	AverageAngles = `true`,
		bool	preventCollisions = `true`
	)

inlinestatic

The documentation for this class was generated from the following file:

src/utils/PlannerUtils.hpp

Public Member Functions

Static Public Member Functions

Detailed Description

Constructor & Destructor Documentation

◆ PlannerUtils()

Member Function Documentation

◆ closestPoint()

◆ collides() [1/6]

◆ collides() [2/6]

◆ collides() [3/6]

◆ collides() [4/6]

◆ collides() [5/6]

◆ collides() [6/6]

◆ equals()

◆ equidistantSampling()

◆ gradientDescent() [1/2]

◆ gradientDescent() [2/2]

◆ interpolated() [1/2]

◆ interpolated() [2/2]

◆ linearInterpolate() [1/2]

◆ linearInterpolate() [2/2]

◆ normalizeAngle()

◆ num2str()

◆ slope() [1/3]

◆ slope() [2/3]

◆ slope() [3/3]

◆ toSteeredPoints()

◆ totalLength()

◆ updateAngles()