Finite differences grid with variable resolution for SE2. More...

#include <finite_differences_variable_grid_se2.h>

Inheritance diagram for mpc_local_planner::FiniteDifferencesVariableGridSE2:

[legend]

Public Types
enum	GridAdaptStrategy { GridAdaptStrategy::NoGridAdapt, GridAdaptStrategy::TimeBasedSingleStep, GridAdaptStrategy::TimeBasedAggressiveEstimate, GridAdaptStrategy::SimpleShrinkingHorizon }

using	Ptr = std::shared_ptr< FiniteDifferencesVariableGridSE2 >

using	UPtr = std::unique_ptr< FiniteDifferencesVariableGridSE2 >

Public Types inherited from mpc_local_planner::FiniteDifferencesGridSE2
using	BaseEdge = corbo::BaseEdge

using	BaseMixedEdge = corbo::BaseMixedEdge

using	Ptr = std::shared_ptr< FiniteDifferencesGridSE2 >

Public Types inherited from mpc_local_planner::FullDiscretizationGridBaseSE2
enum	CostIntegrationRule { CostIntegrationRule::LeftSum, CostIntegrationRule::TrapezoidalRule }

using	NlpFunctions = corbo::NlpFunctions

using	OptimizationEdgeSet = corbo::OptimizationEdgeSet

using	PartiallyFixedVectorVertex = corbo::PartiallyFixedVectorVertex

using	Ptr = std::shared_ptr< FullDiscretizationGridBaseSE2 >

using	ReferenceTrajectoryInterface = corbo::ReferenceTrajectoryInterface

using	ScalarVertex = corbo::ScalarVertex

using	SystemDynamicsInterface = corbo::SystemDynamicsInterface

using	TimeSeries = corbo::TimeSeries

using	UPtr = std::unique_ptr< FullDiscretizationGridBaseSE2 >

using	VectorVertex = corbo::VectorVertex

using	VertexInterface = corbo::VertexInterface

Public Types inherited from corbo::DiscretizationGridInterface
typedef std::shared_ptr< DiscretizationGridInterface >	Ptr

typedef std::unique_ptr< DiscretizationGridInterface >	UPtr

Public Types inherited from corbo::VertexSetInterface
typedef std::shared_ptr< VertexSetInterface >	Ptr

Public Member Functions
void	disableGridAdaptation ()
	Disable grid adapation. More...

	FiniteDifferencesVariableGridSE2 ()=default

corbo::DiscretizationGridInterface::Ptr	getInstance () const override
	Return a newly created shared instance of the implemented class. More...

void	setDtBounds (double dt_lb, double dt_ub)
	Set lower and upper bounds for the temporal resolution. More...

void	setGridAdaptSimpleShrinkingHorizon (bool adapt_first_iter=false)
	Set grid adaptation strategy to 'shrinking horizon'. More...

void	setGridAdaptTimeBasedAggressiveEstimate (int n_max, double dt_hyst_ratio=0.1, bool adapt_first_iter=false)
	Set grid adaptation strategy to 'aggressive'. More...

void	setGridAdaptTimeBasedSingleStep (int n_max, double dt_hyst_ratio=0.1, bool adapt_first_iter=false)
	Set grid adaptation strategy to 'single step'. More...

void	setNmin (int n_min)
	Set minium grid size for grid adaptation. More...

virtual	~FiniteDifferencesVariableGridSE2 ()=default

Public Member Functions inherited from mpc_local_planner::FiniteDifferencesGridSE2
	FiniteDifferencesGridSE2 ()=default
	Default constructor. More...

virtual	~FiniteDifferencesGridSE2 ()=default
	Default destructor. More...

Public Member Functions inherited from mpc_local_planner::FullDiscretizationGridBaseSE2
void	clear () override

int	findClosestPose (double x_ref, double y_ref, int start_idx=0, double *distance=nullptr) const
	Find the closest pose (first part of the state vector) on the grid w.r.t. to a provided reference point. More...

	FullDiscretizationGridBaseSE2 ()=default

std::vector< VertexInterface * > &	getActiveVertices () override

double	getDt () const
	get current temporal resolution More...

