GCC Code Coverage Report

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