Public Types | Public Member Functions | Protected Attributes | List of all members
mpc_local_planner::MinTimeViaPointsCost Class Reference

Hybrid cost function with minimum time and via-point objectives. More...

#include <min_time_via_points_cost.h>

Inheritance diagram for mpc_local_planner::MinTimeViaPointsCost:
Inheritance graph
[legend]

Public Types

using Ptr = std::shared_ptr< MinTimeViaPointsCost >
 
using ViaPointContainer = std::vector< teb_local_planner::PoseSE2 >
 
- 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 computeNonIntegralDtTerm (int k, double dt, 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
 
int getNonIntegralDtTermDimension (int k) const override
 
int getNonIntegralStateTermDimension (int k) const override
 
bool hasIntegralTerms (int k) const override
 
bool hasNonIntegralTerms (int k) const override
 
bool isLsqFormNonIntegralDtTerm (int k) const override
 
 MinTimeViaPointsCost ()=default
 Default constructor. More...
 
 MinTimeViaPointsCost (const ViaPointContainer &via_points, double position_weight, double orientation_weight, bool via_points_ordered)
 Construct with via-point container and weights. More...
 
void setViaPointContainer (const ViaPointContainer &via_points)
 Set reference to via-point container (warning, object must remain allocated) More...
 
void setViaPointOrderedMode (bool ordered)
 Set if the optimzier should try to match the via-point ordering. More...
 
void setViaPointWeights (double position_weight, double orientation_weight)
 Set weights for via-point attraction. More...
 
bool update (int n, double, corbo::ReferenceTrajectoryInterface &, corbo::ReferenceTrajectoryInterface &, corbo::ReferenceTrajectoryInterface *, bool single_dt, const Eigen::VectorXd &x0, corbo::StagePreprocessor::Ptr, const std::vector< double > &, const corbo::DiscretizationGridInterface *grid) override
 
- Public Member Functions inherited from corbo::StageFunction
virtual bool checkParameters (int state_dim, int control_dim, std::stringstream *issues) const
 
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 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
 
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 computeNonIntegralControlTerm (int k, const Eigen::Ref< const Eigen::VectorXd > &u_k, 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 getIntegralStateControlTermDimension (int k) const
 
virtual int getNonIntegralControlDeviationTermDimension (int k) const
 
virtual int getNonIntegralControlTermDimension (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 isLsqFormNonIntegralControlTerm (int k) const
 
virtual bool isLsqFormNonIntegralStateTerm (int k) const
 
virtual ~StageFunction ()=default
 

Protected Attributes

bool _single_dt = false
 
double _time_weight = 1
 
const ViaPointContainer * _via_points = nullptr
 
bool _via_points_ordered = false
 
std::vector< std::pair< std::vector< const teb_local_planner::PoseSE2 * >, std::size_t > > _vp_association
 
double _vp_orientation_weight = 0
 
double _vp_position_weight = 1e-3
 

Detailed Description

Hybrid cost function with minimum time and via-point objectives.

This class defines a joint objective of minimum time and via-point attraction. A user defined weight controls the trade-off between both individual objectives.

The current via-point strategy is borrowed from the teb_local_planner.

Todo:
we can implement this class as LSQ version as well
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 70 of file min_time_via_points_cost.h.

Member Typedef Documentation

◆ Ptr

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

Definition at line 93 of file min_time_via_points_cost.h.

◆ ViaPointContainer

using mpc_local_planner::MinTimeViaPointsCost::ViaPointContainer = std::vector<teb_local_planner::PoseSE2>

Definition at line 95 of file min_time_via_points_cost.h.

Constructor & Destructor Documentation

◆ MinTimeViaPointsCost() [1/2]

mpc_local_planner::MinTimeViaPointsCost::MinTimeViaPointsCost ( )
default

Default constructor.

◆ MinTimeViaPointsCost() [2/2]

mpc_local_planner::MinTimeViaPointsCost::MinTimeViaPointsCost ( const ViaPointContainer &  via_points,
double  position_weight,
double  orientation_weight,
bool  via_points_ordered 
)
inline

Construct with via-point container and weights.

Parameters
[in]via_pointsReference to the via-point container (must remain allocated)
[in]position_weightVia-point attraction weight for the position part
[in]orientation_weightVia-point attraction weight for the angular part
[in]via_points_orderedxTrue, if via-points should be associated w.r.t. the ordering in via_points

Definition at line 108 of file min_time_via_points_cost.h.

Member Function Documentation

◆ computeNonIntegralDtTerm()

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

Reimplemented from corbo::StageFunction.

Definition at line 140 of file min_time_via_points_cost.cpp.

◆ computeNonIntegralStateTerm()

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

Reimplemented from corbo::StageFunction.

Definition at line 150 of file min_time_via_points_cost.cpp.

◆ getInstance()

corbo::StageCost::Ptr mpc_local_planner::MinTimeViaPointsCost::getInstance ( ) const
inlineoverridevirtual

Implements corbo::StageCost.

Definition at line 116 of file min_time_via_points_cost.h.

◆ getNonIntegralDtTermDimension()

int mpc_local_planner::MinTimeViaPointsCost::getNonIntegralDtTermDimension ( int  k) const
inlineoverridevirtual

Reimplemented from corbo::StageFunction.

Definition at line 124 of file min_time_via_points_cost.h.

◆ getNonIntegralStateTermDimension()

int mpc_local_planner::MinTimeViaPointsCost::getNonIntegralStateTermDimension ( int  k) const
overridevirtual

Reimplemented from corbo::StageFunction.

Definition at line 53 of file min_time_via_points_cost.cpp.

◆ hasIntegralTerms()

bool mpc_local_planner::MinTimeViaPointsCost::hasIntegralTerms ( int  k) const
inlineoverridevirtual

Reimplemented from corbo::StageFunction.

Definition at line 121 of file min_time_via_points_cost.h.

◆ hasNonIntegralTerms()

bool mpc_local_planner::MinTimeViaPointsCost::hasNonIntegralTerms ( int  k) const
inlineoverridevirtual

Reimplemented from corbo::StageFunction.

Definition at line 119 of file min_time_via_points_cost.h.

◆ isLsqFormNonIntegralDtTerm()

bool mpc_local_planner::MinTimeViaPointsCost::isLsqFormNonIntegralDtTerm ( int  k) const
inlineoverridevirtual

Reimplemented from corbo::StageFunction.

Definition at line 126 of file min_time_via_points_cost.h.

◆ setViaPointContainer()

void mpc_local_planner::MinTimeViaPointsCost::setViaPointContainer ( const ViaPointContainer &  via_points)
inline

Set reference to via-point container (warning, object must remain allocated)

Definition at line 142 of file min_time_via_points_cost.h.

◆ setViaPointOrderedMode()

void mpc_local_planner::MinTimeViaPointsCost::setViaPointOrderedMode ( bool  ordered)
inline

Set if the optimzier should try to match the via-point ordering.

Definition at line 144 of file min_time_via_points_cost.h.

◆ setViaPointWeights()

void mpc_local_planner::MinTimeViaPointsCost::setViaPointWeights ( double  position_weight,
double  orientation_weight 
)
inline

Set weights for via-point attraction.

Definition at line 146 of file min_time_via_points_cost.h.

◆ update()

bool mpc_local_planner::MinTimeViaPointsCost::update ( int  n,
double  ,
corbo::ReferenceTrajectoryInterface &  ,
corbo::ReferenceTrajectoryInterface &  ,
corbo::ReferenceTrajectoryInterface *  ,
bool  single_dt,
const Eigen::VectorXd &  x0,
corbo::StagePreprocessor::Ptr  ,
const std::vector< double > &  ,
const corbo::DiscretizationGridInterface *  grid 
)
overridevirtual

Reimplemented from corbo::StageFunction.

Definition at line 60 of file min_time_via_points_cost.cpp.

Member Data Documentation

◆ _single_dt

bool mpc_local_planner::MinTimeViaPointsCost::_single_dt = false
protected

Definition at line 153 of file min_time_via_points_cost.h.

◆ _time_weight

double mpc_local_planner::MinTimeViaPointsCost::_time_weight = 1
protected

Definition at line 154 of file min_time_via_points_cost.h.

◆ _via_points

const ViaPointContainer* mpc_local_planner::MinTimeViaPointsCost::_via_points = nullptr
protected

Definition at line 160 of file min_time_via_points_cost.h.

◆ _via_points_ordered

bool mpc_local_planner::MinTimeViaPointsCost::_via_points_ordered = false
protected

Definition at line 156 of file min_time_via_points_cost.h.

◆ _vp_association

std::vector<std::pair<std::vector<const teb_local_planner::PoseSE2*>, std::size_t> > mpc_local_planner::MinTimeViaPointsCost::_vp_association
protected

Definition at line 161 of file min_time_via_points_cost.h.

◆ _vp_orientation_weight

double mpc_local_planner::MinTimeViaPointsCost::_vp_orientation_weight = 0
protected

Definition at line 158 of file min_time_via_points_cost.h.

◆ _vp_position_weight

double mpc_local_planner::MinTimeViaPointsCost::_vp_position_weight = 1e-3
protected

Definition at line 157 of file min_time_via_points_cost.h.

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

mpc_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Mar 2 2022 00:35:06