double	getDtRef () const
	get current reference temporal resolution More...

double	getFinalTime () const override

bool	getFirstControlInput (Eigen::VectorXd &u0) override

double	getFirstDt () const override

double	getInitialDt () const override

int	getInitialN () const override

int	getN () const override
	get current horizon length More...

int	getNRef () const
	get reference horizon length More...

const Eigen::VectorXd &	getState (int k) const
	Return state at time stamp k. More...

void	getStateAndControlTimeSeries (TimeSeries::Ptr x_sequence, TimeSeries::Ptr u_sequence, double t_max=corbo::CORBO_INF_DBL) const override

void	getVertices (std::vector< VertexInterface *> &vertices) override

bool	hasConstantControls () const override

bool	hasSingleDt () const override

bool	isEmpty () const override

bool	isTimeVariableGrid () const override

bool	isUniformGrid () const override

virtual bool	isValid () const

bool	providesStateTrajectory () const override

void	setCostIntegrationRule (CostIntegrationRule integration)
	Set cost integration rule. More...

void	setDtRef (double dt)
	set reference temporal resolution More...

void	setFiniteDifferencesCollocationMethod (corbo::FiniteDifferencesCollocationInterface::Ptr fd_eval)
	Set finite differences collocation method. More...

void	setInitialDt (double dt) override

void	setN (int n, bool try_resample=true) override

void	setNRef (int n)
	set reference horizon length More...

void	setWarmStart (bool active)
	activate or deactive warmstart More...

void	setXfFixed (const Eigen::Matrix< bool, -1, 1 > &xf_fixed)
	Set individual components of the final state to fixed or unfixed. More...

corbo::GridUpdateResult	update (const Eigen::VectorXd &x0, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, NlpFunctions &nlp_fun, OptimizationEdgeSet &edges, SystemDynamicsInterface::Ptr dynamics, bool new_run, const corbo::Time &t, ReferenceTrajectoryInterface sref=nullptr, const Eigen::VectorXd prev_u=nullptr, double prev_u_dt=0, ReferenceTrajectoryInterface xinit=nullptr, ReferenceTrajectoryInterface uinit=nullptr) override

virtual	~FullDiscretizationGridBaseSE2 ()=default

Public Member Functions inherited from corbo::DiscretizationGridInterface
	DiscretizationGridInterface ()=default

	DiscretizationGridInterface (int state_dim, int control_dim)

virtual	~DiscretizationGridInterface ()=default

Public Member Functions inherited from corbo::VertexSetInterface
void	applyIncrementNonFixed (const Eigen::Ref< const Eigen::VectorXd > &increment)

void	applyIncrementNonFixed (int idx, double increment)

void	backupParametersActiveVertices ()

void	clearConnectedEdges ()

void	computeVertexIndices ()

void	discardBackupParametersActiveVertices (bool all=false)

void	getBounds (Eigen::Ref< Eigen::VectorXd > lb, Eigen::Ref< Eigen::VectorXd > ub)

double	getLowerBound (int idx)

int	getParameterDimension ()

double	getParameterValue (int idx)

void	getParameterVector (Eigen::Ref< Eigen::VectorXd > x)

double	getUpperBound (int idx)

bool	isModified () const

void	restoreBackupParametersActiveVertices (bool keep_backup)

void	setBounds (const Eigen::Ref< const Eigen::VectorXd > &lb, const Eigen::Ref< const Eigen::VectorXd > &ub)

void	setLowerBound (int idx, double lb)

void	setModified (bool modified)

void	setParameterValue (int idx, double x)

void	setParameterVector (const Eigen::Ref< const Eigen::VectorXd > &x)

void	setUpperBound (int idx, double ub)

	VertexSetInterface ()

virtual	~VertexSetInterface ()

Static Public Member Functions
static corbo::Factory< corbo::DiscretizationGridInterface > &	getFactory ()
	Get access to the associated factory. More...

Static Public Member Functions inherited from mpc_local_planner::FiniteDifferencesGridSE2
static corbo::Factory< corbo::DiscretizationGridInterface > &	getFactory ()
	Get access to the associated factory. More...

