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

#include <dubins_state_space.hpp>

Classes
class	Dubins_Path
	Complete description of a Dubins path. More...

Public Types
enum	Dubins_Path_Segment_Type { DUBINS_LEFT = 0, DUBINS_STRAIGHT = 1, DUBINS_RIGHT = 2 }
	The Dubins path segment type. More...

Public Member Functions
Dubins_Path	dubins (const State &state1, const State &state2) const
	Returns type and length of segments of path from state1 to state2 with curvature = 1.0. More...

	Dubins_State_Space (double kappa, double discretization=0.1, bool forwards=true)
	Constructor of the Dubins_State_Space. More...

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

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 Dubins_Path_Segment_Type	dubins_path_type [6][3]
	Dubins path types. More...

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

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

bool	forwards_
	Driving direction. 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 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.

Definition at line 141 of file dubins_state_space.hpp.

Member Enumeration Documentation

◆ Dubins_Path_Segment_Type

enum steering::Dubins_State_Space::Dubins_Path_Segment_Type

The Dubins path segment type.

Enumerator
DUBINS_LEFT
DUBINS_STRAIGHT
DUBINS_RIGHT

Definition at line 200 of file dubins_state_space.hpp.

Constructor & Destructor Documentation

◆ Dubins_State_Space()

steering::Dubins_State_Space::Dubins_State_Space	(	double	kappa,
		double	discretization = `0.1`,
		bool	forwards = `true`
	)

inline

Constructor of the Dubins_State_Space.

Definition at line 243 of file dubins_state_space.hpp.

Member Function Documentation

◆ dubins()

Dubins_State_Space::Dubins_Path steering::Dubins_State_Space::dubins	(	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 228 of file dubins_state_space.cpp.

◆ get_controls()

vector< Control > steering::Dubins_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 249 of file dubins_state_space.cpp.

◆ get_distance()

double steering::Dubins_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 241 of file dubins_state_space.cpp.

◆ get_path()

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

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

Definition at line 291 of file dubins_state_space.cpp.

◆ get_path_with_covariance()

vector< State_With_Covariance > steering::Dubins_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 297 of file dubins_state_space.cpp.

◆ integrate()

vector< State > steering::Dubins_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 304 of file dubins_state_space.cpp.

◆ integrate_ODE()

State steering::Dubins_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 484 of file dubins_state_space.cpp.

◆ integrate_with_covariance()

vector< State_With_Covariance > steering::Dubins_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 353 of file dubins_state_space.cpp.

◆ interpolate()

State steering::Dubins_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 421 of file dubins_state_space.cpp.

◆ set_filter_parameters()

void steering::Dubins_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 235 of file dubins_state_space.cpp.

Member Data Documentation

◆ discretization_

double steering::Dubins_State_Space::discretization_

private

Discretization of path.

Definition at line 291 of file dubins_state_space.hpp.

◆ dubins_path_type

const Dubins_State_Space::Dubins_Path_Segment_Type steering::Dubins_State_Space::dubins_path_type

static

Initial value:

= {
  { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT },  { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT },
  { DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT }, { DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT },
  { DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT },    { DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT }
}

Dubins path types.

Definition at line 208 of file dubins_state_space.hpp.

◆ ekf_

EKF steering::Dubins_State_Space::ekf_

private

Extended Kalman Filter for uncertainty propagation.

Definition at line 297 of file dubins_state_space.hpp.

◆ forwards_

bool steering::Dubins_State_Space::forwards_

private

Driving direction.

Definition at line 294 of file dubins_state_space.hpp.

◆ kappa_

double steering::Dubins_State_Space::kappa_

private

Curvature.

Definition at line 285 of file dubins_state_space.hpp.

◆ kappa_inv_

double steering::Dubins_State_Space::kappa_inv_

private

Inverse of curvature.

Definition at line 288 of file dubins_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

◆ Dubins_Path_Segment_Type

Constructor & Destructor Documentation

◆ Dubins_State_Space()

Member Function Documentation

◆ dubins()

◆ get_controls()

◆ get_distance()

◆ get_path()

◆ get_path_with_covariance()

◆ integrate()

◆ integrate_ODE()

◆ integrate_with_covariance()

◆ interpolate()

◆ set_filter_parameters()

Member Data Documentation

◆ discretization_

◆ dubins_path_type

◆ ekf_

◆ forwards_

◆ kappa_

◆ kappa_inv_