Namespaces
	cc_dubins
	cc-dubins path types: E (Empty), S (Straight), T (Turn)

	hc_cc_rs
	hc-/cc-reeds-shepp path types: E (Empty), S (Straight), T (Turn), c (Cusp)

Classes
class	CC00_Dubins_State_Space
	An implementation of continuous curvature (CC) steer for a Dubins car with zero curvature at the start and goal configuration as described in: T. Fraichard and A. Scheuer, "From Reeds and Shepp's to continuous-curvature paths," IEEE Transactions on Robotics (Volume 20, Issue: 6, Dec. 2004). It evaluates all Dubins families and returns the shortest path. More...

class	CC00_Reeds_Shepp_State_Space
	An implementation of continuous curvature (CC) steer for a Reeds-Shepp car with zero curvature at the start and goal configuration as described in: T. Fraichard and A. Scheuer, "From Reeds and Shepp's to continuous- curvature paths," IEEE Transactions on Robotics (Volume 20, Issue: 6, Dec. 2004). It evaluates all Reeds-Shepp families plus the four families TTT, TcST, TScT, TcScT, where "T" stands for a turn, "S" for a straight line and "c" for a cusp, and returns the shortest path. Topological paths are not included in this implementation. More...

class	CC0pm_Dubins_State_Space
	An implementation of continuous curvature (CC) steer for a Dubins car with zero curvature at the start and either positive (p) or negative (n) max. curvature at the goal configuration. It evaluates all Dubins families and returns the shortest path. More...

class	CC_Dubins_Path

class	CC_Dubins_State_Space
	An implementation of continuous curvature (CC) steer for a Dubins car with arbitrary curvature at the start and goal configuration. More...

class	CCpm0_Dubins_State_Space
	An implementation of continuous curvature (CC) steer for a Dubins car with either positive (p) or negative (n) max. curvature at the start and zero curvature at the goal configuration. It evaluates all Dubins families and returns the shortest path. More...

class	CCpmpm_Dubins_State_Space
	An implementation of continuous curvature (CC) steer for a Dubins car with either positive (p) or negative (n) max. curvature at the start and goal configuration. It evaluates all Dubins families plus the the family TTTT, where "T" stands for a turn, and returns the shortest path. More...

class	Configuration

struct	Control
	Description of a path segment with its corresponding control inputs. More...

struct	Controller
	Parameters of the feedback controller. More...

class	Dubins_State_Space
	An SE(2) state space where distance is measured by the length of Dubins curves. Note that this Dubins distance is not a proper distance metric, so nearest neighbor methods that rely on distance() being a metric (such as ompl::NearestNeighborsGNAT) will not always return the true nearest neighbors or get stuck in an infinite loop. The notation and solutions in the code are taken from: A.M. Shkel and V. Lumelsky, “Classification of the Dubins set,” Robotics and Autonomous Systems, 34(4):179-202, 2001. DOI: 10.1016/S0921-8890(00)00127-5 The classification scheme described there is not actually used, since it only applies to “long” paths. More...

class	EKF

class	HC00_Reeds_Shepp_State_Space
	An implementation of hybrid curvature (HC) steer with zero curvature at the start and goal configuration as described in: H. Banzhaf et al., "Hybrid Curvature Steer: A Novel Extend Function for Sampling-Based Non- holonomic Motion Planning in Tight Environments," IEEE International Conference on Intelligent Transportation Systems (Oct. 2017). It evaluates all Reeds-Shepp families plus the four families TTT, TcST, TScT, TcScT, where "T" stands for a turn, "S" for a straight line and "c" for a cusp, and returns the shortest path. More...

class	HC0pm_Reeds_Shepp_State_Space
	An implementation of hybrid curvature (HC) steer with zero curvature at the start configuration and either positive (p) or negative (n) max. curvature at the goal configuration, also see: H. Banzhaf et al., "Hybrid Curvature Steer: A Novel Extend Function for Sampling-Based Non- holonomic Motion Planning in Tight Environments," IEEE International Conference on Intelligent Transportation Systems (Oct. 2017). It evaluates all Reeds-Shepp families plus the four families TTT, TcST, TScT, TcScT, where "T" stands for a turn, "S" for a straight line and "c" for a cusp, and returns the shortest path. More...

class	HC_CC_Circle

class	HC_CC_Circle_Param

class	HC_CC_RS_Path

class	HC_CC_State_Space

class	HC_Reeds_Shepp_State_Space
	An implementation of hybrid curvature (HC) steer with arbitrary curvature at the start and goal configuration. More...

