Full discretization grid specialization for SE2. More...

#include <full_discretization_grid_base_se2.h>

Inheritance diagram for mpc_local_planner::FullDiscretizationGridBaseSE2:

[legend]

Public Types
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	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

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

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 ()

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

bool	checkAndInitializeXfFixedFlags (int dim_x)

void	computeActiveVertices () override

virtual void	createEdges (NlpFunctions &nlp_fun, OptimizationEdgeSet &edges, SystemDynamicsInterface::Ptr dynamics)=0

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 bool	isDtFixedIntended () const

virtual bool	isGridAdaptActive () const

virtual bool	isMovingHorizonWarmStartActive () const

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

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

Detailed Description

Full discretization grid specialization for SE2.

This class defines the basis for full discretization grids similar to corbo::FullDiscretizationGridBase. However, the main difference is, that this grid requires the first three state components to be embedded in SE2. Whereas the first two states are ordinary real numbers on (-inf, inf), the third component must remain in [-pi, pi).

Therefore, the minimum dimension state dimension is 3. However, larger dimensions are allowed, but the related state components are considered as standard real numbers on (-inf, inf).

Note the full discretization grid currently supports only finite difference collocation to discretize the continuous time dynamics. Full discretization is also possible with multiple shooting and (expicit) numerical integration, but for this purpose we need to specialize corbo::ShootingGridBase for SE2.

See also: corbo::FullDiscretizationGridBase corbo::DiscretizationGridInterface 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 84 of file full_discretization_grid_base_se2.h.

Member Typedef Documentation

◆ NlpFunctions

using mpc_local_planner::FullDiscretizationGridBaseSE2::NlpFunctions = corbo::NlpFunctions

Definition at line 111 of file full_discretization_grid_base_se2.h.

◆ OptimizationEdgeSet

using mpc_local_planner::FullDiscretizationGridBaseSE2::OptimizationEdgeSet = corbo::OptimizationEdgeSet

Definition at line 112 of file full_discretization_grid_base_se2.h.

◆ PartiallyFixedVectorVertex

using mpc_local_planner::FullDiscretizationGridBaseSE2::PartiallyFixedVectorVertex = corbo::PartiallyFixedVectorVertex

Definition at line 116 of file full_discretization_grid_base_se2.h.

◆ Ptr

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

Definition at line 107 of file full_discretization_grid_base_se2.h.

◆ ReferenceTrajectoryInterface

using mpc_local_planner::FullDiscretizationGridBaseSE2::ReferenceTrajectoryInterface = corbo::ReferenceTrajectoryInterface

Definition at line 109 of file full_discretization_grid_base_se2.h.

◆ ScalarVertex

using mpc_local_planner::FullDiscretizationGridBaseSE2::ScalarVertex = corbo::ScalarVertex

Definition at line 114 of file full_discretization_grid_base_se2.h.

◆ SystemDynamicsInterface

using mpc_local_planner::FullDiscretizationGridBaseSE2::SystemDynamicsInterface = corbo::SystemDynamicsInterface

Definition at line 110 of file full_discretization_grid_base_se2.h.

◆ TimeSeries

using mpc_local_planner::FullDiscretizationGridBaseSE2::TimeSeries = corbo::TimeSeries

Definition at line 117 of file full_discretization_grid_base_se2.h.

◆ UPtr

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

Definition at line 108 of file full_discretization_grid_base_se2.h.

◆ VectorVertex

using mpc_local_planner::FullDiscretizationGridBaseSE2::VectorVertex = corbo::VectorVertex

Definition at line 115 of file full_discretization_grid_base_se2.h.

◆ VertexInterface

using mpc_local_planner::FullDiscretizationGridBaseSE2::VertexInterface = corbo::VertexInterface

Definition at line 113 of file full_discretization_grid_base_se2.h.

Member Enumeration Documentation

◆ CostIntegrationRule

