GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actions/lqr.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 190 242 78.5%
Branches: 209 906 23.1%
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 namespace crocoddyl {
11
12 template <typename Scalar>
13 ✗ ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const MatrixXs& A,
14 const MatrixXs& B,
15 const MatrixXs& Q,
16 const MatrixXs& R,
17 const MatrixXs& N)
18 ✗ : Base(std::make_shared<StateVector>(A.cols()), B.cols(), 0),
19 ✗ drift_free_(true),
20 ✗ updated_lqr_(false) {
21 ✗ const std::size_t nx = state_->get_nx();
22 ✗ MatrixXs G = MatrixXs::Zero(ng_, nx + nu_);
23 ✗ MatrixXs H = MatrixXs::Zero(nh_, nx + nu_);
24 ✗ VectorXs f = VectorXs::Zero(nx);
25 ✗ VectorXs q = VectorXs::Zero(nx);
26 ✗ VectorXs r = VectorXs::Zero(nu_);
27 ✗ VectorXs g = VectorXs::Zero(ng_);
28 ✗ VectorXs h = VectorXs::Zero(nh_);
29 ✗ set_LQR(A, B, Q, R, N, G, H, f, q, r, g, h);
30 }
31
32 template <typename Scalar>
33 ✗ ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(
34 const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q, const MatrixXs& R,
35 const MatrixXs& N, const VectorXs& f, const VectorXs& q, const VectorXs& r)
36 ✗ : Base(std::make_shared<StateVector>(A.cols()), B.cols(), 0),
37 ✗ drift_free_(false),
38 ✗ updated_lqr_(false) {
39 ✗ const std::size_t nx = state_->get_nx();
40 ✗ MatrixXs G = MatrixXs::Zero(ng_, nx + nu_);
41 ✗ MatrixXs H = MatrixXs::Zero(ng_, nx + nu_);
42 ✗ VectorXs g = VectorXs::Zero(ng_);
43 ✗ VectorXs h = VectorXs::Zero(nh_);
44 ✗ set_LQR(A, B, Q, R, N, G, H, f, q, r, g, h);
45 }
46
47 template <typename Scalar>
48 57 ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(
49 const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q, const MatrixXs& R,
50 const MatrixXs& N, const MatrixXs& G, const MatrixXs& H, const VectorXs& f,
51 const VectorXs& q, const VectorXs& r, const VectorXs& g, const VectorXs& h)
52 ✗ : Base(std::make_shared<StateVector>(A.cols()), B.cols(), 0, G.rows(),
53 171 H.rows(), G.rows(), H.rows()),
54 57 drift_free_(false),
55
15/30
✓ Branch 7 taken 57 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 57 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 57 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 57 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 57 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 57 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 57 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 57 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 57 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 57 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 57 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 57 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 57 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 57 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 57 times.
✗ Branch 53 not taken.
 57 updated_lqr_(false) {
56
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 set_LQR(A, B, Q, R, N, G, H, f, q, r, g, h);
57 57 }
58
59 template <typename Scalar>
60 94 ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const std::size_t nx,
61 const std::size_t nu,
62 const bool drift_free)
63 : Base(std::make_shared<StateVector>(nx), nu, 0),
64
1/2
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
 94 A_(MatrixXs::Identity(nx, nx)),
65
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 B_(MatrixXs::Identity(nx, nu)),
66
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 Q_(MatrixXs::Identity(nx, nx)),
67
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 R_(MatrixXs::Identity(nu, nu)),
68
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 N_(MatrixXs::Zero(nx, nu)),
69
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 G_(MatrixXs::Zero(0, nx + nu)),
70
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 H_(MatrixXs::Zero(0, nx + nu)),
71
5/8
✓ Branch 0 taken 38 times.
✓ Branch 1 taken 56 times.
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 94 times.
✗ Branch 10 not taken.
 94 f_(drift_free ? VectorXs::Zero(nx) : VectorXs::Ones(nx)),