class	HCpm0_Reeds_Shepp_State_Space
	An implementation of hybrid curvature (HC) steer with either positive (p) or negative (n) max. curvature at the start configuration and zero curvature at the goal configuration, also see: H. Banzhaf et al., "Hybrid Curvature Steer: A Novel Extend Function for Sampling-Based Non- holonomic Motion Planning in Tight Environments," IEEE International Conference on Intelligent Transportation Systems (Oct. 2017). It evaluates all Reeds-Shepp families plus the four families TTT, TcST, TScT, TcScT, where "T" stands for a turn, "S" for a straight line and "c" for a cusp, and returns the shortest path. More...

class	HCpmpm_Reeds_Shepp_State_Space
	An implementation of hybrid curvature (HC) steer with either positive (p) or negative (n) max. curvature at the start and goal configuration as described in: H. Banzhaf et al., "Hybrid Curvature Steer: A Novel Extend Function for Sampling-Based Nonholonomic Motion Planning in Tight Environments," IEEE International Conference on Intelligent Transportation Systems (Oct. 2017). It evaluates all Reeds-Shepp families plus the four families TTT, TcST, TScT, TcScT, where "T" stands for a turn, "S" for a straight line and "c" for a cusp, and returns the shortest path. More...

struct	Measurement_Noise
	Parameters of the measurement noise. More...

struct	Motion_Noise
	Parameters of the motion noise model according to the book: Probabilistic Robotics, S. Thrun and others, MIT Press, 2006, p. 127-128 and p.204-206. More...

class	Path

class	Reeds_Shepp_State_Space
	An SE(2) state space where distance is measured by the length of Reeds-Shepp curves. The notation and solutions are taken from: J.A. Reeds and L.A. Shepp, “Optimal paths for a car that goes both forwards and backwards,” Pacific Journal of Mathematics, 145(2):367–393, 1990. This implementation explicitly computes all 48 Reeds-Shepp curves and returns the shortest valid solution. This can be improved by using the configuration space partition described in: P. Souères and J.-P. Laumond, “Shortest paths synthesis for a car-like robot,” IEEE Trans. on Automatic Control, 41(5):672–688, May 1996. More...

struct	State
	Description of a kinematic car's state. More...

struct	State_With_Covariance
	Description of a kinematic car's state with covariance. More...

Typedefs
typedef Eigen::Matrix< double, 2, 3 >	Matrix23d

typedef Eigen::Matrix< double, 2, 2 >	Matrix2d

typedef Eigen::Matrix< double, 3, 2 >	Matrix32d

typedef Eigen::Matrix< double, 3, 3 >	Matrix3d

Functions
int	array_index_min (double array[], int size)
	Find index with minimal value in double array. More...

void	cc_default_controls (const HC_CC_Circle &c, const Configuration &q, double delta, bool order, std::vector< Control > &controls)
	Appends controls with a default cc-turn consisting of two clothoids and a circular arc. More...

void	cc_default_controls (const HC_CC_Circle &c, const Configuration &q, double delta, bool order, vector< Control > &controls)

bool	cc_elementary_controls (const HC_CC_Circle &c, const Configuration &q, double delta, bool order, std::vector< Control > &controls)
	Appends controls with an elementary path if one exists. More...

bool	cc_elementary_controls (const HC_CC_Circle &c, const Configuration &q, double delta, bool order, vector< Control > &controls)

void	cc_turn_controls (const HC_CC_Circle &c, const Configuration &q, bool order, std::vector< Control > &controls)
	Appends controls with a cc-turn. More...

void	cc_turn_controls (const HC_CC_Circle &c, const Configuration &q, bool order, vector< Control > &controls)

double	center_distance (const HC_CC_Circle &c1, const HC_CC_Circle &c2)
	Cartesian distance between the centers of two circles. More...

bool	configuration_aligned (const Configuration &q1, const Configuration &q2)
	Are two configurations aligned? More...

double	configuration_distance (const Configuration &q1, const Configuration &q2)
	Cartesian distance between two configurations. More...

bool	configuration_equal (const Configuration &q1, const Configuration &q2)
	Are two configurations equal? More...

bool	configuration_on_hc_cc_circle (const HC_CC_Circle &c, const Configuration &q)
	Configuration on the circle? More...

int	direction (bool forward, bool order)

void	double_array_init (double array[], int size, double value)
	Initialize an array with a given value. More...

void	empty_controls (std::vector< Control > &controls)
	Appends controls with 0 input. More...

void	empty_controls (vector< Control > &controls)

