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...

#include <reeds_shepp_state_space.hpp>

Classes
class	Reeds_Shepp_Path
	Complete description of a ReedsShepp path. More...

Public Types
enum	Reeds_Shepp_Path_Segment_Type { RS_NOP = 0, RS_LEFT = 1, RS_STRAIGHT = 2, RS_RIGHT = 3 }
	The Reeds-Shepp path segment types. More...

Public Member Functions
std::vector< Control >	get_controls (const State &state1, const State &state2) const
	Returns controls of the shortest path from state1 to state2 with curvature = kappa_. More...

double	get_distance (const State &state1, const State &state2) const
	Returns shortest path length from state1 to state2 with curvature = kappa_. More...

std::vector< State >	get_path (const State &state1, const State &state2) const
	Returns shortest path from state1 to state2 with curvature = kappa_. More...

std::vector< State_With_Covariance >	get_path_with_covariance (const State_With_Covariance &state1, const State &state2) const
	Returns shortest path including covariances from state1 to state2 with curvature = kappa_. More...

std::vector< State >	integrate (const State &state, const std::vector< Control > &controls) const
	Returns integrated states given a start state and controls with curvature = kappa_. More...

State	integrate_ODE (const State &state, const Control &control, double integration_step) const
	Returns integrated state given a start state, a control, and an integration step. More...

std::vector< State_With_Covariance >	integrate_with_covariance (const State_With_Covariance &state, const std::vector< Control > &controls) const
	Returns integrated states including covariance given a start state and controls with curvature = kappa_. More...

State	interpolate (const State &state, const std::vector< Control > &controls, double t) const
	Returns interpolated state at distance t in [0,1] (percent of total path length with curvature = kappa_) More...

Reeds_Shepp_Path	reeds_shepp (const State &state1, const State &state2) const
	Returns type and length of segments of path from state1 to state2 with curvature = 1.0. More...

	Reeds_Shepp_State_Space (double kappa, double discretization=0.1)
	Constructor of the Reeds_Shepp_State_Space. More...

void	set_filter_parameters (const Motion_Noise &motion_noise, const Measurement_Noise &measurement_noise, const Controller &controller)
	Sets the parameters required by the filter. More...

Static Public Attributes
static const Reeds_Shepp_Path_Segment_Type	reeds_shepp_path_type [18][5]
	Reeds-Shepp path types. More...

Private Attributes
double	discretization_
	Discretization of path. More...

EKF	ekf_
	Extended Kalman Filter for uncertainty propagation. More...

double	kappa_
	Curvature. More...

double	kappa_inv_
	Inverse of curvature. More...

Detailed Description

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.

Definition at line 141 of file reeds_shepp_state_space.hpp.

Member Enumeration Documentation

◆ Reeds_Shepp_Path_Segment_Type

enum steering::Reeds_Shepp_State_Space::Reeds_Shepp_Path_Segment_Type

The Reeds-Shepp path segment types.

Enumerator
RS_NOP
RS_LEFT
RS_STRAIGHT
RS_RIGHT

Definition at line 200 of file reeds_shepp_state_space.hpp.

Constructor & Destructor Documentation

◆ Reeds_Shepp_State_Space()

steering::Reeds_Shepp_State_Space::Reeds_Shepp_State_Space	(	double	kappa,
		double	discretization = `0.1`
	)

inline

Constructor of the Reeds_Shepp_State_Space.

Definition at line 239 of file reeds_shepp_state_space.hpp.

Member Function Documentation

◆ get_controls()

vector< Control > steering::Reeds_Shepp_State_Space::get_controls	(	const State &	state1,
		const State &	state2
	)		const

Returns controls of the shortest path from state1 to state2 with curvature = kappa_.

Definition at line 547 of file reeds_shepp_state_space.cpp.

◆ get_distance()

double steering::Reeds_Shepp_State_Space::get_distance	(	const State &	state1,
		const State &	state2
	)		const

Returns shortest path length from state1 to state2 with curvature = kappa_.

Definition at line 542 of file reeds_shepp_state_space.cpp.

◆ get_path()

vector< State > steering::Reeds_Shepp_State_Space::get_path	(	const State &	state1,
		const State &	state2
	)		const

Returns shortest path from state1 to state2 with curvature = kappa_.

Definition at line 580 of file reeds_shepp_state_space.cpp.

◆ get_path_with_covariance()

vector< State_With_Covariance > steering::Reeds_Shepp_State_Space::get_path_with_covariance	(	const State_With_Covariance &	state1,
		const State &	state2
	)		const

Returns shortest path including covariances from state1 to state2 with curvature = kappa_.

Definition at line 586 of file reeds_shepp_state_space.cpp.

◆ integrate()