72
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 q_(VectorXs::Ones(nx)),
73
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 r_(VectorXs::Ones(nu)),
74
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 g_(VectorXs::Zero(0)),
75
2/4
✓ Branch 1 taken 94 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 94 times.
✗ Branch 5 not taken.
 94 h_(VectorXs::Zero(0)),
76
3/6
✓ Branch 3 taken 94 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 94 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 94 times.
✗ Branch 12 not taken.
 188 drift_free_(drift_free) {}
77
78 template <typename Scalar>
79 59 ActionModelLQRTpl<Scalar>::ActionModelLQRTpl(const ActionModelLQRTpl& copy)
80 59 : Base(std::make_shared<StateVector>(copy.get_A().cols()),
81 177 copy.get_B().cols(), 0, copy.get_G().rows(), copy.get_H().rows(),
82 118 copy.get_G().rows(), copy.get_H().rows()),
83 59 drift_free_(false),
84
15/30
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 59 times.
✗ Branch 12 not taken.
✓ Branch 16 taken 59 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 59 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 59 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 59 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 59 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 59 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 59 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 59 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 59 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 59 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 59 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 59 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 59 times.
✗ Branch 53 not taken.
 413 updated_lqr_(false) {
85
5/10
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 59 times.
✗ Branch 14 not taken.
 59 set_LQR(copy.get_A(), copy.get_B(), copy.get_Q(), copy.get_R(), copy.get_N(),
86
5/10
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 59 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 59 times.
✗ Branch 14 not taken.
 59 copy.get_G(), copy.get_H(), copy.get_f(), copy.get_q(), copy.get_r(),
87
3/6
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 59 times.
✗ Branch 8 not taken.
 59 copy.get_g(), copy.get_h());
88 59 }
89
90 template <typename Scalar>
91 2856 void ActionModelLQRTpl<Scalar>::calc(
92 const std::shared_ptr<ActionDataAbstract>& data,
93 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
94
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 2856 times.
 2856 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
95 ✗ throw_pretty(
96 "Invalid argument: " << "x has wrong dimension (it should be " +
97 std::to_string(state_->get_nx()) + ")");
98 }
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2856 times.
 2856 if (static_cast<std::size_t>(u.size()) != nu_) {
100 ✗ throw_pretty(
101 "Invalid argument: " << "u has wrong dimension (it should be " +
102 std::to_string(nu_) + ")");
103 }
104 2856 Data* d = static_cast<Data*>(data.get());
105
106
2/4
✓ Branch 3 taken 2856 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
 2856 data->xnext.noalias() = A_ * x;
107
2/4
✓ Branch 3 taken 2856 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
 2856 data->xnext.noalias() += B_ * u;
108 2856 data->xnext += f_;
109
110 // cost = 0.5 * x^T * Q * x + 0.5 * u^T * R * u + x^T * N * u + q^T * x + r^T
111 // * u
112
2/4
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2856 times.
✗ Branch 6 not taken.
 2856 d->Q_x_tmp.noalias() = Q_ * x;
113 2856 data->cost = Scalar(0.5) * x.dot(d->Q_x_tmp);
114
2/4
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2856 times.
✗ Branch 6 not taken.
 2856 d->R_u_tmp.noalias() = R_ * u;
115 2856 data->cost += Scalar(0.5) * u.dot(d->R_u_tmp);
116
2/4
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2856 times.
✗ Branch 6 not taken.
 2856 d->Q_x_tmp.noalias() = N_ * u;
117 2856 data->cost += x.dot(d->Q_x_tmp);
118 2856 data->cost += q_.dot(x);
119 2856 data->cost += r_.dot(u);
120
121 // constraints
122 2856 const std::size_t nx = state_->get_nx();
123
3/6
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2856 times.
✗ Branch 10 not taken.
 2856 data->g.noalias() = G_.leftCols(nx) * x;
