Quadratic form running cost (specialized for SE2) More...

#include <quadratic_cost_se2.h>

Inheritance diagram for mpc_local_planner::QuadraticFormCostSE2:

[legend]

Public Types
using	Ptr = std::shared_ptr< QuadraticFormCostSE2 >

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

Public Types inherited from corbo::StageCost
typedef std::shared_ptr< const StageCost >	ConstPtr

typedef std::shared_ptr< StageCost >	Ptr

Public Types inherited from corbo::StageFunction
typedef std::shared_ptr< const StageFunction >	ConstPtr

typedef std::shared_ptr< StageFunction >	Ptr

Public Member Functions
void	computeIntegralStateControlTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, const Eigen::Ref< const Eigen::VectorXd > &u_k, Eigen::Ref< Eigen::VectorXd > cost) const override

void	computeNonIntegralStateTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override

corbo::StageCost::Ptr	getInstance () const override

	QuadraticFormCostSE2 ()
	Default constructor. More...

	QuadraticFormCostSE2 (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, bool integral_form=false, bool lsq_form=false)
	Construct with weight matrices. More...

Public Member Functions inherited from corbo::QuadraticFormCost
bool	checkParameters (int state_dim, int control_dim, std::stringstream *issues) const override

void	computeNonIntegralControlTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &u_k, Eigen::Ref< Eigen::VectorXd > cost) const override

int	getIntegralStateControlTermDimension (int k) const override

int	getNonIntegralControlTermDimension (int k) const override

int	getNonIntegralStateTermDimension (int k) const override

const Eigen::MatrixXd &	getWeightQ () const

const Eigen::MatrixXd &	getWeightR () const

bool	hasIntegralTerms (int k) const override

bool	hasNonIntegralTerms (int k) const override

bool	isLsqFormNonIntegralControlTerm (int k) const override

bool	isLsqFormNonIntegralStateTerm (int k) const override

	QuadraticFormCost ()

	QuadraticFormCost (const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, bool integral_form=false, bool lsq_form=false)

void	scaleCurrentWeightQ (double scale)

void	scaleCurrentWeightR (double scale)

void	setIntegralForm (bool integral_form)

void	setLsqForm (bool lsq_form)

bool	setWeightQ (const Eigen::DiagonalMatrix< double, -1 > &Q)

bool	setWeightQ (const Eigen::Ref< const Eigen::MatrixXd > &Q)

bool	setWeightR (const Eigen::DiagonalMatrix< double, -1 > &R)

bool	setWeightR (const Eigen::Ref< const Eigen::MatrixXd > &R)

bool	update (int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface sref, bool single_dt, const Eigen::VectorXd &x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface ) override

Public Member Functions inherited from corbo::StageFunction
virtual void	computeConcatenatedNonIntegralStateControlTerms (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, const Eigen::Ref< const Eigen::VectorXd > &u_k, double dt_k, Eigen::Ref< Eigen::VectorXd > cost, bool lsq_mode=false) const

virtual void	computeConcatenatedNonIntegralStateTerms (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, const Eigen::Ref< const Eigen::VectorXd > &u_k, double dt_k, Eigen::Ref< Eigen::VectorXd > cost, bool lsq_mode=false) const

virtual void	computeNonIntegralControlDeviationTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &u_k, const Eigen::Ref< const Eigen::VectorXd > &u_prev, double dt_prev, Eigen::Ref< Eigen::VectorXd > cost) const

virtual void	computeNonIntegralDtTerm (int k, double dt, Eigen::Ref< Eigen::VectorXd > cost) const

virtual void	computeNonIntegralStateControlDtTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, const Eigen::Ref< const Eigen::VectorXd > &u_k, double dt_k, Eigen::Ref< Eigen::VectorXd > cost) const

virtual void	computeNonIntegralStateControlTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, const Eigen::Ref< const Eigen::VectorXd > &u_k, Eigen::Ref< Eigen::VectorXd > cost) const

virtual void	computeNonIntegralStateDtTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, double dt_k, Eigen::Ref< Eigen::VectorXd > cost) const

virtual int	getConcatenatedNonIntegralStateControlTermDimension (int k, bool lsq_mode=false) const

virtual int	getConcatenatedNonIntegralStateTermDimension (int k, bool lsq_mode=false) const

virtual int	getNonIntegralControlDeviationTermDimension (int k) const

virtual int	getNonIntegralDtTermDimension (int k) const

virtual int	getNonIntegralStateControlDtTermDimension (int k) const

virtual int	getNonIntegralStateControlTermDimension (int k) const

virtual int	getNonIntegralStateDtTermDimension (int k) const

virtual bool	isLinearNonIntegralControlTerm (int k) const

virtual bool	isLinearNonIntegralDtTerm (int k) const

