aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
expose-cost-stack.cpp
Go to the documentation of this file.
3
6
7#include <eigenpy/std-pair.hpp>
8#include <eigenpy/variant.hpp>
9#include <eigenpy/std-map.hpp>
10
11namespace aligator {
12namespace python {
16using context::Scalar;
19
21 char msg[100];
22 std::visit(
24 [&](size_t k) { snprintf(msg, 100ul, "Key %zu not found.", k); },
25 [&](const std::string &k) {
26 snprintf(msg, 100ul, "Key %s not found.", k.data());
27 },
28 },
29 key);
30 PyErr_SetString(PyExc_KeyError, msg);
31 bp::throw_error_already_set();
32};
33
35 using CostStackData = CostStackDataTpl<Scalar>;
37 using PolyManifold = xyz::polymorphic<Manifold>;
38 using CostItem = CostStack::CostItem;
39 using CostMap = CostStack::CostMap;
40 eigenpy::StdPairConverter<CostItem>::registration();
41 eigenpy::VariantConverter<CostKey>::registration();
42 eigenpy::GenericMapVisitor<CostMap, true>::expose("CostMap");
43
44 {
45 bp::scope scope =
46 bp::class_<CostStack, bp::bases<CostAbstract>>(
47 "CostStack", "A weighted sum of other cost functions.", bp::no_init)
48 .def(
49 bp::init<PolyManifold, const int, const std::vector<PolyCost> &,
50 const std::vector<Scalar> &>(
51 ("self"_a, "space", "nu", "components"_a = bp::list(),
52 "weights"_a = bp::list())))
53 .def(bp::init<const PolyCost &>(("self"_a, "cost")))
54 .def(bp::init<PolyManifold, int, const CostMap &>(
55 ("self"_a, "components"),
56 "Construct the CostStack from a CostMap object."))
57 .def_readonly("components", &CostStack::components_,
58 "Components of this cost stack.")
59 .def(
60 "getComponent",
61 +[](CostStack &self, const CostKey &key) -> PolyCost & {
62 if (!self.components_.contains(key)) {
64 }
65 CostItem &c = self.components_.at(key);
66 return c.first;
67 },
68 ("self"_a, "key"), bp::return_internal_reference<>())
69 .def(
70 "getWeight",
71 +[](CostStack &self, const CostKey &key) {
72 return self.getWeight(key);
73 },
74 ("self"_a, "key"))
75 .def("setWeight", &CostStack::setWeight, ("self"_a, "key", "value"),
76 bp::return_internal_reference<>())
77 .def(
78 "addCost",
79 +[](CostStack &self, const PolyCost &cost, const Scalar weight)
80 -> PolyCost & { return self.addCost(cost, weight).first; },
81 bp::return_internal_reference<>(),
82 ("self"_a, "cost", "weight"_a = 1.))
83 .def(
84 "addCost",
85 +[](CostStack &self, CostKey key, const PolyCost &cost,
86 const Scalar weight) -> PolyCost & {
87 return self.addCost(key, cost, weight).first;
88 },
89 bp::return_internal_reference<>(),
90 ("self"_a, "key", "cost", "weight"_a = 1.))
91 .add_property("size", &CostStack::size,
92 "Number of cost components.")
95 }
96
97 {
98 bp::register_ptr_to_python<shared_ptr<CostStackData>>();
99 bp::scope scope =
100 bp::class_<CostStackData, bp::bases<CostData>>(
101 "CostStackData", "Data struct for CostStack.", bp::no_init)
102 .def_readonly("sub_cost_data", &CostStackData::sub_cost_data);
103 eigenpy::GenericMapVisitor<CostStackData::DataMap, true>::expose("CostMap");
104 }
105}
106
107} // namespace python
108} // namespace aligator
CostAbstractTpl< Scalar > CostAbstract
Definition context.hpp:26
ManifoldAbstractTpl< Scalar > Manifold
Definition context.hpp:14
CostDataAbstractTpl< Scalar > CostData
Definition context.hpp:27
The Python bindings.
Definition blk-matrix.hpp:5
xyz::polymorphic< Manifold > PolyManifold
void key_err_python_manager(const CostKey &key)
xyz::polymorphic< CostAbstract > PolyCost
CostStack::CostKey CostKey
void exposeCostStack()
fwd-declare exposeCostStack()
CostStackTpl< Scalar > CostStack
Main package namespace.
Weighted sum of multiple cost components.
CostItem & addCost(const PolyCost &cost, const Scalar weight=1.)
std::variant< std::size_t, std::string > CostKey
xyz::polymorphic< CostBase > PolyCost
boost::unordered::unordered_map< CostKey, CostItem > CostMap
Scalar getWeight(const CostKey &key) const
Get weight for a given component.
CostStackTpl & setWeight(const CostKey &key, const Scalar value)
std::pair< PolyCost, Scalar > CostItem
Utility helper struct for creating visitors from lambdas.
Definition overloads.hpp:6