124
3/6
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2856 times.
✗ Branch 10 not taken.
 2856 data->g.noalias() += G_.rightCols(nu_) * u;
125 2856 data->g += g_;
126
3/6
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2856 times.
✗ Branch 10 not taken.
 2856 data->h.noalias() = H_.leftCols(nx) * x;
127
3/6
✓ Branch 2 taken 2856 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2856 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2856 times.
✗ Branch 10 not taken.
 2856 data->h.noalias() += H_.rightCols(nu_) * u;
128 2856 data->h += h_;
129 2856 }
130
131 template <typename Scalar>
132 232 void ActionModelLQRTpl<Scalar>::calc(
133 const std::shared_ptr<ActionDataAbstract>& data,
134 const Eigen::Ref<const VectorXs>& x) {
135
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 232 times.
 232 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
136 ✗ throw_pretty(
137 "Invalid argument: " << "x has wrong dimension (it should be " +
138 std::to_string(state_->get_nx()) + ")");
139 }
140 232 Data* d = static_cast<Data*>(data.get());
141
142 232 d->xnext = x;
143
144 // cost = 0.5 * x^T * Q * x + q^T * x
145
2/4
✓ Branch 2 taken 232 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 232 times.
✗ Branch 6 not taken.
 232 d->Q_x_tmp.noalias() = Q_ * x;
146 232 data->cost = Scalar(0.5) * x.dot(d->Q_x_tmp);
147 232 data->cost += q_.dot(x);
148
149 // constraints
150 232 const std::size_t nx = state_->get_nx();
151
3/6
✓ Branch 2 taken 232 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 232 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 232 times.
✗ Branch 10 not taken.
 232 data->g.noalias() = G_.leftCols(nx) * x;
152 232 data->g += g_;
153
3/6
✓ Branch 2 taken 232 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 232 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 232 times.
✗ Branch 10 not taken.
 232 data->h.noalias() = H_.leftCols(nx) * x;
154 232 data->h += h_;
155 232 }
156
157 template <typename Scalar>
158 1846 void ActionModelLQRTpl<Scalar>::calcDiff(
159 const std::shared_ptr<ActionDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
161
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1846 times.
 1846 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
162 ✗ throw_pretty(
163 "Invalid argument: " << "x has wrong dimension (it should be " +
164 std::to_string(state_->get_nx()) + ")");
165 }
166
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1846 times.
 1846 if (static_cast<std::size_t>(u.size()) != nu_) {
167 ✗ throw_pretty(
168 "Invalid argument: " << "u has wrong dimension (it should be " +
169 std::to_string(nu_) + ")");
170 }
171
172 1846 const std::size_t nx = state_->get_nx();
173
2/2
✓ Branch 0 taken 37 times.
✓ Branch 1 taken 1809 times.
 1846 if (!updated_lqr_) {
174 37 data->Fx = A_;
175 37 data->Fu = B_;
176 37 data->Lxx = Q_;
177 37 data->Luu = R_;
178 37 data->Lxu = N_;
179
1/2
✓ Branch 3 taken 37 times.
✗ Branch 4 not taken.
 37 data->Gx = G_.leftCols(nx);
180
1/2
✓ Branch 3 taken 37 times.
✗ Branch 4 not taken.
 37 data->Gu = G_.rightCols(nu_);
181
1/2
✓ Branch 3 taken 37 times.
✗ Branch 4 not taken.
 37 data->Hx = H_.leftCols(nx);
182
1/2
✓ Branch 3 taken 37 times.
✗ Branch 4 not taken.
 37 data->Hu = H_.rightCols(nu_);
183 37 updated_lqr_ = true;
184 }
185 1846 data->Lx = q_;
186
2/4
✓ Branch 3 taken 1846 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1846 times.
✗ Branch 7 not taken.
 1846 data->Lx.noalias() += Q_ * x;