vector< State > steering::Reeds_Shepp_State_Space::integrate	(	const State &	state,
		const std::vector< Control > &	controls
	)		const

Returns integrated states given a start state and controls with curvature = kappa_.

Definition at line 593 of file reeds_shepp_state_space.cpp.

◆ integrate_ODE()

State steering::Reeds_Shepp_State_Space::integrate_ODE	(	const State &	state,
		const Control &	control,
		double	integration_step
	)		const

inline

Returns integrated state given a start state, a control, and an integration step.

Definition at line 773 of file reeds_shepp_state_space.cpp.

◆ integrate_with_covariance()

vector< State_With_Covariance > steering::Reeds_Shepp_State_Space::integrate_with_covariance	(	const State_With_Covariance &	state,
		const std::vector< Control > &	controls
	)		const

Returns integrated states including covariance given a start state and controls with curvature = kappa_.

Definition at line 642 of file reeds_shepp_state_space.cpp.

◆ interpolate()

State steering::Reeds_Shepp_State_Space::interpolate	(	const State &	state,
		const std::vector< Control > &	controls,
		double	t
	)		const

Returns interpolated state at distance t in [0,1] (percent of total path length with curvature = kappa_)

Definition at line 710 of file reeds_shepp_state_space.cpp.

◆ reeds_shepp()

Reeds_Shepp_State_Space::Reeds_Shepp_Path steering::Reeds_Shepp_State_Space::reeds_shepp	(	const State &	state1,
		const State &	state2
	)		const

Returns type and length of segments of path from state1 to state2 with curvature = 1.0.

Definition at line 526 of file reeds_shepp_state_space.cpp.

◆ set_filter_parameters()

void steering::Reeds_Shepp_State_Space::set_filter_parameters	(	const Motion_Noise &	motion_noise,
		const Measurement_Noise &	measurement_noise,
		const Controller &	controller
	)

Sets the parameters required by the filter.

Definition at line 535 of file reeds_shepp_state_space.cpp.

Member Data Documentation

◆ discretization_

double steering::Reeds_Shepp_State_Space::discretization_

private

Discretization of path.

Definition at line 286 of file reeds_shepp_state_space.hpp.

◆ ekf_

EKF steering::Reeds_Shepp_State_Space::ekf_

private

Extended Kalman Filter for uncertainty propagation.

Definition at line 289 of file reeds_shepp_state_space.hpp.

◆ kappa_

double steering::Reeds_Shepp_State_Space::kappa_

private

Curvature.

Definition at line 280 of file reeds_shepp_state_space.hpp.

◆ kappa_inv_

double steering::Reeds_Shepp_State_Space::kappa_inv_

private

Inverse of curvature.

Definition at line 283 of file reeds_shepp_state_space.hpp.

◆ reeds_shepp_path_type

const Reeds_Shepp_State_Space::Reeds_Shepp_Path_Segment_Type steering::Reeds_Shepp_State_Space::reeds_shepp_path_type

static

Initial value:

= {
  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP, RS_NOP },         
  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP, RS_NOP },        
  { RS_LEFT, RS_RIGHT, RS_LEFT, RS_RIGHT, RS_NOP },       
  { RS_RIGHT, RS_LEFT, RS_RIGHT, RS_LEFT, RS_NOP },       
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP },    
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP },   
  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },    
  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },   
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP },   
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP },    
  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_LEFT, RS_NOP },   
  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_RIGHT, RS_NOP },    
  { RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },     
  { RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },     
  { RS_LEFT, RS_STRAIGHT, RS_LEFT, RS_NOP, RS_NOP },      
  { RS_RIGHT, RS_STRAIGHT, RS_RIGHT, RS_NOP, RS_NOP },    
  { RS_LEFT, RS_RIGHT, RS_STRAIGHT, RS_LEFT, RS_RIGHT },  
  { RS_RIGHT, RS_LEFT, RS_STRAIGHT, RS_RIGHT, RS_LEFT }   
}

Reeds-Shepp path types.

Definition at line 209 of file reeds_shepp_state_space.hpp.

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

Classes

Public Types

Public Member Functions

Static Public Attributes

Private Attributes

Detailed Description

Member Enumeration Documentation

◆ Reeds_Shepp_Path_Segment_Type

Constructor & Destructor Documentation

◆ Reeds_Shepp_State_Space()

Member Function Documentation

◆ get_controls()

◆ get_distance()

◆ get_path()

◆ get_path_with_covariance()

◆ integrate()

◆ integrate_ODE()

◆ integrate_with_covariance()

◆ interpolate()

◆ reeds_shepp()

◆ set_filter_parameters()

Member Data Documentation

◆ discretization_

◆ ekf_

◆ kappa_

◆ kappa_inv_

◆ reeds_shepp_path_type