Static Public Member Functions inherited from corbo::DiscretizationGridInterface
static Factory< DiscretizationGridInterface > &	getFactory ()

Protected Member Functions
bool	adaptGrid (bool new_run, NlpFunctions &nlp_fun) override

bool	adaptGridSimpleShrinkingHorizon (NlpFunctions &nlp_fun)

bool	adaptGridTimeBasedAggressiveEstimate (NlpFunctions &nlp_fun)

bool	adaptGridTimeBasedSingleStep (NlpFunctions &nlp_fun)

bool	isDtFixedIntended () const override

bool	isGridAdaptActive () const override

bool	isMovingHorizonWarmStartActive () const override

Protected Member Functions inherited from mpc_local_planner::FiniteDifferencesGridSE2
void	createEdges (NlpFunctions &nlp_fun, OptimizationEdgeSet &edges, SystemDynamicsInterface::Ptr dynamics) override

Protected Member Functions inherited from mpc_local_planner::FullDiscretizationGridBaseSE2
bool	checkAndInitializeXfFixedFlags (int dim_x)

void	computeActiveVertices () override

int	findNearestState (const Eigen::VectorXd &x0)

virtual void	initializeSequences (const Eigen::VectorXd &x0, const Eigen::VectorXd &xf, ReferenceTrajectoryInterface &uref, NlpFunctions &nlp_fun)

virtual void	initializeSequences (const Eigen::VectorXd &x0, const Eigen::VectorXd &xf, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, NlpFunctions &nlp_fun)

virtual void	resampleTrajectory (int n_new)

void	updateBounds (const NlpFunctions &nlp_fun)

virtual void	warmStartShifting (const Eigen::VectorXd &x0)

Protected Member Functions inherited from corbo::DiscretizationGridInterface
void	setLastControlRef (const Eigen::VectorXd &last_u_ref)

void	setPreviousControl (const Eigen::VectorXd &prev_u, double prev_u_dt)

Protected Member Functions inherited from corbo::VertexSetInterface
void	setVertexIdx (VertexInterface &vertex, int idx)

Protected Attributes
bool	_adapt_first_iter = false

double	_dt_hyst_ratio = 0.1

GridAdaptStrategy	_grid_adapt = GridAdaptStrategy::NoGridAdapt

int	_n_max = 1000

int	_n_min = 2

Protected Attributes inherited from mpc_local_planner::FullDiscretizationGridBaseSE2
std::vector< VertexInterface * >	_active_vertices

CostIntegrationRule	_cost_integration

ScalarVertex	_dt

double	_dt_lb = 0

double	_dt_ref = 0.1

double	_dt_ub = corbo::CORBO_INF_DBL

corbo::FiniteDifferencesCollocationInterface::Ptr	_fd_eval = std::make_shared<corbo::CrankNicolsonDiffCollocation>()

bool	_first_run = true

int	_n_adapt = 0

int	_n_ref = 11

const NlpFunctions *	_nlp_fun = nullptr

std::vector< VectorVertex >	_u_seq

bool	_warm_start = false

std::vector< VectorVertexSE2 >	_x_seq

PartiallyFixedVectorVertexSE2	_xf

Eigen::Matrix< bool, -1, 1 >	_xf_fixed

Protected Attributes inherited from corbo::DiscretizationGridInterface
VectorVertex	_u_prev

ScalarVertex	_u_prev_dt

VectorVertex	_u_ref

Protected Attributes inherited from corbo::VertexSetInterface
bool	_modified

Detailed Description

Finite differences grid with variable resolution for SE2.

This class implements a full discretization grid with finite difference collocation. The temporal resolution is free. This grid corresponds to the global uniform grid in time-optimal MPC as defined in [1,2].

[1] C. Rösmann, F. Hoffmann und T. Bertram: Timed-Elastic-Bands for Time-Optimal Point-to-Point Nonlinear Model Predictive Control, ECC 2015. [2] C. Rösmann: Time-optimal nonlinear model predictive control - Direct transcription methods with variable discretization and structural sparsity exploitation. Dissertation, TU Dortmund University, Oct. 2019.

See also: FiniteDifferencesGridSE2