void	end_of_circular_arc (double x_i, double y_i, double theta_i, double kappa, double direction, double length, double x_f, double y_f, double *theta_f)
	Computation of the end point on a circular arc x_i, y_i, theta_i: initial configuration kappa: curvature of circular arc direction: driving direction {-1.0, 1.0} length: arc length (positive) x_f, y_f, theta_f: final configuration on circular arc. More...

void	end_of_clothoid (double x_i, double y_i, double theta_i, double kappa_i, double sigma, double direction, double length, double x_f, double y_f, double theta_f, double kappa_f)
	Computation of the end point on a clothoid x_i, y_i, theta_i, kappa_i: initial configuration sigma: sharpness of clothoid direction: driving direction {-1.0, 1.0} length: length of clothoid (positive) x_f, y_f, theta_f, kappa_f: final configuration on clothoid. More...

void	end_of_straight_line (double x_i, double y_i, double theta, double direction, double length, double x_f, double y_f)
	Computation of the end point on a straight line x_i, y_i: initial configuration theta: angle of straight line direction: driving direction {-1.0, 1.0} length: line length (positive) x_f, y_f: final configuration on straight line. More...

void	fresnel (double s, double &S_f, double &C_f)
	Fresnel integrals: S_f = int_0_s(sin(pi/2 uu)du), C_f = int_0_s(cos(pi/2 uu)du) approximated with Chebyshev polynomials. More...

void	fresnel_0_8 (double x, double &S_f, double &C_f)

void	fresnel_8_inf (double x, double &S_f, double &C_f)

double	get_epsilon ()
	Return value of epsilon. More...

void	global_frame_change (double x, double y, double theta, double local_x, double local_y, double global_x, double global_y)
	Transformation of (local_x, local_y) from local coordinate system to global one. More...

void	hc_turn_controls (const HC_CC_Circle &c, const Configuration &q, bool order, std::vector< Control > &controls)
	Appends controls with a hc-turn. More...

void	hc_turn_controls (const HC_CC_Circle &c, const Configuration &q, bool order, vector< Control > &controls)

void	local_frame_change (double x, double y, double theta, double global_x, double global_y, double local_x, double local_y)
	Transformation of (global_x, global_y) from global coordinate system to local one. More...

double	pify (double alpha)
	Conversion of arbitrary angle given in [rad] to [-pi, pi[. More...

double	point_distance (double x1, double y1, double x2, double y2)
	Cartesian distance between two points. More...

void	pointer_array_init (void *array[], int size)
	Initialize an array with nullptr. More...

void	polar (double x, double y, double &r, double &theta)
	Computation of a point's polar coordinates. More...

void	reverse_control (Control &control)
	Reverses a control. More...

void	rs_turn_controls (const HC_CC_Circle &c, const Configuration &q, bool order, std::vector< Control > &controls)
	Appends controls with a rs-turn. More...

void	rs_turn_controls (const HC_CC_Circle &c, const Configuration &q, bool order, vector< Control > &controls)

double	sgn (double x)
	Return sign of a number. More...

bool	state_equal (const State &state1, const State &state2)
	Checks whether two states are equal. More...

void	straight_controls (const Configuration &q1, const Configuration &q2, std::vector< Control > &controls)
	Appends controls with a straight line. More...

void	straight_controls (const Configuration &q1, const Configuration &q2, vector< Control > &controls)

Control	subtract_control (const Control &control1, const Control &control2)
	Subtracts control2 from control1. More...

double	twopify (double alpha)
	Conversion of arbitrary angle given in [rad] to [0, 2*pi[. More...

Variables
const double	epsilon = 1e-4

const int	nb_cc_dubins_paths = 7

const int	nb_hc_cc_rs_paths = 18

Typedef Documentation

◆ Matrix23d

typedef Eigen::Matrix<double, 2, 3> steering::Matrix23d

Definition at line 46 of file ekf.hpp.

◆ Matrix2d

typedef Eigen::Matrix<double, 2, 2> steering::Matrix2d

Definition at line 43 of file ekf.hpp.

◆ Matrix32d

typedef Eigen::Matrix<double, 3, 2> steering::Matrix32d

Definition at line 45 of file ekf.hpp.

◆ Matrix3d

typedef Eigen::Matrix<double, 3, 3> steering::Matrix3d

Definition at line 44 of file ekf.hpp.

Function Documentation

◆ array_index_min()

int steering::array_index_min	(	double	array[],
		int	size
	)

Find index with minimal value in double array.

Definition at line 241 of file utilities.cpp.

◆ cc_default_controls() [1/2]

void steering::cc_default_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		double	delta,
		bool	order,
		std::vector< Control > &	controls
	)

Appends controls with a default cc-turn consisting of two clothoids and a circular arc.

◆ cc_default_controls() [2/2]

void steering::cc_default_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		double	delta,
		bool	order,
		vector< Control > &	controls
	)

Definition at line 392 of file paths.cpp.

◆ cc_elementary_controls() [1/2]

bool steering::cc_elementary_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		double	delta,
		bool	order,
		std::vector< Control > &	controls
	)

Appends controls with an elementary path if one exists.

◆ cc_elementary_controls() [2/2]

bool steering::cc_elementary_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		double	delta,
		bool	order,
		vector< Control > &	controls
	)

