GCC Code Coverage Report

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