virtual bool	isLinearNonIntegralStateTerm (int k) const

virtual bool	isLsqFormNonIntegralDtTerm (int k) const

virtual	~StageFunction ()=default

Additional Inherited Members
Protected Attributes inherited from corbo::QuadraticFormCost
bool	_integral_form

bool	_lsq_form

Eigen::MatrixXd	_Q

Eigen::DiagonalMatrix< double, -1 >	_Q_diag

Eigen::DiagonalMatrix< double, -1 >	_Q_diag_sqrt

bool	_Q_diagonal_mode

bool	_Q_diagonal_mode_intentionally

Eigen::MatrixXd	_Q_sqrt

Eigen::MatrixXd	_R

Eigen::DiagonalMatrix< double, -1 >	_R_diag

Eigen::DiagonalMatrix< double, -1 >	_R_diag_sqrt

bool	_R_diagonal_mode

bool	_R_diagonal_mode_intentionally

Eigen::MatrixXd	_R_sqrt

const ReferenceTrajectoryInterface *	_u_ref

const ReferenceTrajectoryInterface *	_x_ref

bool	_zero_u_ref

bool	_zero_x_ref

Detailed Description

Quadratic form running cost (specialized for SE2)

This class implements quadratic form running costs

$l(x, u) = (x_{ref} - x)^T Q (x_{xref} - x) + (u_{ref} - u)^T R (u_{ref} - u)$

However, this class ensures that the state distance (x_{ref} - x) is computed properly in SO(2) for the third state component.

Note, the class also provides a least squares form if not in integral form which is: $l_{lsq} = \begin{bmatrix} \sqrt{Q} (x_{ref} - x) \\ \sqrt{R} (u_{ref} - u) \end{bmatrix}$ $\sqrt{Q}$ $\sqrt{R}$ are computed once using the cholesky decomposition. This form is later squared by the solver.

See also: corbo::QuadraticFinalStateCost

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 73 of file quadratic_cost_se2.h.

Member Typedef Documentation

◆ Ptr

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

Definition at line 96 of file quadratic_cost_se2.h.

Constructor & Destructor Documentation

◆ QuadraticFormCostSE2() [1/2]

mpc_local_planner::QuadraticFormCostSE2::QuadraticFormCostSE2 ( )

inline

Default constructor.

Definition at line 99 of file quadratic_cost_se2.h.

◆ QuadraticFormCostSE2() [2/2]

mpc_local_planner::QuadraticFormCostSE2::QuadraticFormCostSE2	(	const Eigen::Ref< const Eigen::MatrixXd > &	Q,
		const Eigen::Ref< const Eigen::MatrixXd > &	R,
		bool	integral_form = `false`,
		bool	lsq_form = `false`
	)

inline

Construct with weight matrices.

Parameters

[in]	Q	Positive definite state weighting matrix (must match dimensions!)
[in]	R	Positive definite control weighting matrix (must match dimensions!)
[in]	integral_form	If true, the running costs are later integrated over the grid interval, otherwise, the values are just summed up as in sampled-data MPC.
[in]	lsq_form	Set to true in order to prefer the least-squares form

Definition at line 110 of file quadratic_cost_se2.h.

Member Function Documentation

◆ computeIntegralStateControlTerm()

void mpc_local_planner::QuadraticFormCostSE2::computeIntegralStateControlTerm	(	int	k,
		const Eigen::Ref< const Eigen::VectorXd > &	x_k,
		const Eigen::Ref< const Eigen::VectorXd > &	u_k,
		Eigen::Ref< Eigen::VectorXd >	cost
	)		const

overridevirtual

Reimplemented from corbo::QuadraticFormCost.

Definition at line 74 of file quadratic_cost_se2.cpp.

◆ computeNonIntegralStateTerm()

void mpc_local_planner::QuadraticFormCostSE2::computeNonIntegralStateTerm	(	int	k,
		const Eigen::Ref< const Eigen::VectorXd > &	x_k,
		Eigen::Ref< Eigen::VectorXd >	cost
	)		const

overridevirtual

Reimplemented from corbo::QuadraticFormCost.

Definition at line 51 of file quadratic_cost_se2.cpp.

◆ getInstance()

corbo::StageCost::Ptr mpc_local_planner::QuadraticFormCostSE2::getInstance ( ) const

inlineoverridevirtual

Reimplemented from corbo::QuadraticFormCost.

Definition at line 117 of file quadratic_cost_se2.h.

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

Public Types

Public Member Functions

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ Ptr

Constructor & Destructor Documentation

◆ QuadraticFormCostSE2() [1/2]

◆ QuadraticFormCostSE2() [2/2]

Member Function Documentation

◆ computeIntegralStateControlTerm()

◆ computeNonIntegralStateTerm()

◆ getInstance()