187
2/4
✓ Branch 3 taken 1846 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1846 times.
✗ Branch 7 not taken.
 1846 data->Lx.noalias() += N_ * u;
188 1846 data->Lu = r_;
189
3/6
✓ Branch 2 taken 1846 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1846 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1846 times.
✗ Branch 10 not taken.
 1846 data->Lu.noalias() += N_.transpose() * x;
190
2/4
✓ Branch 3 taken 1846 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1846 times.
✗ Branch 7 not taken.
 1846 data->Lu.noalias() += R_ * u;
191 1846 }
192
193 template <typename Scalar>
194 110 void ActionModelLQRTpl<Scalar>::calcDiff(
195 const std::shared_ptr<ActionDataAbstract>& data,
196 const Eigen::Ref<const VectorXs>& x) {
197
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 110 times.
 110 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
198 ✗ throw_pretty(
199 "Invalid argument: " << "x has wrong dimension (it should be " +
200 std::to_string(state_->get_nx()) + ")");
201 }
202
203 110 const std::size_t nx = state_->get_nx();
204
2/2
✓ Branch 0 taken 13 times.
✓ Branch 1 taken 97 times.
 110 if (!updated_lqr_) {
205 13 data->Lxx = Q_;
206
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 data->Gx = G_.leftCols(nx);
207
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 data->Hx = H_.leftCols(nx);
208 13 updated_lqr_ = true;
209 }
210 110 data->Lx = q_;
211
2/4
✓ Branch 3 taken 110 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 110 times.
✗ Branch 7 not taken.
 110 data->Lx.noalias() += Q_ * x;
