Quadratic final state cost (specialized for SE2) More...

#include <final_state_conditions_se2.h>

Inheritance diagram for mpc_local_planner::QuadraticFinalStateCostSE2:

[legend]

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

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

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

typedef std::shared_ptr< BaseQuadraticFinalStateCost >	Ptr

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

typedef std::shared_ptr< FinalStageCost >	Ptr

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

typedef std::shared_ptr< StageFunction >	Ptr

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

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

	QuadraticFinalStateCostSE2 ()

	QuadraticFinalStateCostSE2 (const Eigen::Ref< const Eigen::MatrixXd > &Qf, bool lsq_form=false)

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

int	getNonIntegralStateTermDimension (int k) const override

const Eigen::MatrixXd &	getWeightQf () const override

bool	isLsqFormNonIntegralStateTerm (int k) const override

	QuadraticFinalStateCost ()

	QuadraticFinalStateCost (const Eigen::Ref< const Eigen::MatrixXd > &Qf, bool lsq_form)

void	setLsqForm (bool lsq_form)

bool	setWeightQf (const Eigen::DiagonalMatrix< double, -1 > &Qf)

bool	setWeightQf (const Eigen::Ref< const Eigen::MatrixXd > &Qf)

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

Public Member Functions inherited from corbo::FinalStageCost
bool	hasIntegralTerms (int k) const final

bool	hasNonIntegralTerms (int k) const final

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	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	getNonIntegralStateDtTermDimension (int k) const

virtual bool	isLinearNonIntegralControlTerm (int k) const

virtual bool	isLinearNonIntegralDtTerm (int k) const

virtual bool	isLinearNonIntegralStateTerm (int k) const

virtual bool	isLsqFormNonIntegralControlTerm (int k) const

virtual bool	isLsqFormNonIntegralDtTerm (int k) const

virtual	~StageFunction ()=default

Additional Inherited Members
Protected Attributes inherited from corbo::QuadraticFinalStateCost
bool	_diagonal_mode

bool	_diagonal_mode_intentionally

bool	_lsq_form

Eigen::MatrixXd	_Qf

Eigen::DiagonalMatrix< double, -1 >	_Qf_diag

Eigen::DiagonalMatrix< double, -1 >	_Qf_diag_sqrt

Eigen::MatrixXd	_Qf_sqrt

const ReferenceTrajectoryInterface *	_x_ref

bool	_zero_x_ref

Detailed Description

Quadratic final state cost (specialized for SE2)

This class implements quadratic final costs

$J_f(x) = (x_{ref} - x)^T Q_f (x_{xref} - x)$

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

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 68 of file final_state_conditions_se2.h.

Member Typedef Documentation

◆ Ptr

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

Definition at line 91 of file final_state_conditions_se2.h.

Constructor & Destructor Documentation

◆ QuadraticFinalStateCostSE2() [1/2]

mpc_local_planner::QuadraticFinalStateCostSE2::QuadraticFinalStateCostSE2 ( )

inline

Definition at line 93 of file final_state_conditions_se2.h.

◆ QuadraticFinalStateCostSE2() [2/2]

mpc_local_planner::QuadraticFinalStateCostSE2::QuadraticFinalStateCostSE2	(	const Eigen::Ref< const Eigen::MatrixXd > &	Qf,
		bool	lsq_form = `false`
	)

inline

Definition at line 95 of file final_state_conditions_se2.h.

Member Function Documentation

◆ computeNonIntegralStateTerm()

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