enum mpc_local_planner::FullDiscretizationGridBaseSE2::CostIntegrationRule

strong

Enumerator
LeftSum
TrapezoidalRule

Definition at line 119 of file full_discretization_grid_base_se2.h.

Constructor & Destructor Documentation

◆ FullDiscretizationGridBaseSE2()

mpc_local_planner::FullDiscretizationGridBaseSE2::FullDiscretizationGridBaseSE2 ( )

default

◆ ~FullDiscretizationGridBaseSE2()

virtual mpc_local_planner::FullDiscretizationGridBaseSE2::~FullDiscretizationGridBaseSE2 ( )

virtualdefault

Member Function Documentation

◆ adaptGrid()

virtual bool mpc_local_planner::FullDiscretizationGridBaseSE2::adaptGrid	(	bool	new_run,
		NlpFunctions &	nlp_fun
	)

inlineprotectedvirtual

Reimplemented in mpc_local_planner::FiniteDifferencesVariableGridSE2.

Definition at line 228 of file full_discretization_grid_base_se2.h.

◆ checkAndInitializeXfFixedFlags()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::checkAndInitializeXfFixedFlags ( int dim_x )

protected

Definition at line 410 of file full_discretization_grid_base_se2.cpp.

◆ clear()

void mpc_local_planner::FullDiscretizationGridBaseSE2::clear ( )

overridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 546 of file full_discretization_grid_base_se2.cpp.

◆ computeActiveVertices()

void mpc_local_planner::FullDiscretizationGridBaseSE2::computeActiveVertices ( )

overrideprotectedvirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 584 of file full_discretization_grid_base_se2.cpp.

◆ createEdges()

virtual void mpc_local_planner::FullDiscretizationGridBaseSE2::createEdges	(	NlpFunctions &	nlp_fun,
		OptimizationEdgeSet &	edges,
		SystemDynamicsInterface::Ptr	dynamics
	)

protectedpure virtual

Implemented in mpc_local_planner::FiniteDifferencesGridSE2.

◆ findClosestPose()

int mpc_local_planner::FullDiscretizationGridBaseSE2::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.

Parameters

[in]	x_ref	X-position of the reference point
[in]	y_ref	Y-position of the reference point
[in]	begin_idx	Start search at this time stamp
[out]	distance	[optional] the resulting minimum distance

Returns: Index to the closest state/pose on the grid

Definition at line 384 of file full_discretization_grid_base_se2.cpp.

◆ findNearestState()

int mpc_local_planner::FullDiscretizationGridBaseSE2::findNearestState ( const Eigen::VectorXd & x0 )

protected

Definition at line 324 of file full_discretization_grid_base_se2.cpp.

◆ getActiveVertices()

std::vector<VertexInterface*>& mpc_local_planner::FullDiscretizationGridBaseSE2::getActiveVertices ( )

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 218 of file full_discretization_grid_base_se2.h.

◆ getDt()

double mpc_local_planner::FullDiscretizationGridBaseSE2::getDt ( ) const

inline

get current temporal resolution

Definition at line 191 of file full_discretization_grid_base_se2.h.

◆ getDtRef()

double mpc_local_planner::FullDiscretizationGridBaseSE2::getDtRef ( ) const

inline

get current reference temporal resolution

Definition at line 189 of file full_discretization_grid_base_se2.h.

◆ getFinalTime()

double mpc_local_planner::FullDiscretizationGridBaseSE2::getFinalTime ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 137 of file full_discretization_grid_base_se2.h.

◆ getFirstControlInput()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::getFirstControlInput ( Eigen::VectorXd & u0 )

overridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 361 of file full_discretization_grid_base_se2.cpp.

◆ getFirstDt()

double mpc_local_planner::FullDiscretizationGridBaseSE2::getFirstDt ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 135 of file full_discretization_grid_base_se2.h.

◆ getInitialDt()

