Finite differences grid for SE2. More...

#include <finite_differences_grid_se2.h>

Inheritance diagram for mpc_local_planner::FiniteDifferencesGridSE2:

[legend]

Public Types
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
	FiniteDifferencesGridSE2 ()=default
	Default constructor. More...

corbo::DiscretizationGridInterface::Ptr	getInstance () const override
	Return a newly created shared instance of the implemented class. 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 corbo::DiscretizationGridInterface
static Factory< DiscretizationGridInterface > &	getFactory ()

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

bool	isDtFixedIntended () const override

Protected Member Functions inherited from mpc_local_planner::FullDiscretizationGridBaseSE2
virtual bool	adaptGrid (bool new_run, NlpFunctions &nlp_fun)

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

Additional Inherited Members
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 for SE2.

This class implements a full discretization grid with finite difference collocation. The temporal resolution is fixed.

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 43 of file finite_differences_grid_se2.h.

Member Typedef Documentation

◆ BaseEdge

using mpc_local_planner::FiniteDifferencesGridSE2::BaseEdge = corbo::BaseEdge

Definition at line 48 of file finite_differences_grid_se2.h.

◆ BaseMixedEdge

using mpc_local_planner::FiniteDifferencesGridSE2::BaseMixedEdge = corbo::BaseMixedEdge

Definition at line 49 of file finite_differences_grid_se2.h.

◆ Ptr

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

Definition at line 46 of file finite_differences_grid_se2.h.

Constructor & Destructor Documentation

◆ FiniteDifferencesGridSE2()

mpc_local_planner::FiniteDifferencesGridSE2::FiniteDifferencesGridSE2 ( )

default

Default constructor.

◆ ~FiniteDifferencesGridSE2()

virtual mpc_local_planner::FiniteDifferencesGridSE2::~FiniteDifferencesGridSE2 ( )

virtualdefault

Default destructor.

Member Function Documentation

◆ createEdges()

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