Definition at line 368 of file paths.cpp.

◆ cc_turn_controls() [1/2]

void steering::cc_turn_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		bool	order,
		std::vector< Control > &	controls
	)

Appends controls with a cc-turn.

◆ cc_turn_controls() [2/2]

void steering::cc_turn_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		bool	order,
		vector< Control > &	controls
	)

Definition at line 417 of file paths.cpp.

◆ center_distance()

double steering::center_distance	(	const HC_CC_Circle &	c1,
		const HC_CC_Circle &	c2
	)

Cartesian distance between the centers of two circles.

Definition at line 307 of file hc_cc_circle.cpp.

◆ configuration_aligned()

bool steering::configuration_aligned	(	const Configuration &	q1,
		const Configuration &	q2
	)

Are two configurations aligned?

Definition at line 59 of file configuration.cpp.

◆ configuration_distance()

double steering::configuration_distance	(	const Configuration &	q1,
		const Configuration &	q2
	)

Cartesian distance between two configurations.

Definition at line 54 of file configuration.cpp.

◆ configuration_equal()

bool steering::configuration_equal	(	const Configuration &	q1,
		const Configuration &	q2
	)

Are two configurations equal?

Definition at line 69 of file configuration.cpp.

◆ configuration_on_hc_cc_circle()

bool steering::configuration_on_hc_cc_circle	(	const HC_CC_Circle &	c,
		const Configuration &	q
	)

Configuration on the circle?

Definition at line 312 of file hc_cc_circle.cpp.

◆ direction()

int steering::direction	(	bool	forward,
		bool	order
	)

Definition at line 312 of file paths.cpp.

◆ double_array_init()

void steering::double_array_init	(	double	array[],
		int	size,
		double	value
	)

Initialize an array with a given value.

Definition at line 256 of file utilities.cpp.

◆ empty_controls() [1/2]

void steering::empty_controls ( std::vector< Control > & controls )

Appends controls with 0 input.

◆ empty_controls() [2/2]

void steering::empty_controls ( vector< Control > & controls )

Definition at line 291 of file paths.cpp.

◆ end_of_circular_arc()

void steering::end_of_circular_arc	(	double	x_i,
		double	y_i,
		double	theta_i,
		double	kappa,
		double	direction,
		double	length,
		double *	x_f,
		double *	y_f,
		double *	theta_f
	)

Computation of the end point on a circular arc x_i, y_i, theta_i: initial configuration kappa: curvature of circular arc direction: driving direction {-1.0, 1.0} length: arc length (positive) x_f, y_f, theta_f: final configuration on circular arc.

Definition at line 208 of file utilities.cpp.

◆ end_of_clothoid()

void steering::end_of_clothoid	(	double	x_i,
		double	y_i,
		double	theta_i,
		double	kappa_i,
		double	sigma,
		double	direction,
		double	length,
		double *	x_f,
		double *	y_f,
		double *	theta_f,
		double *	kappa_f
	)

Computation of the end point on a clothoid x_i, y_i, theta_i, kappa_i: initial configuration sigma: sharpness of clothoid direction: driving direction {-1.0, 1.0} length: length of clothoid (positive) x_f, y_f, theta_f, kappa_f: final configuration on clothoid.

Definition at line 176 of file utilities.cpp.

◆ end_of_straight_line()

void steering::end_of_straight_line	(	double	x_i,
		double	y_i,
		double	theta,
		double	direction,
		double	length,
		double *	x_f,
		double *	y_f
	)

Computation of the end point on a straight line x_i, y_i: initial configuration theta: angle of straight line direction: driving direction {-1.0, 1.0} length: line length (positive) x_f, y_f: final configuration on straight line.

Definition at line 216 of file utilities.cpp.

◆ fresnel()

void steering::fresnel	(	double	s,
		double &	S_f,
		double &	C_f
	)

