GCC Code Coverage Report


Directory: ./
File: bindings/python/crocoddyl/core/costs/cost-sum.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 61 63 96.8%
Branches: 138 276 50.0%

Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/core/costs/cost-sum.hpp"
11
12 #include <functional>
13 #include <map>
14 #include <memory>
15
16 #include "python/crocoddyl/core/action-base.hpp"
17 #include "python/crocoddyl/core/core.hpp"
18 #include "python/crocoddyl/core/diff-action-base.hpp"
19 #include "python/crocoddyl/utils/deprecate.hpp"
20 #include "python/crocoddyl/utils/map-converter.hpp"
21
22 namespace crocoddyl {
23 namespace python {
24
25 template <typename Model>
26 struct CostItemVisitor : public bp::def_visitor<CostItemVisitor<Model>> {
27 template <class PyClass>
28 40 void visit(PyClass& cl) const {
29
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
40 cl.def_readwrite("name", &Model::name, "cost name")
30
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
31 "cost",
32 bp::make_getter(&Model::cost,
33 bp::return_value_policy<bp::return_by_value>()),
34 "cost model")
35
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
40 .def_readwrite("weight", &Model::weight, "cost weight")
36
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
40 .def_readwrite("active", &Model::active, "cost status");
37 40 }
38 };
39
40 template <typename Model>
41 struct CostModelSumVisitor
42 : public bp::def_visitor<CostModelSumVisitor<Model>> {
43
1/2
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
150 BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(CostModelSum_addCost_wrap,
44 Model::addCost, 3, 4)
45 typedef typename Model::CostDataSum Data;
46 typedef typename Model::StateAbstract State;
47 typedef typename Model::VectorXs VectorXs;
48 template <class PyClass>
49 40 void visit(PyClass& cl) const {
50
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
40 cl.def(bp::init<std::shared_ptr<State>, std::size_t>(
51 bp::args("self", "state", "nu"),
52 "Initialize the total cost model.\n\n"
53 "For this case the default nu is equals to model.nv.\n"
54 ":param state: state description\n"
55 ":param nu: dimension of control vector"))
56
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .def(bp::init<std::shared_ptr<State>>(
57 bp::args("self", "state"),
58 "Initialize the total cost model.\n\n"
59 "For this case the default nu is equals to model.nv.\n"
60 ":param state: state description"))
61 80 .def("addCost", &Model::addCost,
62
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 CostModelSum_addCost_wrap(
63 bp::args("self", "name", "cost", "weight", "active"),
64 "Add a cost item.\n\n"
65 ":param name: cost name\n"
66 ":param cost: cost model\n"
67 ":param weight: cost weight\n"
68 ":param active: True if the cost is activated (default true)"))
69
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def("removeCost", &Model::removeCost, bp::args("self", "name"),
70 "Remove a cost item.\n\n"
71 ":param name: cost name")
72
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def("changeCostStatus", &Model::changeCostStatus,
73 bp::args("self", "name", "active"),
74 "Change the cost status.\n\n"
75 ":param name: cost name\n"
76 ":param active: cost status (true for active and false for "
77 "inactive)")
78
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def(
79 "calc",
80 static_cast<void (Model::*)(
81 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
82 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
83 bp::args("self", "data", "x", "u"),
84 "Compute the total cost.\n\n"
85 ":param data: cost-sum data\n"
86 ":param x: state point (dim. state.nx)\n"
87 ":param u: control input (dim. nu)")
88
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def(
89 "calc",
90 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
91 const Eigen::Ref<const VectorXs>&)>(
92 &Model::calc),
93 bp::args("self", "data", "x"),
94 "Compute the total cost value for nodes that depends only on the "
95 "state.\n\n"
96 "It updates the total cost based on the state only.\n"
97 "This function is used in the terminal nodes of an optimal control "
98 "problem.\n"
99 ":param data: cost-sum data\n"
100 ":param x: state point (dim. state.nx)")
101
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def(
102 "calcDiff",
103 static_cast<void (Model::*)(
104 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
105 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
106 bp::args("self", "data", "x", "u"),
107 "Compute the derivatives of the total cost.\n\n"
108 "It assumes that calc has been run first.\n"
109 ":param data: action data\n"
110 ":param x: state point (dim. state.nx)\n"
111 ":param u: control input (dim. nu)")
112
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def(
113 "calcDiff",
114 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
115 const Eigen::Ref<const VectorXs>&)>(
116 &Model::calcDiff),
117 bp::args("self", "data", "x"),
118 "Compute the Jacobian and Hessian of the total cost for nodes that "
119 "depends on the state only.\n\n"
120 "It updates the Jacobian and Hessian of the total cost based on "
121 "the "
122 "state only.\n"
123 "This function is used in the terminal nodes of an optimal control "
124 "problem.\n"
125 ":param data: cost-sum data\n"
126 ":param x: state point (dim. state.nx)")
127
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def("createData", &Model::createData,
128 bp::with_custodian_and_ward_postcall<0, 2>(),
129 bp::args("self", "data"),
130 "Create the total cost data.\n\n"
131 ":param data: shared data\n"
132 ":return total cost data.")
133
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
40 .add_property(
134 "state",
135 bp::make_function(&Model::get_state,
136 40 bp::return_value_policy<bp::return_by_value>()),
137 "state description")
138
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
139 "costs",
140 bp::make_function(&Model::get_costs,
141 40 bp::return_value_policy<bp::return_by_value>()),
142 "stack of costs")
143
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .add_property("nu", bp::make_function(&Model::get_nu),
144 "dimension of control vector")
145
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .add_property("nr", bp::make_function(&Model::get_nr),
146 "dimension of the residual vector of active cost")
147
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .add_property("nr_total", bp::make_function(&Model::get_nr_total),
148 "dimension of the total residual vector")
149
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
150 "active",
151 bp::make_function(
152 &Model::get_active,
153
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
80 deprecated<bp::return_value_policy<bp::return_by_value>>(
154 "Deprecated. Use property active_set")),
155 "list of names of active contact items")
156
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
157 "inactive",
158 bp::make_function(
159 &Model::get_inactive,
160
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
80 deprecated<bp::return_value_policy<bp::return_by_value>>(
161 "Deprecated. Use property inactive_set")),
162 "list of names of inactive contact items")
163
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
164 "active_set",
165 bp::make_function(&Model::get_active_set,
166 40 bp::return_value_policy<bp::return_by_value>()),
167 "name of the active set of cost items")
168
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
169 "inactive_set",
170 bp::make_function(&Model::get_inactive_set,
171 40 bp::return_value_policy<bp::return_by_value>()),
172 "name of the inactive set of cost items")
173
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .def("getCostStatus", &Model::getCostStatus, bp::args("self", "name"),
174 "Return the cost status of a given cost name.\n\n"
175 ":param name: cost name");
176 40 }
177 };
178
179 template <typename Data>
180 struct CostDataSumVisitor : public bp::def_visitor<CostDataSumVisitor<Data>> {
181 typedef typename Data::Scalar Scalar;
182 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionData;
183 typedef ActionDataAbstractTpl<Scalar> ActionData;
184 template <class PyClass>
185 40 void visit(PyClass& cl) const {
186 40 cl.def(
187 "shareMemory", &Data::template shareMemory<DifferentialActionData>,
188 bp::args("self", "model"),
189 "Share memory with a given differential action data\n\n"
190 ":param model: differential action data that we want to share memory")
191
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
80 .def("shareMemory", &Data::template shareMemory<ActionData>,
192 bp::args("self", "model"),
193 "Share memory with a given action data\n\n"
194 ":param model: action data that we want to share memory")
195
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
40 .add_property(
196 "costs",
197 bp::make_getter(&Data::costs,
198 40 bp::return_value_policy<bp::return_by_value>()),
199 "stack of costs data")
200
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
40 .add_property(
201 "shared",
202 40 bp::make_getter(&Data::shared, bp::return_internal_reference<>()),
203 "shared data")
204
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .add_property(
205 "cost",
206 bp::make_getter(&Data::cost,
207 40 bp::return_value_policy<bp::return_by_value>()),
208 bp::make_setter(&Data::cost), "cost value")
209
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .add_property(
210 "Lx",
211 bp::make_function(&Data::get_Lx,
212 40 bp::return_value_policy<bp::return_by_value>()),
213 bp::make_function(&Data::set_Lx), "Jacobian of the cost")
214
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .add_property(
215 "Lu",
216 bp::make_function(&Data::get_Lu,
217 40 bp::return_value_policy<bp::return_by_value>()),
218 bp::make_function(&Data::set_Lu), "Jacobian of the cost")
219
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .add_property(
220 "Lxx",
221 bp::make_function(&Data::get_Lxx,
222 40 bp::return_value_policy<bp::return_by_value>()),
223 bp::make_function(&Data::set_Lxx), "Hessian of the cost")
224
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .add_property(
225 "Lxu",
226 bp::make_function(&Data::get_Lxu,
227 40 bp::return_value_policy<bp::return_by_value>()),
228 bp::make_function(&Data::set_Lxu), "Hessian of the cost")
229
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
80 .add_property(
230 "Luu",
231 bp::make_function(&Data::get_Luu,
232 40 bp::return_value_policy<bp::return_by_value>()),
233 bp::make_function(&Data::set_Luu), "Hessian of the cost");
234 40 }
235 };
236
237 #define CROCODDYL_COST_ITEM_PYTHON_BINDINGS(Scalar) \
238 typedef CostItemTpl<Scalar> Model; \
239 typedef Model::CostModelAbstract CostModel; \
240 typedef std::shared_ptr<Model> CostItemPtr; \
241 StdMapPythonVisitor< \
242 std::string, CostItemPtr, std::less<std::string>, \
243 std::allocator<std::pair<const std::string, CostItemPtr>>, \
244 true>::expose("StdMap_CostItem"); \
245 typedef CostDataAbstractTpl<Scalar> CostData; \
246 typedef std::shared_ptr<CostData> CostDataPtr; \
247 StdMapPythonVisitor< \
248 std::string, CostDataPtr, std::less<std::string>, \
249 std::allocator<std::pair<const std::string, CostDataPtr>>, \
250 true>::expose("StdMap_CostData"); \
251 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
252 bp::class_<Model>( \
253 "CostItem", "Describe a cost item.\n\n", \
254 bp::init<std::string, std::shared_ptr<CostModel>, Scalar, \
255 bp::optional<bool>>( \
256 bp::args("self", "name", "cost", "weight", "active"), \
257 "Initialize the cost item.\n\n" \
258 ":param name: cost name\n" \
259 ":param cost: cost model\n" \
260 ":param weight: cost weight\n" \
261 ":param active: True if the cost is activated (default true)")) \
262 .def(CostItemVisitor<Model>()) \
263 .def(CastVisitor<Model>()) \
264 .def(PrintableVisitor<Model>()) \
265 .def(CopyableVisitor<Model>());
266
267 #define CROCODDYL_COST_MODEL_SUM_PYTHON_BINDINGS(Scalar) \
268 typedef CostModelSumTpl<Scalar> Model; \
269 typedef Model::StateAbstract State; \
270 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
271 bp::class_<Model>("CostModelSum", \
272 bp::init<std::shared_ptr<State>, std::size_t>( \
273 bp::args("self", "state", "nu"), \
274 "Initialize the total cost model.\n\n" \
275 ":param state: state description\n" \
276 ":param nu: dimension of control vector")) \
277 .def(CostModelSumVisitor<Model>()) \
278 .def(CastVisitor<Model>()) \
279 .def(PrintableVisitor<Model>()) \
280 .def(CopyableVisitor<Model>());
281
282 #define CROCODDYL_COST_DATA_SUM_PYTHON_BINDINGS(Scalar) \
283 typedef CostDataSumTpl<Scalar> Data; \
284 typedef CostModelSumTpl<Scalar> Model; \
285 typedef Model::DataCollectorAbstract DataCollector; \
286 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
287 bp::class_<Data>( \
288 "CostDataSum", "Class for total cost data.\n\n", \
289 bp::init<Model*, DataCollector*>( \
290 bp::args("self", "model", "data"), \
291 "Create total cost data.\n\n" \
292 ":param model: total cost model\n" \
293 ":param data: shared data")[bp::with_custodian_and_ward<1, 3>()]) \
294 .def(CostDataSumVisitor<Data>()) \
295 .def(CopyableVisitor<Data>());
296
297 10 void exposeCostSum() {
298
29/58
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 33 taken 10 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 10 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 10 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 10 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 10 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 10 times.
✗ Branch 49 not taken.
✓ Branch 54 taken 10 times.
✗ Branch 55 not taken.
✓ Branch 57 taken 10 times.
✗ Branch 58 not taken.
✓ Branch 60 taken 10 times.
✗ Branch 61 not taken.
✓ Branch 66 taken 10 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 10 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 10 times.
✗ Branch 74 not taken.
✓ Branch 81 taken 10 times.
✗ Branch 82 not taken.
✓ Branch 85 taken 10 times.
✗ Branch 86 not taken.
✓ Branch 88 taken 10 times.
✗ Branch 89 not taken.
✓ Branch 95 taken 10 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 10 times.
✗ Branch 99 not taken.
✓ Branch 101 taken 10 times.
✗ Branch 102 not taken.
✓ Branch 104 taken 10 times.
✗ Branch 105 not taken.
✓ Branch 107 taken 10 times.
✗ Branch 108 not taken.
✓ Branch 110 taken 10 times.
✗ Branch 111 not taken.
✓ Branch 113 taken 10 times.
✗ Branch 114 not taken.
✓ Branch 116 taken 10 times.
✗ Branch 117 not taken.
20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_COST_ITEM_PYTHON_BINDINGS)
299
17/34
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 10 times.
✗ Branch 31 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 10 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 10 times.
✗ Branch 51 not taken.
✓ Branch 53 taken 10 times.
✗ Branch 54 not taken.
✓ Branch 56 taken 10 times.
✗ Branch 57 not taken.
20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_COST_MODEL_SUM_PYTHON_BINDINGS)
300
15/30
✓ Branch 3 taken 10 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 10 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 10 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 10 times.
✗ Branch 28 not taken.
✓ Branch 32 taken 10 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 10 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 10 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 10 times.
✗ Branch 42 not taken.
✓ Branch 44 taken 10 times.
✗ Branch 45 not taken.
✓ Branch 47 taken 10 times.
✗ Branch 48 not taken.
✓ Branch 50 taken 10 times.
✗ Branch 51 not taken.
20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_COST_DATA_SUM_PYTHON_BINDINGS)
301 10 }
302
303 } // namespace python
304 } // namespace crocoddyl
305