double mpc_local_planner::FullDiscretizationGridBaseSE2::getInitialDt ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 176 of file full_discretization_grid_base_se2.h.

◆ getInitialN()

int mpc_local_planner::FullDiscretizationGridBaseSE2::getInitialN ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 178 of file full_discretization_grid_base_se2.h.

◆ getInstance()

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

overridepure virtual

Return a newly created shared instance of the implemented class.

Implements corbo::DiscretizationGridInterface.

Implemented in mpc_local_planner::FiniteDifferencesVariableGridSE2, and mpc_local_planner::FiniteDifferencesGridSE2.

◆ getN()

int mpc_local_planner::FullDiscretizationGridBaseSE2::getN ( ) const

inlineoverridevirtual

get current horizon length

Implements corbo::DiscretizationGridInterface.

Definition at line 183 of file full_discretization_grid_base_se2.h.

◆ getNRef()

int mpc_local_planner::FullDiscretizationGridBaseSE2::getNRef ( ) const

inline

get reference horizon length

Definition at line 181 of file full_discretization_grid_base_se2.h.

◆ getState()

const Eigen::VectorXd& mpc_local_planner::FullDiscretizationGridBaseSE2::getState ( int k ) const

inline

Return state at time stamp k.

Definition at line 153 of file full_discretization_grid_base_se2.h.

◆ getStateAndControlTimeSeries()

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

overridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 599 of file full_discretization_grid_base_se2.cpp.

◆ getVertices()

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

overridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 571 of file full_discretization_grid_base_se2.cpp.

◆ hasConstantControls()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::hasConstantControls ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 140 of file full_discretization_grid_base_se2.h.

◆ hasSingleDt()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::hasSingleDt ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 142 of file full_discretization_grid_base_se2.h.

◆ initializeSequences() [1/2]

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

protectedvirtual

Definition at line 156 of file full_discretization_grid_base_se2.cpp.

◆ initializeSequences() [2/2]

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

protectedvirtual

Definition at line 212 of file full_discretization_grid_base_se2.cpp.

◆ isDtFixedIntended()

virtual bool mpc_local_planner::FullDiscretizationGridBaseSE2::isDtFixedIntended ( ) const

inlineprotectedvirtual

Reimplemented in mpc_local_planner::FiniteDifferencesVariableGridSE2, and mpc_local_planner::FiniteDifferencesGridSE2.

Definition at line 240 of file full_discretization_grid_base_se2.h.

◆ isEmpty()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::isEmpty ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 167 of file full_discretization_grid_base_se2.h.

◆ isGridAdaptActive()

virtual bool mpc_local_planner::FullDiscretizationGridBaseSE2::isGridAdaptActive ( ) const

inlineprotectedvirtual

Reimplemented in mpc_local_planner::FiniteDifferencesVariableGridSE2.

Definition at line 242 of file full_discretization_grid_base_se2.h.

◆ isMovingHorizonWarmStartActive()

virtual bool mpc_local_planner::FullDiscretizationGridBaseSE2::isMovingHorizonWarmStartActive ( ) const

inlineprotectedvirtual

Reimplemented in mpc_local_planner::FiniteDifferencesVariableGridSE2.

Definition at line 241 of file full_discretization_grid_base_se2.h.

◆ isTimeVariableGrid()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::isTimeVariableGrid ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 144 of file full_discretization_grid_base_se2.h.

◆ isUniformGrid()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::isUniformGrid ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 146 of file full_discretization_grid_base_se2.h.

◆ isValid()

virtual bool mpc_local_planner::FullDiscretizationGridBaseSE2::isValid ( ) const

inlinevirtual

Definition at line 169 of file full_discretization_grid_base_se2.h.

◆ providesStateTrajectory()

bool mpc_local_planner::FullDiscretizationGridBaseSE2::providesStateTrajectory ( ) const

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 148 of file full_discretization_grid_base_se2.h.

◆ resampleTrajectory()