Author: Christoph Rösmann (chris.nosp@m.toph.nosp@m..roes.nosp@m.mann.nosp@m.@tu-d.nosp@m.ortm.nosp@m.und.d.nosp@m.e)

Definition at line 47 of file finite_differences_variable_grid_se2.h.

Member Typedef Documentation

◆ Ptr

using mpc_local_planner::FiniteDifferencesVariableGridSE2::Ptr = std::shared_ptr<FiniteDifferencesVariableGridSE2>

Definition at line 50 of file finite_differences_variable_grid_se2.h.

◆ UPtr

using mpc_local_planner::FiniteDifferencesVariableGridSE2::UPtr = std::unique_ptr<FiniteDifferencesVariableGridSE2>

Definition at line 51 of file finite_differences_variable_grid_se2.h.

Member Enumeration Documentation

◆ GridAdaptStrategy

enum mpc_local_planner::FiniteDifferencesVariableGridSE2::GridAdaptStrategy

strong

Enumerator
NoGridAdapt
TimeBasedSingleStep
TimeBasedAggressiveEstimate
SimpleShrinkingHorizon

Definition at line 53 of file finite_differences_variable_grid_se2.h.

Constructor & Destructor Documentation

◆ FiniteDifferencesVariableGridSE2()

mpc_local_planner::FiniteDifferencesVariableGridSE2::FiniteDifferencesVariableGridSE2 ( )

default

◆ ~FiniteDifferencesVariableGridSE2()

virtual mpc_local_planner::FiniteDifferencesVariableGridSE2::~FiniteDifferencesVariableGridSE2 ( )

virtualdefault

Member Function Documentation

◆ adaptGrid()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::adaptGrid	(	bool	new_run,
		NlpFunctions &	nlp_fun
	)

overrideprotectedvirtual

Reimplemented from mpc_local_planner::FullDiscretizationGridBaseSE2.

Definition at line 64 of file finite_differences_variable_grid_se2.cpp.

◆ adaptGridSimpleShrinkingHorizon()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::adaptGridSimpleShrinkingHorizon ( NlpFunctions & nlp_fun )

protected

Definition at line 152 of file finite_differences_variable_grid_se2.cpp.

◆ adaptGridTimeBasedAggressiveEstimate()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::adaptGridTimeBasedAggressiveEstimate ( NlpFunctions & nlp_fun )

protected

Definition at line 123 of file finite_differences_variable_grid_se2.cpp.

◆ adaptGridTimeBasedSingleStep()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::adaptGridTimeBasedSingleStep ( NlpFunctions & nlp_fun )

protected

Definition at line 99 of file finite_differences_variable_grid_se2.cpp.

◆ disableGridAdaptation()

void mpc_local_planner::FiniteDifferencesVariableGridSE2::disableGridAdaptation ( )

inline

Disable grid adapation.

Definition at line 69 of file finite_differences_variable_grid_se2.h.

◆ getFactory()

static corbo::Factory<corbo::DiscretizationGridInterface>& mpc_local_planner::FiniteDifferencesVariableGridSE2::getFactory ( )

inlinestatic

Get access to the associated factory.

Definition at line 62 of file finite_differences_variable_grid_se2.h.

◆ getInstance()

corbo::DiscretizationGridInterface::Ptr mpc_local_planner::FiniteDifferencesVariableGridSE2::getInstance ( ) const

inlineoverridevirtual

Return a newly created shared instance of the implemented class.

Reimplemented from mpc_local_planner::FiniteDifferencesGridSE2.

Definition at line 59 of file finite_differences_variable_grid_se2.h.

◆ isDtFixedIntended()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::isDtFixedIntended ( ) const

inlineoverrideprotectedvirtual

Reimplemented from mpc_local_planner::FiniteDifferencesGridSE2.

Definition at line 78 of file finite_differences_variable_grid_se2.h.

◆ isGridAdaptActive()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::isGridAdaptActive ( ) const

inlineoverrideprotectedvirtual

Reimplemented from mpc_local_planner::FullDiscretizationGridBaseSE2.

Definition at line 86 of file finite_differences_variable_grid_se2.h.

◆ isMovingHorizonWarmStartActive()

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::isMovingHorizonWarmStartActive ( ) const