Fresnel integrals: S_f = int_0_s(sin(pi/2 u*u)du), C_f = int_0_s(cos(pi/2 u*u)du) approximated with Chebyshev polynomials.

Definition at line 161 of file utilities.cpp.

◆ fresnel_0_8()

void steering::fresnel_0_8	(	double	x,
		double &	S_f,
		double &	C_f
	)

Definition at line 93 of file utilities.cpp.

◆ fresnel_8_inf()

void steering::fresnel_8_inf	(	double	x,
		double &	S_f,
		double &	C_f
	)

Definition at line 127 of file utilities.cpp.

◆ get_epsilon()

double steering::get_epsilon ( )

Return value of epsilon.

Definition at line 57 of file utilities.cpp.

◆ global_frame_change()

void steering::global_frame_change	(	double	x,
		double	y,
		double	theta,
		double	local_x,
		double	local_y,
		double *	global_x,
		double *	global_y
	)

Transformation of (local_x, local_y) from local coordinate system to global one.

Definition at line 223 of file utilities.cpp.

◆ hc_turn_controls() [1/2]

void steering::hc_turn_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		bool	order,
		std::vector< Control > &	controls
	)

Appends controls with a hc-turn.

◆ hc_turn_controls() [2/2]

void steering::hc_turn_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		bool	order,
		vector< Control > &	controls
	)

Definition at line 336 of file paths.cpp.

◆ local_frame_change()

void steering::local_frame_change	(	double	x,
		double	y,
		double	theta,
		double	global_x,
		double	global_y,
		double *	local_x,
		double *	local_y
	)

Transformation of (global_x, global_y) from global coordinate system to local one.

Definition at line 232 of file utilities.cpp.

◆ pify()

double steering::pify ( double alpha )

Conversion of arbitrary angle given in [rad] to [-pi, pi[.

Definition at line 83 of file utilities.cpp.

◆ point_distance()

double steering::point_distance	(	double	x1,
		double	y1,
		double	x2,
		double	y2
	)

Cartesian distance between two points.

Definition at line 67 of file utilities.cpp.

◆ pointer_array_init()

void steering::pointer_array_init	(	void *	array[],
		int	size
	)

Initialize an array with nullptr.

Definition at line 264 of file utilities.cpp.

◆ polar()

void steering::polar	(	double	x,
		double	y,
		double &	r,
		double &	theta
	)

Computation of a point's polar coordinates.

Definition at line 72 of file utilities.cpp.

◆ reverse_control()

void steering::reverse_control ( Control & control )

Reverses a control.

Definition at line 274 of file paths.cpp.

◆ rs_turn_controls() [1/2]

void steering::rs_turn_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		bool	order,
		std::vector< Control > &	controls
	)

Appends controls with a rs-turn.

◆ rs_turn_controls() [2/2]

void steering::rs_turn_controls	(	const HC_CC_Circle &	c,
		const Configuration &	q,
		bool	order,
		vector< Control > &	controls
	)

Definition at line 320 of file paths.cpp.

◆ sgn()

double steering::sgn ( double x )

Return sign of a number.

Definition at line 62 of file utilities.cpp.

◆ state_equal()

bool steering::state_equal	(	const State &	state1,
		const State &	state2
	)

Checks whether two states are equal.

Definition at line 263 of file paths.cpp.

◆ straight_controls() [1/2]

void steering::straight_controls	(	const Configuration &	q1,
		const Configuration &	q2,
		std::vector< Control > &	controls
	)

Appends controls with a straight line.

◆ straight_controls() [2/2]

void steering::straight_controls	(	const Configuration &	q1,
		const Configuration &	q2,
		vector< Control > &	controls
	)

Definition at line 300 of file paths.cpp.

◆ subtract_control()

Control steering::subtract_control	(	const Control &	control1,
		const Control &	control2
	)

Subtracts control2 from control1.

Definition at line 281 of file paths.cpp.

◆ twopify()

double steering::twopify ( double alpha )

Conversion of arbitrary angle given in [rad] to [0, 2*pi[.

Definition at line 78 of file utilities.cpp.

Variable Documentation

◆ epsilon

const double steering::epsilon = 1e-4

Definition at line 41 of file utilities.hpp.

◆ nb_cc_dubins_paths

const int steering::nb_cc_dubins_paths = 7

Definition at line 94 of file paths.hpp.

◆ nb_hc_cc_rs_paths

const int steering::nb_hc_cc_rs_paths = 18

Definition at line 147 of file paths.hpp.

Namespaces

Classes

Typedefs

Functions

Variables