void mpc_local_planner::FullDiscretizationGridBaseSE2::resampleTrajectory ( int n_new )

protectedvirtual

Todo:: (roesmann) More efficient strategy without copying containers at all?

Definition at line 460 of file full_discretization_grid_base_se2.cpp.

◆ setCostIntegrationRule()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setCostIntegrationRule ( CostIntegrationRule integration )

inline

Set cost integration rule.

Definition at line 204 of file full_discretization_grid_base_se2.h.

◆ setDtRef()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setDtRef ( double dt )

inline

set reference temporal resolution

Definition at line 187 of file full_discretization_grid_base_se2.h.

◆ setFiniteDifferencesCollocationMethod()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setFiniteDifferencesCollocationMethod ( corbo::FiniteDifferencesCollocationInterface::Ptr fd_eval )

inline

Set finite differences collocation method.

Definition at line 202 of file full_discretization_grid_base_se2.h.

◆ setInitialDt()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setInitialDt ( double dt )

inlineoverridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 174 of file full_discretization_grid_base_se2.h.

◆ setN()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setN	(	int	n,
		bool	try_resample = `true`
	)

overridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 558 of file full_discretization_grid_base_se2.cpp.

◆ setNRef()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setNRef ( int n )

set reference horizon length

Definition at line 370 of file full_discretization_grid_base_se2.cpp.

◆ setWarmStart()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setWarmStart ( bool active )

inline

activate or deactive warmstart

Definition at line 193 of file full_discretization_grid_base_se2.h.

◆ setXfFixed()

void mpc_local_planner::FullDiscretizationGridBaseSE2::setXfFixed ( const Eigen::Matrix< bool, -1, 1 > & xf_fixed )

inline

Set individual components of the final state to fixed or unfixed.

Definition at line 195 of file full_discretization_grid_base_se2.h.

◆ update()