inlineoverrideprotectedvirtual

Reimplemented from mpc_local_planner::FullDiscretizationGridBaseSE2.

Definition at line 85 of file finite_differences_variable_grid_se2.h.

◆ setDtBounds()

void mpc_local_planner::FiniteDifferencesVariableGridSE2::setDtBounds	(	double	dt_lb,
		double	dt_ub
	)

Set lower and upper bounds for the temporal resolution.

Definition at line 36 of file finite_differences_variable_grid_se2.cpp.

◆ setGridAdaptSimpleShrinkingHorizon()

void mpc_local_planner::FiniteDifferencesVariableGridSE2::setGridAdaptSimpleShrinkingHorizon ( bool adapt_first_iter = false )

Set grid adaptation strategy to 'shrinking horizon'.

Definition at line 58 of file finite_differences_variable_grid_se2.cpp.

◆ setGridAdaptTimeBasedAggressiveEstimate()

void mpc_local_planner::FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedAggressiveEstimate	(	int	n_max,
		double	dt_hyst_ratio = `0.1`,
		bool	adapt_first_iter = `false`
	)

Set grid adaptation strategy to 'aggressive'.

Definition at line 50 of file finite_differences_variable_grid_se2.cpp.

◆ setGridAdaptTimeBasedSingleStep()

void mpc_local_planner::FiniteDifferencesVariableGridSE2::setGridAdaptTimeBasedSingleStep	(	int	n_max,
		double	dt_hyst_ratio = `0.1`,
		bool	adapt_first_iter = `false`
	)

Set grid adaptation strategy to 'single step'.

Definition at line 42 of file finite_differences_variable_grid_se2.cpp.

◆ setNmin()

void mpc_local_planner::FiniteDifferencesVariableGridSE2::setNmin ( int n_min )

inline

Set minium grid size for grid adaptation.

Definition at line 67 of file finite_differences_variable_grid_se2.h.

Member Data Documentation

◆ _adapt_first_iter

bool mpc_local_planner::FiniteDifferencesVariableGridSE2::_adapt_first_iter = false

protected

Definition at line 93 of file finite_differences_variable_grid_se2.h.

◆ _dt_hyst_ratio

double mpc_local_planner::FiniteDifferencesVariableGridSE2::_dt_hyst_ratio = 0.1

protected

Definition at line 91 of file finite_differences_variable_grid_se2.h.

◆ _grid_adapt

GridAdaptStrategy mpc_local_planner::FiniteDifferencesVariableGridSE2::_grid_adapt = GridAdaptStrategy::NoGridAdapt

protected

Definition at line 89 of file finite_differences_variable_grid_se2.h.

◆ _n_max

int mpc_local_planner::FiniteDifferencesVariableGridSE2::_n_max = 1000

protected

Definition at line 90 of file finite_differences_variable_grid_se2.h.

◆ _n_min

int mpc_local_planner::FiniteDifferencesVariableGridSE2::_n_min = 2

protected

Definition at line 92 of file finite_differences_variable_grid_se2.h.

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

Public Types

Public Member Functions

Static Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ Ptr

◆ UPtr

Member Enumeration Documentation

◆ GridAdaptStrategy

Constructor & Destructor Documentation

◆ FiniteDifferencesVariableGridSE2()

◆ ~FiniteDifferencesVariableGridSE2()

Member Function Documentation

◆ adaptGrid()

◆ adaptGridSimpleShrinkingHorizon()

◆ adaptGridTimeBasedAggressiveEstimate()

◆ adaptGridTimeBasedSingleStep()

◆ disableGridAdaptation()

◆ getFactory()

◆ getInstance()

◆ isDtFixedIntended()

◆ isGridAdaptActive()

◆ isMovingHorizonWarmStartActive()

◆ setDtBounds()

◆ setGridAdaptSimpleShrinkingHorizon()

◆ setGridAdaptTimeBasedAggressiveEstimate()

◆ setGridAdaptTimeBasedSingleStep()

◆ setNmin()

Member Data Documentation

◆ _adapt_first_iter

◆ _dt_hyst_ratio

◆ _grid_adapt

◆ _n_max

◆ _n_min