aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
sum-of-costs.hpp
Go to the documentation of this file.
1#pragma once
2
4// Faster than std::unordered_map with Bost 1.80
5// https://martin.ankerl.com/2022/08/27/hashmap-bench-01/#boost__unordered_map
6#include <boost/unordered_map.hpp>
7
8namespace aligator {
9
10template <typename Scalar> struct CostStackDataTpl;
11
19template <typename _Scalar> struct CostStackTpl : CostAbstractTpl<_Scalar> {
20 using Scalar = _Scalar;
24 using PolyCost = xyz::polymorphic<CostBase>;
27 using CostItem = std::pair<PolyCost, Scalar>;
28 using CostKey = std::variant<std::size_t, std::string>;
29 using CostMap = boost::unordered::unordered_map<CostKey, CostItem>;
30 using CostIterator = typename CostMap::iterator;
31
33
37 bool checkDimension(const CostBase &comp) const;
38
41 CostStackTpl(xyz::polymorphic<Manifold> space, const int nu,
42 const std::vector<PolyCost> &comps = {},
43 const std::vector<Scalar> &weights = {});
44
45 CostStackTpl(xyz::polymorphic<Manifold> space, const int nu,
46 const CostMap &comps)
47 : CostBase(space, nu)
48 , components_(comps) {
49 for (const auto &[key, item] : comps) {
50 auto &cost = *item.first;
51 if (!this->checkDimension(cost)) {
53 "Cannot add new component due to inconsistent input dimensions "
54 "(got ({:d}, {:d}), expected ({:d}, {:d}))",
55 cost.ndx(), cost.nu, this->ndx(), this->nu);
56 }
57 }
58 }
59
61 CostStackTpl(const PolyCost &cost);
62
63 inline CostItem &addCost(const PolyCost &cost, const Scalar weight = 1.) {
64 const std::size_t size = components_.size();
65 return this->addCost(size, cost, weight);
66 }
67
68 CostItem &addCost(const CostKey &key, const PolyCost &cost,
69 const Scalar weight = 1.);
70
71 inline std::size_t size() const { return components_.size(); }
72
74 template <typename Derived> Derived *getComponent(const CostKey &key) {
75 CostItem &item = components_.at(key);
76 return dynamic_cast<Derived *>(&*item.first);
77 }
78
80 template <typename Derived>
81 const Derived *getComponent(const CostKey &key) const {
82 CostItem &item = components_.at(key);
83 return dynamic_cast<const Derived *>(&*item.first);
84 }
85
87 Scalar getWeight(const CostKey &key) const {
88 const CostItem &item = components_.at(key);
89 return item.second;
90 }
91
94 Scalar &getWeight(const CostKey &key) {
95 CostItem &item = components_.at(key);
96 return item.second;
97 }
98
101 CostStackTpl &setWeight(const CostKey &key, const Scalar value) {
102 CostItem &item = components_.at(key);
103 item.second = value;
104 return *this;
105 }
106
107 void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
108 CostData &data) const;
109
110 void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u,
111 CostData &data) const;
112
113 void computeHessians(const ConstVectorRef &x, const ConstVectorRef &u,
114 CostData &data) const;
115
116 shared_ptr<CostData> createData() const;
117};
118
119namespace {
120template <typename T>
121using CostPtr =
122 xyz::polymorphic<CostAbstractTpl<T>>; //< convenience typedef for rest
123 // of this file
124}
125
126template <typename T>
127xyz::polymorphic<CostStackTpl<T>> operator+(const CostPtr<T> &c1,
128 const CostPtr<T> &c2) {
129 return xyz::polymorphic<CostStackTpl<T>>({c1, c2}, {1., 1.});
130}
131
132template <typename T>
133xyz::polymorphic<CostStackTpl<T>>
134operator+(xyz::polymorphic<CostStackTpl<T>> &&c1, const CostPtr<T> &c2) {
135 c1->addCost(c2, 1.);
136 return c1;
137}
138
139template <typename T>
140xyz::polymorphic<CostStackTpl<T>>
141operator+(xyz::polymorphic<CostStackTpl<T>> &&c1, CostPtr<T> &&c2) {
142 c1->addCost(std::move(c2), 1.);
143 return c1;
144}
145
146template <typename T>
147xyz::polymorphic<CostStackTpl<T>>
148operator+(const xyz::polymorphic<CostStackTpl<T>> &c1, CostPtr<T> &&c2) {
149 c1->addCost(std::move(c2), 1.);
150 return c1;
151}
152
153template <typename T>
154xyz::polymorphic<CostStackTpl<T>>
155operator*(T u, xyz::polymorphic<CostStackTpl<T>> &&c1) {
156 for (auto &[key, item] : c1->components_) {
157 item.second *= u;
158 }
159 return c1;
160}
161
162template <typename _Scalar>
164 using Scalar = _Scalar;
167 using CostKey = typename CostStack::CostKey;
168 using DataMap =
169 boost::unordered::unordered_map<CostKey, shared_ptr<CostData>>;
172};
173} // namespace aligator
174
175#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
176#include "aligator/modelling/costs/sum-of-costs.txx"
177#endif
#define ALIGATOR_DOMAIN_ERROR(...)
Main package namespace.
xyz::polymorphic< CostStackTpl< T > > operator*(T u, xyz::polymorphic< CostStackTpl< T > > &&c1)
xyz::polymorphic< CostStackTpl< T > > operator+(const CostPtr< T > &c1, const CostPtr< T > &c2)
xyz::polymorphic< Manifold > space
CostAbstractTpl(U &&space, const int nu)
Data struct for CostAbstractTpl.
CostDataAbstractTpl(const int ndx, const int nu)
CostStackDataTpl(const CostStackTpl< Scalar > &obj)
typename CostStack::CostKey CostKey
boost::unordered::unordered_map< CostKey, shared_ptr< CostData > > DataMap
CostDataAbstractTpl< Scalar > CostData
Weighted sum of multiple cost components.
CostItem & addCost(const PolyCost &cost, const Scalar weight=1.)
CostItem & addCost(const CostKey &key, const PolyCost &cost, const Scalar weight=1.)
void computeGradients(const ConstVectorRef &x, const ConstVectorRef &u, CostData &data) const
Compute the cost gradients .
std::variant< std::size_t, std::string > CostKey
ManifoldAbstractTpl< Scalar > Manifold
Derived * getComponent(const CostKey &key)
Get component, cast down to the specified type.
xyz::polymorphic< CostBase > PolyCost
CostStackTpl(const PolyCost &cost)
Constructor from a single CostBase instance.
CostStackTpl(xyz::polymorphic< Manifold > space, const int nu, const std::vector< PolyCost > &comps={}, const std::vector< Scalar > &weights={})
Constructor with a specified dimension, and optional vector of components and weights.
boost::unordered::unordered_map< CostKey, CostItem > CostMap
Scalar getWeight(const CostKey &key) const
Get weight for a given component.
CostStackTpl(xyz::polymorphic< Manifold > space, const int nu, const CostMap &comps)
CostAbstractTpl< Scalar > CostBase
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
CostStackTpl & setWeight(const CostKey &key, const Scalar value)
Set the weight for a given component.
Scalar & getWeight(const CostKey &key)
Get weight for a given component.
void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, CostData &data) const
Evaluate the cost function.
bool checkDimension(const CostBase &comp) const
Check the dimension of a component.
typename CostMap::iterator CostIterator
const Derived * getComponent(const CostKey &key) const
Get component, cast down to the specified type.
void computeHessians(const ConstVectorRef &x, const ConstVectorRef &u, CostData &data) const
Compute the cost Hessians .
shared_ptr< CostData > createData() const
CostDataAbstractTpl< Scalar > CostData
std::pair< PolyCost, Scalar > CostItem
CostStackDataTpl< Scalar > SumCostData
Base class for manifolds, to use in cost funcs, solvers...