GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/core/actions/lqr.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 66 66 100.0%
Branches: 129 258 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/actions/lqr.hpp"
11
12 #include "python/crocoddyl/core/action-base.hpp"
13 #include "python/crocoddyl/core/core.hpp"
14 #include "python/crocoddyl/utils/deprecate.hpp"
15
16 namespace crocoddyl {
17 namespace python {
18
19 template <typename Model>
20 struct ActionModelLQRVisitor
21 : public bp::def_visitor<ActionModelLQRVisitor<Model>> {
22 typedef typename Model::ActionDataAbstract Data;
23 typedef typename Model::Base ModelBase;
24 typedef typename Model::MatrixXs MatrixXs;
25 typedef typename Model::VectorXs VectorXs;
26 42 BOOST_PYTHON_FUNCTION_OVERLOADS(ActionModelLQR_Random_wrap, Model::Random, 2,
27 4)
28 template <class PyClass>
29 40 void visit(PyClass& cl) const {
30
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<MatrixXs, MatrixXs, MatrixXs, MatrixXs, MatrixXs, VectorXs,
31 VectorXs, VectorXs>(
32 bp::args("self", "A", "B", "Q", "R", "N", "f", "q", "r"),
33 "Initialize the differential LQR action model.\n\n"
34 ":param A: state matrix\n"
35 ":param B: input matrix\n"
36 ":param Q: state weight matrix\n"
37 ":param R: input weight matrix\n"
38 ":param N: state-input weight matrix\n"
39 ":param f: dynamics drift\n"
40 ":param q: state weight vector\n"
41 ":param r: input weight vector"))
42
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<MatrixXs, MatrixXs, MatrixXs, MatrixXs, MatrixXs,
43 MatrixXs, MatrixXs, VectorXs, VectorXs, VectorXs,
44 VectorXs, VectorXs>(
45 bp::args("self", "A", "B", "Q", "R", "N", "G", "H", "f", "q", "r",
46 "g", "h"),
47 "Initialize the LQR action model.\n\n"
48 ":param A: state matrix\n"
49 ":param B: input matrix\n"
50 ":param Q: state weight matrix\n"
51 ":param R: input weight matrix\n"
52 ":param N: state-input weight matrix\n"
53 ":param G: state-input inequality constraint matrix\n"
54 ":param H: state-input equality constraint matrix\n"
55 ":param f: dynamics drift\n"
56 ":param q: state weight vector\n"
57 ":param r: input weight vector\n"
58 ":param g: state-input equality constraint bias\n"
59 ":param h: state-input inequality constraint bias"))
60
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::size_t, std::size_t, bp::optional<bool>>(
61 bp::args("self", "nx", "nu", "driftFree"),
62 "Initialize the LQR action model.\n\n"
63 ":param nx: dimension of the state vector\n"
64 ":param nu: dimension of the control vector\n"
65 ":param driftFree: enable/disable the bias term of the linear "
66 "dynamics (default True)"))
67
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def(
68 "calc",
69 static_cast<void (Model::*)(
70 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
71 const Eigen::Ref<const VectorXs>&)>(&Model::calc),
72 bp::args("self", "data", "x", "u"),
73 "Compute the next state and cost value.\n\n"
74 "It describes the time-discrete evolution of the LQR system. "
75 "Additionally it computes the cost value associated to this "
76 "discrete state and control pair.\n"
77 ":param data: action data\n"
78 ":param x: state point (dim. state.nx)\n"
79 ":param u: control input (dim. nu)")
80
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calc",
81 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
82 const Eigen::Ref<const VectorXs>&)>(
83 &Model::calc),
84 bp::args("self", "data", "x"))
85
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def(
86 "calcDiff",
87 static_cast<void (Model::*)(
88 const std::shared_ptr<Data>&, const Eigen::Ref<const VectorXs>&,
89 const Eigen::Ref<const VectorXs>&)>(&Model::calcDiff),
90 bp::args("self", "data", "x", "u"),
91 "Compute the derivatives of the LQR dynamics and cost "
92 "functions.\n\n"
93 "It computes the partial derivatives of the LQR system and the "
94 "cost function. It assumes that calc has been run first. This "
95 "function builds a quadratic approximation of the action model "
96 "(i.e. dynamical system and cost function).\n"
97 ":param data: action data\n"
98 ":param x: state point (dim. state.nx)\n"
99 ":param u: control input (dim. nu)")
100
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("calcDiff",
101 static_cast<void (Model::*)(const std::shared_ptr<Data>&,
102 const Eigen::Ref<const VectorXs>&)>(
103 &Model::calcDiff),
104 bp::args("self", "data", "x"))
105
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, bp::args("self"),
106 "Create the LQR action data.")
107
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 .def("Random", &Model::Random,
108
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 ActionModelLQR_Random_wrap(
109 bp::args("nx", "nu", "ng", "nh"),
110 "Create a random LQR model.\n\n"
111 ":param nx: state dimension\n"
112 ":param nu: control dimension\n"
113 ":param ng: inequality constraint dimension (default 0)\n"
114 ":param nh: equality constraint dimension (default 0)"))
115
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 .staticmethod("Random")
116
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 80 .def("setLQR", &Model::set_LQR,
117 bp::args("self", "A", "B", "Q", "R", "N", "G", "H", "f", "q", "r",
118 "g", "h"),
119 "Modify the LQR action model.\n\n"
120 ":param A: state matrix\n"
121 ":param B: input matrix\n"
122 ":param Q: state weight matrix\n"
123 ":param R: input weight matrix\n"
124 ":param N: state-input weight matrix\n"
125 ":param G: state-input inequality constraint matrix\n"
126 ":param H: state-input equality constraint matrix\n"
127 ":param f: dynamics drift\n"
128 ":param q: state weight vector\n"
129 ":param r: input weight vector\n"
130 ":param g: state-input inequality constraint bias\n"
131 ":param h: state-input equality constraint bias")
132
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(
133 "A",
134 40 bp::make_function(&Model::get_A, bp::return_internal_reference<>()),
135 "state matrix")
136
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
137 "B",
138 40 bp::make_function(&Model::get_B, bp::return_internal_reference<>()),
139 "input matrix")
140
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
141 "f",
142 40 bp::make_function(&Model::get_f, bp::return_internal_reference<>()),
143 "dynamics drift")
144
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
145 "Q",
146 40 bp::make_function(&Model::get_Q, bp::return_internal_reference<>()),
147 "state weight matrix")
148
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
149 "R",
150 40 bp::make_function(&Model::get_R, bp::return_internal_reference<>()),
151 "input weight matrix")
152
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
153 "N",
154 40 bp::make_function(&Model::get_N, bp::return_internal_reference<>()),
155 "state-input weight matrix")
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 "G",
158 40 bp::make_function(&Model::get_G, bp::return_internal_reference<>()),
159 "state-input inequality constraint matrix")
160
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
161 "H",
162 40 bp::make_function(&Model::get_H, bp::return_internal_reference<>()),
163 "state-input equality constraint matrix")
164
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
165 "q",
166 40 bp::make_function(&Model::get_q, bp::return_internal_reference<>()),
167 "state weight vector")
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 "r",
170 40 bp::make_function(&Model::get_r, bp::return_internal_reference<>()),
171 "input weight vector")
172
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
173 "g",
174 40 bp::make_function(&Model::get_g, bp::return_internal_reference<>()),
175 "state-input inequality constraint bias")
176
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
177 "h",
178 40 bp::make_function(&Model::get_h, bp::return_internal_reference<>()),
179 "state-input equality constraint bias")
180 // deprecated function
181
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 .add_property(
182 "Fx",
183 bp::make_function(&Model::get_A,
184
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 80 deprecated<bp::return_internal_reference<>>(
185 "Deprecated. Use set_LQR.")),
186 &Model::set_Fx, "state matrix")
187
3/6
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 20 times.
✗ Branch 9 not taken.
 120 .add_property(
188 "Fu",
189 bp::make_function(&Model::get_B,
190
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
191 "Deprecated. Use B.")),
192 bp::make_function(&Model::set_Fu,
193
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
194 "input matrix")
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.
 80 .add_property(
196 "f0",
197 bp::make_function(&Model::get_f,
198
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
199 "Deprecated. Use f.")),
200 bp::make_function(&Model::set_f0,
201
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
202 "dynamics drift")
203
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(
204 "lx",
205 bp::make_function(&Model::get_q,
206
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
207 "Deprecated. Use q.")),
208 bp::make_function(&Model::set_lx,
209
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
210 "state weight vector")
211
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(
212 "lu",
213 bp::make_function(&Model::get_r,
214
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
215 "Deprecated. Use r.")),
216 bp::make_function(&Model::set_lu,
217
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
218 "input weight vector")
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(&Model::get_Q,
222
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
223 "Deprecated. Use Q.")),
224 bp::make_function(&Model::set_Lxx,
225
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
226 "state weight matrix")
227
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(
228 "Lxu",
229 bp::make_function(&Model::get_N,
230
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
231 "Deprecated. Use N.")),
232 bp::make_function(&Model::set_Lxu,
233
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
234 "state-input weight matrix")
235
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(
236 "Luu",
237 bp::make_function(&Model::get_R,
238
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<bp::return_internal_reference<>>(
239 "Deprecated. Use R.")),
240 bp::make_function(&Model::set_Luu,
241
2/4
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 deprecated<>("Deprecated. Use set_LQR.")),
242 "input weight matrix");
243 40 }
244 };
245
246 #define CROCODDYL_ACTION_MODEL_LQR_PYTHON_BINDINGS(Scalar) \
247 typedef ActionModelLQRTpl<Scalar> Model; \
248 typedef ActionModelAbstractTpl<Scalar> ModelBase; \
249 typedef typename ModelBase::VectorXs MatrixXs; \
250 bp::register_ptr_to_python<std::shared_ptr<Model>>(); \
251 bp::class_<Model, bp::bases<ModelBase>>( \
252 "ActionModelLQR", \
253 "LQR action model.\n\n" \
254 "A linear-quadratic regulator (LQR) action has a transition model of " \
255 "the form\n" \
256 " xnext(x,u) = A x + B u + f.\n" \
257 "Its cost function is quadratic of the form:\n" \
258 " 1/2 [x,u].T [Q N; N.T R] [x,u] + [q,r].T [x,u],\n" \
259 "and the linear equality and inequality constraints has the form:\n" \
260 " g(x,u) = G [x,u] + g<=0\n" \
261 " h(x,u) = H [x,u] + h.", \
262 bp::init<MatrixXs, MatrixXs, MatrixXs, MatrixXs, MatrixXs>( \
263 bp::args("self", "A", "B", "Q", "R", "N"), \
264 "Initialize the LQR action model.\n\n" \
265 ":param A: state matrix\n" \
266 ":param B: input matrix\n" \
267 ":param Q: state weight matrix\n" \
268 ":param R: input weight matrix\n" \
269 ":param N: state-input weight matrix")) \
270 .def(ActionModelLQRVisitor<Model>()) \
271 .def(CastVisitor<Model>()) \
272 .def(PrintableVisitor<Model>()) \
273 .def(CopyableVisitor<Model>());
274
275 #define CROCODDYL_ACTION_DATA_LQR_PYTHON_BINDINGS(Scalar) \
276 typedef ActionDataLQRTpl<Scalar> Data; \
277 typedef ActionDataAbstractTpl<Scalar> DataBase; \
278 typedef ActionModelLQRTpl<Scalar> Model; \
279 bp::register_ptr_to_python<std::shared_ptr<Data>>(); \
280 bp::class_<Data, bp::bases<DataBase>>( \
281 "ActionDataLQR", "Action data for the LQR system.", \
282 bp::init<Model*>(bp::args("self", "model"), \
283 "Create LQR data.\n\n" \
284 ":param model: LQR action model")) \
285 .def(CopyableVisitor<Data>());
286
287 10 void exposeActionLQR() {
288 // TODO: Remove once the deprecated update call has been removed in a future
289 // release
290 #pragma GCC diagnostic push
291 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
292
293
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_ACTION_MODEL_LQR_PYTHON_BINDINGS)
294
11/22
✓ 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 15 taken 10 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 10 times.
✗ Branch 22 not taken.
✓ Branch 26 taken 10 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 10 times.
✗ Branch 30 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.
 20 CROCODDYL_PYTHON_SCALARS(CROCODDYL_ACTION_DATA_LQR_PYTHON_BINDINGS)
295
296 #pragma GCC diagnostic pop
297 10 }
298
299 } // namespace python
300 } // namespace crocoddyl
301