23 #ifndef MIN_TIME_VIA_POINTS_COSTS_H_ 24 #define MIN_TIME_VIA_POINTS_COSTS_H_ 53 using Ptr = std::shared_ptr<MinTimeViaPointsCost>;
121 std::vector<std::pair<std::vector<const teb_local_planner::PoseSE2*>, std::size_t>>
_vp_association;
127 #endif // MIN_TIME_VIA_POINTS_COSTS_H_
int getNonIntegralDtTermDimension(int k) const override
Hybrid cost function with minimum time and via-point objectives.
std::vector< std::pair< std::vector< const teb_local_planner::PoseSE2 * >, std::size_t > > _vp_association
double _vp_position_weight
void setViaPointContainer(const ViaPointContainer &via_points)
Set reference to via-point container (warning, object must remain allocated)
std::vector< teb_local_planner::PoseSE2 > ViaPointContainer
MinTimeViaPointsCost()=default
Default constructor.
MinTimeViaPointsCost(const ViaPointContainer &via_points, double position_weight, double orientation_weight, bool via_points_ordered)
Construct with via-point container and weights.
bool hasIntegralTerms(int k) const override
int getNonIntegralStateTermDimension(int k) const override
void computeNonIntegralDtTerm(int k, double dt, Eigen::Ref< Eigen::VectorXd > cost) const override
bool isLsqFormNonIntegralDtTerm(int k) const override
std::shared_ptr< StagePreprocessor > Ptr
std::shared_ptr< StageCost > Ptr
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
bool hasNonIntegralTerms(int k) const override
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
void setViaPointOrderedMode(bool ordered)
Set if the optimzier should try to match the via-point ordering.
const ViaPointContainer * _via_points
double _vp_orientation_weight
void setViaPointWeights(double position_weight, double orientation_weight)
Set weights for via-point attraction.
corbo::StageCost::Ptr getInstance() const override