212 110 }
213
214 template <typename Scalar>
215 std::shared_ptr<ActionDataAbstractTpl<Scalar>>
216 1617 ActionModelLQRTpl<Scalar>::createData() {
217
1/2
✓ Branch 2 taken 1617 times.
✗ Branch 3 not taken.
 1617 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
218 }
219
220 template <typename Scalar>
221 template <typename NewScalar>
222 ✗ ActionModelLQRTpl<NewScalar> ActionModelLQRTpl<Scalar>::cast() const {
223 typedef ActionModelLQRTpl<NewScalar> ReturnType;
224 ✗ ReturnType ret(A_.template cast<NewScalar>(), B_.template cast<NewScalar>(),
225 ✗ Q_.template cast<NewScalar>(), R_.template cast<NewScalar>(),
226 ✗ N_.template cast<NewScalar>(), G_.template cast<NewScalar>(),
227 ✗ H_.template cast<NewScalar>(), f_.template cast<NewScalar>(),
228 ✗ q_.template cast<NewScalar>(), r_.template cast<NewScalar>(),
229 ✗ g_.template cast<NewScalar>(), h_.template cast<NewScalar>());
230 ✗ return ret;
231 }
232
233 template <typename Scalar>
234 256 bool ActionModelLQRTpl<Scalar>::checkData(
235 const std::shared_ptr<ActionDataAbstract>& data) {
236 256 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
237
1/2
✓ Branch 1 taken 256 times.
✗ Branch 2 not taken.
 256 if (d != NULL) {
238 256 return true;
239 } else {
240 ✗ return false;
241 }
242 256 }
243
244 template <typename Scalar>
245 57 ActionModelLQRTpl<Scalar> ActionModelLQRTpl<Scalar>::Random(
246 const std::size_t nx, const std::size_t nu, const std::size_t ng,
247 const std::size_t nh) {
248
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 MatrixXs A = MatrixXs::Random(nx, nx);
249
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 MatrixXs B = MatrixXs::Random(nx, nu);
250
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 MatrixXs L_tmp = MatrixXs::Random(nx + nu, nx + nu);
251
3/6
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 57 times.
✗ Branch 8 not taken.
 57 MatrixXs L = L_tmp.transpose() * L_tmp;
252
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 const Eigen::Block<MatrixXs> Q = L.topLeftCorner(nx, nx);
253
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 const Eigen::Block<MatrixXs> R = L.bottomRightCorner(nu, nu);
254
1/2
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
 57 const Eigen::Block<MatrixXs> N = L.topRightCorner(nx, nu);
255
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 MatrixXs G = MatrixXs::Random(ng, nx + nu);
256
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 MatrixXs H = MatrixXs::Random(nh, nx + nu);
257
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 VectorXs f = VectorXs::Random(nx);
258
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 VectorXs q = VectorXs::Random(nx);
259
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 VectorXs r = VectorXs::Random(nu);
260
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 VectorXs g = VectorXs::Random(ng);
261
2/4
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
 57 VectorXs h = VectorXs::Random(nh);
262
4/8
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 57 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 57 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 57 times.
✗ Branch 11 not taken.
 114 return ActionModelLQRTpl<Scalar>(A, B, Q, R, N, G, H, f, q, r, g, h);
263 57 }
264
265 template <typename Scalar>
266 88 void ActionModelLQRTpl<Scalar>::print(std::ostream& os) const {
267 88 os << "ActionModelLQR {nx=" << state_->get_nx() << ", nu=" << nu_
268 88 << ", ng=" << ng_ << ", nh=" << nh_ << ", drift_free=" << drift_free_
269 88 << "}";
270 88 }
271
272 template <typename Scalar>
273 1736 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_A()
274 const {
275 1736 return A_;
276 }
277
278 template <typename Scalar>
279 1736 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_B()
280 const {
281 1736 return B_;
282 }
283
284 template <typename Scalar>
285 60 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_f()
286 const {
287 60 return f_;
288 }
289
290 template <typename Scalar>
291 1677 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_Q()
292 const {
293 1677 return Q_;
294 }
295
296 template <typename Scalar>
297 1677 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_R()
298 const {
299 1677 return R_;
300 }
301
302 template <typename Scalar>
303 1677 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_N()
304 const {
305 1677 return N_;
306 }
307
308 template <typename Scalar>
309 3411 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_G()
310 const {
311 3411 return G_;
312 }
313
314 template <typename Scalar>
315 3411 const typename MathBaseTpl<Scalar>::MatrixXs& ActionModelLQRTpl<Scalar>::get_H()
316 const {
317 3411 return H_;
318 }
319
320 template <typename Scalar>
321 60 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_q()
322 const {
323 60 return q_;
324 }
325
326 template <typename Scalar>
327 60 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_r()
328 const {
329 60 return r_;
330 }
331
332 template <typename Scalar>
333 59 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_g()
334 const {
335 59 return g_;
336 }
337
338 template <typename Scalar>
339 59 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelLQRTpl<Scalar>::get_h()
340 const {
341 59 return h_;
342 }
343
344 template <typename Scalar>
345 116 void ActionModelLQRTpl<Scalar>::set_LQR(const MatrixXs& A, const MatrixXs& B,
346 const MatrixXs& Q, const MatrixXs& R,
347 const MatrixXs& N, const MatrixXs& G,
348 const MatrixXs& H, const VectorXs& f,
349 const VectorXs& q, const VectorXs& r,
350 const VectorXs& g, const VectorXs& h) {
351
1/2
✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
 116 const std::size_t nx = state_->get_nx();
352
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(A.rows()) != nx) {
353 ✗ throw_pretty(
354 "Invalid argument: " << "A should be a squared matrix with size " +
355 std::to_string(nx));
356 }
357
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(B.rows()) != nx) {
358 ✗ throw_pretty(
359 "Invalid argument: " << "B has wrong dimension (it should have " +
360 std::to_string(nx) + " rows)");
361 }
362
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 116 times.
 232 if (static_cast<std::size_t>(Q.rows()) != nx ||
363
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 static_cast<std::size_t>(Q.cols()) != nx) {
364 ✗ throw_pretty("Invalid argument: "
365 << "Q has wrong dimension (it should be " +
366 std::to_string(nx) + " x " + std::to_string(nx) + ")");
367 }
368
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 116 times.
 232 if (static_cast<std::size_t>(R.rows()) != nu_ ||
369
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 static_cast<std::size_t>(R.cols()) != nu_) {
370 ✗ throw_pretty(
371 "Invalid argument: " << "R has wrong dimension (it should be " +
372 std::to_string(nu_) + " x " +
373 std::to_string(nu_) + ")");
374 }
375
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 116 times.
 232 if (static_cast<std::size_t>(N.rows()) != nx ||
376
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 static_cast<std::size_t>(N.cols()) != nu_) {
377 ✗ throw_pretty("Invalid argument: "
378 << "N has wrong dimension (it should be " +
379 std::to_string(nx) + " x " + std::to_string(nu_) + ")");
380 }
381
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 116 times.
 232 if (static_cast<std::size_t>(G.rows()) != ng_ ||
382
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 static_cast<std::size_t>(G.cols()) != nx + nu_) {
383 ✗ throw_pretty(
384 "Invalid argument: " << "G has wrong dimension (it should be " +
385 std::to_string(ng_) + " x " +
386 std::to_string(nx + nu_) + ")");
387 }
388
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 116 times.
 232 if (static_cast<std::size_t>(H.rows()) != nh_ ||
389
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 static_cast<std::size_t>(H.cols()) != nx + nu_) {
390 ✗ throw_pretty(
391 "Invalid argument: " << "H has wrong dimension (it should be " +
392 std::to_string(nh_) + " x " +
393 std::to_string(nx + nu_) + ")");
394 }
395
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(f.size()) != nx) {
396 ✗ throw_pretty(
397 "Invalid argument: " << "f has wrong dimension (it should be " +
398 std::to_string(nx) + ")");
399 }
400
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(q.size()) != nx) {
401 ✗ throw_pretty(
402 "Invalid argument: " << "q has wrong dimension (it should be " +
403 std::to_string(nx) + ")");
404 }
405
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(r.size()) != nu_) {
406 ✗ throw_pretty(
407 "Invalid argument: " << "r has wrong dimension (it should be " +
408 std::to_string(nu_) + ")");
409 }
410
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(g.size()) != ng_) {
411 ✗ throw_pretty(
412 "Invalid argument: " << "g has wrong dimension (it should be " +
413 std::to_string(ng_) + ")");
414 }
415
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 116 times.
 116 if (static_cast<std::size_t>(h.size()) != nh_) {
416 ✗ throw_pretty(
417 "Invalid argument: " << "h has wrong dimension (it should be " +
418 std::to_string(nh_) + ")");
419 }
420
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 L_ = MatrixXs::Zero(nx + nu_, nx + nu_);
421
5/10
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 116 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 116 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 116 times.
✗ Branch 14 not taken.
 116 L_ << Q, N, N.transpose(), R;
422
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 Eigen::SelfAdjointEigenSolver<MatrixXs> eig(L_);
423
3/6
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 116 times.
 232 if (eig.info() != Eigen::Success ||
424
1/2
✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
 116 eig.eigenvalues().minCoeff() < ScaleNumerics<Scalar>(-1e-9)) {
425 ✗ throw_pretty("Invalid argument "
426 << "[Q, N; N.T, R] is not positive semi-definite");
427 }
428
429
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 A_ = A;
430
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 B_ = B;
431
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 f_ = f;
432
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 Q_ = Q;
433
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 R_ = R;
434
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 N_ = N;
435
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 G_ = G;
436
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 H_ = H;
437
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 q_ = q;
438
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 r_ = r;
439
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 g_ = g;
440
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 h_ = h;
441 116 updated_lqr_ = false;
442 116 }
443
444 } // namespace crocoddyl
445