corbo::GridUpdateResult mpc_local_planner::FullDiscretizationGridBaseSE2::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`
	)

overridevirtual

Implements corbo::DiscretizationGridInterface.

Definition at line 58 of file full_discretization_grid_base_se2.cpp.

◆ updateBounds()

void mpc_local_planner::FullDiscretizationGridBaseSE2::updateBounds ( const NlpFunctions & nlp_fun )

protected

Definition at line 425 of file full_discretization_grid_base_se2.cpp.

◆ warmStartShifting()

void mpc_local_planner::FullDiscretizationGridBaseSE2::warmStartShifting ( const Eigen::VectorXd & x0 )

protectedvirtual

Definition at line 261 of file full_discretization_grid_base_se2.cpp.

Member Data Documentation

◆ _active_vertices

std::vector<VertexInterface*> mpc_local_planner::FullDiscretizationGridBaseSE2::_active_vertices

protected

Definition at line 251 of file full_discretization_grid_base_se2.h.

◆ _cost_integration

CostIntegrationRule mpc_local_planner::FullDiscretizationGridBaseSE2::_cost_integration

protected

Definition at line 267 of file full_discretization_grid_base_se2.h.

◆ _dt

ScalarVertex mpc_local_planner::FullDiscretizationGridBaseSE2::_dt

protected

Definition at line 258 of file full_discretization_grid_base_se2.h.

◆ _dt_lb

double mpc_local_planner::FullDiscretizationGridBaseSE2::_dt_lb = 0

protected

Definition at line 264 of file full_discretization_grid_base_se2.h.

◆ _dt_ref

double mpc_local_planner::FullDiscretizationGridBaseSE2::_dt_ref = 0.1

protected

Definition at line 257 of file full_discretization_grid_base_se2.h.

◆ _dt_ub

double mpc_local_planner::FullDiscretizationGridBaseSE2::_dt_ub = corbo::CORBO_INF_DBL

protected

Definition at line 265 of file full_discretization_grid_base_se2.h.

◆ _fd_eval

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

protected

Definition at line 246 of file full_discretization_grid_base_se2.h.

◆ _first_run

bool mpc_local_planner::FullDiscretizationGridBaseSE2::_first_run = true

protected

Definition at line 260 of file full_discretization_grid_base_se2.h.

◆ _n_adapt

int mpc_local_planner::FullDiscretizationGridBaseSE2::_n_adapt = 0

protected

Definition at line 256 of file full_discretization_grid_base_se2.h.

◆ _n_ref

int mpc_local_planner::FullDiscretizationGridBaseSE2::_n_ref = 11

protected

Definition at line 255 of file full_discretization_grid_base_se2.h.

◆ _nlp_fun

const NlpFunctions* mpc_local_planner::FullDiscretizationGridBaseSE2::_nlp_fun = nullptr

protected

Definition at line 253 of file full_discretization_grid_base_se2.h.

◆ _u_seq

std::vector<VectorVertex> mpc_local_planner::FullDiscretizationGridBaseSE2::_u_seq

protected

Definition at line 249 of file full_discretization_grid_base_se2.h.

◆ _warm_start

bool mpc_local_planner::FullDiscretizationGridBaseSE2::_warm_start = false

protected

Definition at line 259 of file full_discretization_grid_base_se2.h.

◆ _x_seq

std::vector<VectorVertexSE2> mpc_local_planner::FullDiscretizationGridBaseSE2::_x_seq

protected

Definition at line 248 of file full_discretization_grid_base_se2.h.

◆ _xf

PartiallyFixedVectorVertexSE2 mpc_local_planner::FullDiscretizationGridBaseSE2::_xf

protected

Definition at line 250 of file full_discretization_grid_base_se2.h.

◆ _xf_fixed

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

protected

Definition at line 263 of file full_discretization_grid_base_se2.h.

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

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ NlpFunctions

◆ OptimizationEdgeSet

◆ PartiallyFixedVectorVertex

◆ Ptr

◆ ReferenceTrajectoryInterface

◆ ScalarVertex

◆ SystemDynamicsInterface

◆ TimeSeries

◆ UPtr

◆ VectorVertex

◆ VertexInterface

Member Enumeration Documentation

◆ CostIntegrationRule

Constructor & Destructor Documentation

◆ FullDiscretizationGridBaseSE2()

◆ ~FullDiscretizationGridBaseSE2()

Member Function Documentation

◆ adaptGrid()

◆ checkAndInitializeXfFixedFlags()

◆ clear()

◆ computeActiveVertices()

◆ createEdges()

◆ findClosestPose()

◆ findNearestState()

◆ getActiveVertices()

◆ getDt()

◆ getDtRef()

◆ getFinalTime()

◆ getFirstControlInput()

◆ getFirstDt()

◆ getInitialDt()

◆ getInitialN()

◆ getInstance()

◆ getN()

◆ getNRef()

◆ getState()

◆ getStateAndControlTimeSeries()

◆ getVertices()

◆ hasConstantControls()

◆ hasSingleDt()

◆ initializeSequences() [1/2]

◆ initializeSequences() [2/2]

◆ isDtFixedIntended()

◆ isEmpty()

◆ isGridAdaptActive()

◆ isMovingHorizonWarmStartActive()

◆ isTimeVariableGrid()

◆ isUniformGrid()

◆ isValid()

◆ providesStateTrajectory()

◆ resampleTrajectory()

◆ setCostIntegrationRule()

◆ setDtRef()

◆ setFiniteDifferencesCollocationMethod()

◆ setInitialDt()

◆ setN()

◆ setNRef()

◆ setWarmStart()

◆ setXfFixed()

◆ update()

◆ updateBounds()

◆ warmStartShifting()

Member Data Documentation

◆ _active_vertices

◆ _cost_integration

◆ _dt

◆ _dt_lb

◆ _dt_ref

◆ _dt_ub

◆ _fd_eval

◆ _first_run

◆ _n_adapt

◆ _n_ref