GCC Code Coverage Report

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