GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/integrator/rk.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 265 285 93.0%
Branches: 321 792 40.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, University of Trento,
5 // LAAS-CNRS, IRI: CSIC-UPC, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <iostream>
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 552 IntegratedActionModelRKTpl<Scalar>::IntegratedActionModelRKTpl(
16 std::shared_ptr<DifferentialActionModelAbstract> model,
17 std::shared_ptr<ControlParametrizationModelAbstract> control,
18 const RKType rktype, const Scalar time_step, const bool with_cost_residual)
19
1/2
✓ Branch 3 taken 552 times.
✗ Branch 4 not taken.
 552 : Base(model, control, time_step, with_cost_residual), rk_type_(rktype) {
20
1/2
✓ Branch 1 taken 552 times.
✗ Branch 2 not taken.
 552 set_rk_type(rktype);
21 552 }
22
23 template <typename Scalar>
24 280 IntegratedActionModelRKTpl<Scalar>::IntegratedActionModelRKTpl(
25 std::shared_ptr<DifferentialActionModelAbstract> model, const RKType rktype,
26 const Scalar time_step, const bool with_cost_residual)
27
1/2
✓ Branch 2 taken 280 times.
✗ Branch 3 not taken.
 280 : Base(model, time_step, with_cost_residual), rk_type_(rktype) {
28
1/2
✓ Branch 1 taken 280 times.
✗ Branch 2 not taken.
 280 set_rk_type(rktype);
29 280 }
30
31 template <typename Scalar>
32 25224 void IntegratedActionModelRKTpl<Scalar>::calc(
33 const std::shared_ptr<ActionDataAbstract>& data,
34 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
35
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 25224 times.
 25224 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
36 ✗ throw_pretty(
37 "Invalid argument: " << "x has wrong dimension (it should be " +
38 std::to_string(state_->get_nx()) + ")");
39 }
40
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 25224 times.
 25224 if (static_cast<std::size_t>(u.size()) != nu_) {
41 ✗ throw_pretty(
42 "Invalid argument: " << "u has wrong dimension (it should be " +
43 std::to_string(nu_) + ")");
44 }
45 25224 const std::size_t nv = state_->get_nv();
46 25224 Data* d = static_cast<Data*>(data.get());
47
48 const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
49 25224 d->differential[0];
50 const std::shared_ptr<ControlParametrizationDataAbstract>& u0_data =
51 25224 d->control[0];
52 25224 control_->calc(u0_data, rk_c_[0], u);
53 25224 d->ws[0] = u0_data->w;
54
1/2
✓ Branch 4 taken 25224 times.
✗ Branch 5 not taken.
 25224 differential_->calc(k0_data, x, d->ws[0]);
55 25224 d->y[0] = x;
56
2/4
✓ Branch 4 taken 25224 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25224 times.
✗ Branch 8 not taken.
 25224 d->ki[0].head(nv) = d->y[0].tail(nv);
57
1/2
✓ Branch 4 taken 25224 times.
✗ Branch 5 not taken.
 25224 d->ki[0].tail(nv) = k0_data->xout;
58 25224 d->integral[0] = k0_data->cost;
59
2/2
✓ Branch 0 taken 52737 times.
✓ Branch 1 taken 25224 times.
 77961 for (std::size_t i = 1; i < ni_; ++i) {
60 const std::shared_ptr<DifferentialActionDataAbstract>& ki_data =
61 52737 d->differential[i];
62 const std::shared_ptr<ControlParametrizationDataAbstract>& ui_data =
63 52737 d->control[i];
64
3/6
✓ Branch 3 taken 52737 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 52737 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 52737 times.
✗ Branch 11 not taken.
 52737 d->dx_rk[i].noalias() = time_step_ * rk_c_[i] * d->ki[i - 1];
65
2/4
✓ Branch 5 taken 52737 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 52737 times.
✗ Branch 9 not taken.
 52737 state_->integrate(x, d->dx_rk[i], d->y[i]);
66 52737 control_->calc(ui_data, rk_c_[i], u);
67 52737 d->ws[i] = ui_data->w;
68
2/4
✓ Branch 5 taken 52737 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 52737 times.
✗ Branch 9 not taken.
 52737 differential_->calc(ki_data, d->y[i], d->ws[i]);
69
2/4
✓ Branch 4 taken 52737 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 52737 times.
✗ Branch 8 not taken.
 52737 d->ki[i].head(nv) = d->y[i].tail(nv);
70
1/2
✓ Branch 4 taken 52737 times.
✗ Branch 5 not taken.
 52737 d->ki[i].tail(nv) = ki_data->xout;
71 52737 d->integral[i] = ki_data->cost;
72 }
73
74
2/2
✓ Branch 0 taken 6884 times.
✓ Branch 1 taken 18340 times.
 25224 if (ni_ == 2) {
75
1/2
✓ Branch 3 taken 6884 times.
✗ Branch 4 not taken.
 6884 d->dx = d->ki[1] * time_step_;
76 6884 d->cost = d->integral[1] * time_step_;
77
2/2
✓ Branch 0 taken 9167 times.
✓ Branch 1 taken 9173 times.
 18340 } else if (ni_ == 3) {
78
5/10
✓ Branch 2 taken 9167 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9167 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9167 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 9167 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 9167 times.
✗ Branch 16 not taken.
 9167 d->dx = (d->ki[0] + Scalar(3.) * d->ki[2]) * time_step_ / Scalar(4.);
79 9167 d->cost = (d->integral[0] + Scalar(3.) * d->integral[2]) * time_step_ /
80 Scalar(4.);
81 } else {
82
6/12
✓ Branch 1 taken 9173 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9173 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9173 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9173 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9173 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 9173 times.
✗ Branch 17 not taken.
 9173 d->dx =
83
2/4
✓ Branch 2 taken 9173 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9173 times.
✗ Branch 7 not taken.
 9173 (d->ki[0] + Scalar(2.) * d->ki[1] + Scalar(2.) * d->ki[2] + d->ki[3]) *
84 9173 time_step_ / Scalar(6.);
85 9173 d->cost = (d->integral[0] + Scalar(2.) * d->integral[1] +
86 9173 Scalar(2.) * d->integral[2] + d->integral[3]) *
87 9173 time_step_ / Scalar(6.);
88 }
89
2/4
✓ Branch 3 taken 25224 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 25224 times.
✗ Branch 7 not taken.
 25224 state_->integrate(x, d->dx, d->xnext);
90 25224 d->g = k0_data->g;
91 25224 d->h = k0_data->h;
92
1/2
✓ Branch 0 taken 25224 times.
✗ Branch 1 not taken.
 25224 if (with_cost_residual_) {
93 25224 d->r = k0_data->r;
94 }
95 25224 }
96
97 template <typename Scalar>
98 7857 void IntegratedActionModelRKTpl<Scalar>::calc(
99 const std::shared_ptr<ActionDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>& x) {
101
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 7857 times.
 7857 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
102 ✗ throw_pretty(
103 "Invalid argument: " << "x has wrong dimension (it should be " +
104 std::to_string(state_->get_nx()) + ")");
105 }
106 7857 Data* d = static_cast<Data*>(data.get());
107
108 const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
109 7857 d->differential[0];
110 7857 differential_->calc(k0_data, x);
111 7857 d->dx.setZero();
112 7857 d->xnext = x;
113 7857 d->cost = k0_data->cost;
114 7857 d->g = k0_data->g;
115 7857 d->h = k0_data->h;
116
1/2
✓ Branch 0 taken 7857 times.
✗ Branch 1 not taken.
 7857 if (with_cost_residual_) {
117 7857 d->r = k0_data->r;
118 }
119 7857 }
120
121 template <typename Scalar>
122 4327 void IntegratedActionModelRKTpl<Scalar>::calcDiff(
123 const std::shared_ptr<ActionDataAbstract>& data,
124 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
125
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 4327 times.
 4327 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
126 ✗ throw_pretty(
127 "Invalid argument: " << "x has wrong dimension (it should be " +
128 std::to_string(state_->get_nx()) + ")");
129 }
130
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4327 times.
 4327 if (static_cast<std::size_t>(u.size()) != nu_) {
131 ✗ throw_pretty(
132 "Invalid argument: " << "u has wrong dimension (it should be " +
133 std::to_string(nu_) + ")");
134 }
135 4327 const std::size_t nv = state_->get_nv();
136 4327 const std::size_t nu = control_->get_nu();
137 4327 Data* d = static_cast<Data*>(data.get());
138
5/18
✓ Branch 5 taken 4327 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 4327 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 4327 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 4327 times.
✗ Branch 16 not taken.
✗ Branch 18 not taken.
✓ Branch 19 taken 4327 times.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
 4327 assert_pretty(
139 MatrixXs(d->dyi_dx[0])
140 .isApprox(MatrixXs::Identity(state_->get_ndx(), state_->get_ndx())),
141 "you have changed dyi_dx[0] values that supposed to be constant.");
142
4/16
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4327 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4327 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✓ Branch 14 taken 4327 times.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
 4327 assert_pretty(
143 MatrixXs(d->dki_dx[0])
144 .topRightCorner(nv, nv)
145 .isApprox(MatrixXs::Identity(nv, nv)),
146 "you have changed dki_dx[0] values that supposed to be constant.");
147
148
2/2
✓ Branch 0 taken 13007 times.
✓ Branch 1 taken 4327 times.
 17334 for (std::size_t i = 0; i < ni_; ++i) {
149
2/4
✓ Branch 5 taken 13007 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 13007 times.
✗ Branch 10 not taken.
 13007 differential_->calcDiff(d->differential[i], d->y[i], d->ws[i]);
150 }
151
152 const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
153 4327 d->differential[0];
154 const std::shared_ptr<ControlParametrizationDataAbstract>& u0_data =
155 4327 d->control[0];
156
1/2
✓ Branch 4 taken 4327 times.
✗ Branch 5 not taken.
 4327 d->dki_dx[0].bottomRows(nv) = k0_data->Fx;
157
2/4
✓ Branch 2 taken 4327 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4327 times.
✗ Branch 6 not taken.
 12981 control_->multiplyByJacobian(
158
1/2
✓ Branch 2 taken 4327 times.
✗ Branch 3 not taken.
 4327 u0_data, k0_data->Fu,
159 4327 d->dki_du[0].bottomRows(nv)); // dki_du = dki_dw * dw_du
160
161 4327 d->dli_dx[0] = k0_data->Lx;
162
1/2
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
 12981 control_->multiplyJacobianTransposeBy(
163
1/2
✓ Branch 2 taken 4327 times.
✗ Branch 3 not taken.
 4327 u0_data, k0_data->Lu,
164 4327 d->dli_du[0]); // dli_du = dli_dw * dw_du
165
166 4327 d->ddli_ddx[0] = k0_data->Lxx;
167 4327 d->ddli_ddw[0] = k0_data->Luu;
168
2/4
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4327 times.
✗ Branch 7 not taken.
 12981 control_->multiplyByJacobian(
169 4327 u0_data, d->ddli_ddw[0],
170 4327 d->ddli_dwdu[0]); // ddli_dwdu = ddli_ddw * dw_du
171
2/4
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4327 times.
✗ Branch 7 not taken.
 12981 control_->multiplyJacobianTransposeBy(
172 4327 u0_data, d->ddli_dwdu[0],
173 4327 d->ddli_ddu[0]); // ddli_ddu = dw_du.T * ddli_dwdu
174 4327 d->ddli_dxdw[0] = k0_data->Lxu;
175
2/4
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4327 times.
✗ Branch 7 not taken.
 12981 control_->multiplyByJacobian(
176 4327 u0_data, d->ddli_dxdw[0],
177 4327 d->ddli_dxdu[0]); // ddli_dxdu = ddli_dxdw * dw_du
178
179
2/2
✓ Branch 0 taken 8680 times.
✓ Branch 1 taken 4327 times.
 13007 for (std::size_t i = 1; i < ni_; ++i) {
180 const std::shared_ptr<DifferentialActionDataAbstract>& ki_data =
181 8680 d->differential[i];
182 const std::shared_ptr<ControlParametrizationDataAbstract>& ui_data =
183 8680 d->control[i];
184
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->dyi_dx[i].noalias() = d->dki_dx[i - 1] * rk_c_[i] * time_step_;
185
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->dyi_du[i].noalias() = d->dki_du[i - 1] * rk_c_[i] * time_step_;
186
3/6
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
 8680 state_->JintegrateTransport(x, d->dx_rk[i], d->dyi_dx[i], second);
187
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8680 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8680 times.
✗ Branch 15 not taken.
 8680 state_->Jintegrate(x, d->dx_rk[i], d->dyi_dx[i], d->dyi_dx[i], first,
188 addto);
189
3/6
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
 8680 state_->JintegrateTransport(x, d->dx_rk[i], d->dyi_du[i],
190 second); // dyi_du = Jintegrate * dyi_du
191
192 // Sparse matrix-matrix multiplication for computing:
193
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 Eigen::Block<MatrixXs> dkvi_dq = d->dki_dx[i].bottomLeftCorner(nv, nv);
194
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 Eigen::Block<MatrixXs> dkvi_dv = d->dki_dx[i].bottomRightCorner(nv, nv);
195
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 Eigen::Block<MatrixXs> dkqi_du = d->dki_du[i].topLeftCorner(nv, nu);
196
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 Eigen::Block<MatrixXs> dkvi_du = d->dki_du[i].bottomLeftCorner(nv, nu);
197
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 const Eigen::Block<MatrixXs> dki_dqi = ki_data->Fx.bottomLeftCorner(nv, nv);
198 const Eigen::Block<MatrixXs> dki_dvi =
199
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 ki_data->Fx.bottomRightCorner(nv, nv);
200
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 const Eigen::Block<MatrixXs> dqi_dq = d->dyi_dx[i].topLeftCorner(nv, nv);
201
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 const Eigen::Block<MatrixXs> dqi_dv = d->dyi_dx[i].topRightCorner(nv, nv);
202
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 const Eigen::Block<MatrixXs> dvi_dq = d->dyi_dx[i].bottomLeftCorner(nv, nv);
203 const Eigen::Block<MatrixXs> dvi_dv =
204
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 d->dyi_dx[i].bottomRightCorner(nv, nv);
205
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 const Eigen::Block<MatrixXs> dqi_du = d->dyi_du[i].topLeftCorner(nv, nu);
206
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 const Eigen::Block<MatrixXs> dvi_du = d->dyi_du[i].bottomLeftCorner(nv, nu);
207 // i. d->dki_dx[i].noalias() = d->dki_dy[i] * d->dyi_dx[i], where dki_dy
208 // is ki_data.Fx
209
3/6
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8680 times.
✗ Branch 10 not taken.
 8680 d->dki_dx[i].topRows(nv) = d->dyi_dx[i].bottomRows(nv);
210
3/6
✓ Branch 1 taken 8680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
 8680 dkvi_dq.noalias() = dki_dqi * dqi_dq;
211
2/2
✓ Branch 0 taken 4327 times.
✓ Branch 1 taken 4353 times.
 8680 if (i == 1) {
212
2/4
✓ Branch 1 taken 4327 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4327 times.
✗ Branch 5 not taken.
 4327 dkvi_dv = time_step_ / Scalar(2.) * dki_dqi;
213 } else {
214
3/6
✓ Branch 1 taken 4353 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4353 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4353 times.
✗ Branch 8 not taken.
 4353 dkvi_dv.noalias() = dki_dqi * dqi_dv;
215 }
216
3/6
✓ Branch 1 taken 8680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
 8680 dkvi_dq.noalias() += dki_dvi * dvi_dq;
217
3/6
✓ Branch 1 taken 8680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
 8680 dkvi_dv.noalias() += dki_dvi * dvi_dv;
218 // ii. d->dki_du[i].noalias() = d->dki_dy[i] * d->dyi_du[i], where dki_dy
219 // is ki_data.Fx
220
1/2
✓ Branch 1 taken 8680 times.
✗ Branch 2 not taken.
 8680 dkqi_du = dvi_du;
221
3/6
✓ Branch 1 taken 8680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
 8680 dkvi_du.noalias() = dki_dqi * dqi_du;
222
3/6
✓ Branch 1 taken 8680 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8680 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
 8680 dkvi_du.noalias() += dki_dvi * dvi_du;
223
224
3/6
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8680 times.
✗ Branch 10 not taken.
 17360 control_->multiplyByJacobian(ui_data, ki_data->Fu,
225
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 d->dki_du[i].bottomRows(nv),
226 addto); // dfi_du = dki_dw * dw_du
227
228
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->dli_dx[i].noalias() = ki_data->Lx.transpose() * d->dyi_dx[i];
229
3/6
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8680 times.
✗ Branch 10 not taken.
 17360 control_->multiplyJacobianTransposeBy(ui_data, ki_data->Lu,
230 8680 d->dli_du[i]); // dli_du = Lu * dw_du
231
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->dli_du[i].noalias() += ki_data->Lx.transpose() * d->dyi_du[i];
232
233
3/6
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
 8680 d->Lxx_partialx[i].noalias() = ki_data->Lxx * d->dyi_dx[i];
234
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->ddli_ddx[i].noalias() = d->dyi_dx[i].transpose() * d->Lxx_partialx[i];
235
236
3/6
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8680 times.
✗ Branch 10 not taken.
 17360 control_->multiplyByJacobian(ui_data, ki_data->Lxu,
237 8680 d->Lxu_i[i]); // Lxu = Lxw * dw_du
238
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->Luu_partialx[i].noalias() = d->Lxu_i[i].transpose() * d->dyi_du[i];
239
3/6
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 8680 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
 8680 d->Lxx_partialu[i].noalias() = ki_data->Lxx * d->dyi_du[i];
240
2/4
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8680 times.
✗ Branch 6 not taken.
 26040 control_->multiplyByJacobian(
241
1/2
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
 8680 ui_data, ki_data->Luu,
242 8680 d->ddli_dwdu[i]); // ddli_dwdu = ddli_ddw * dw_du
243
3/6
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8680 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8680 times.
✗ Branch 9 not taken.
 26040 control_->multiplyJacobianTransposeBy(
244 8680 ui_data, d->ddli_dwdu[i],
245 8680 d->ddli_ddu[i]); // ddli_ddu = dw_du.T * ddli_dwdu
246
5/10
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8680 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8680 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 8680 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 8680 times.
✗ Branch 16 not taken.
 8680 d->ddli_ddu[i].noalias() += d->Luu_partialx[i].transpose() +
247 8680 d->Luu_partialx[i] +
248
2/4
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
 8680 d->dyi_du[i].transpose() * d->Lxx_partialu[i];
249
250
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->ddli_dxdw[i].noalias() = d->dyi_dx[i].transpose() * ki_data->Lxu;
251
3/6
✓ Branch 2 taken 8680 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 8680 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8680 times.
✗ Branch 9 not taken.
 26040 control_->multiplyByJacobian(
252 8680 ui_data, d->ddli_dxdw[i],
253 8680 d->ddli_dxdu[i]); // ddli_dxdu = ddli_dxdw * dw_du
254
4/8
✓ Branch 3 taken 8680 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8680 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 8680 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8680 times.
✗ Branch 14 not taken.
 8680 d->ddli_dxdu[i].noalias() += d->dyi_dx[i].transpose() * d->Lxx_partialu[i];
255 }
256
257
2/2
✓ Branch 0 taken 1426 times.
✓ Branch 1 taken 2901 times.
 4327 if (ni_ == 2) {
258
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Fx.noalias() = time_step_ * d->dki_dx[1];
259
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Fu.noalias() = time_step_ * d->dki_du[1];
260
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Lx.noalias() = time_step_ * d->dli_dx[1];
261
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Lu.noalias() = time_step_ * d->dli_du[1];
262
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Lxx.noalias() = time_step_ * d->ddli_ddx[1];
263
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Luu.noalias() = time_step_ * d->ddli_ddu[1];
264
2/4
✓ Branch 3 taken 1426 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1426 times.
✗ Branch 7 not taken.
 1426 d->Lxu.noalias() = time_step_ * d->ddli_dxdu[1];
265
2/2
✓ Branch 0 taken 1449 times.
✓ Branch 1 taken 1452 times.
 2901 } else if (ni_ == 3) {
266
3/6
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
 2898 d->Fx.noalias() =
267
2/4
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1449 times.
✗ Branch 7 not taken.
 2898 time_step_ / Scalar(4.) * (d->dki_dx[0] + Scalar(3.) * d->dki_dx[2]);
268
3/6
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
 2898 d->Fu.noalias() =
269
2/4
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1449 times.
✗ Branch 7 not taken.
 2898 time_step_ / Scalar(4.) * (d->dki_du[0] + Scalar(3.) * d->dki_du[2]);
270
3/6
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
 2898 d->Lx.noalias() =
271
2/4
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1449 times.
✗ Branch 7 not taken.
 2898 time_step_ / Scalar(4.) * (d->dli_dx[0] + Scalar(3.) * d->dli_dx[2]);
272
3/6
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
 2898 d->Lu.noalias() =
273
2/4
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1449 times.
✗ Branch 7 not taken.
 2898 time_step_ / Scalar(4.) * (d->dli_du[0] + Scalar(3.) * d->dli_du[2]);
274
4/8
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1449 times.
✗ Branch 11 not taken.
 1449 d->Lxx.noalias() = time_step_ / Scalar(4.) *
275
1/2
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
 1449 (d->ddli_ddx[0] + Scalar(3.) * d->ddli_ddx[2]);
276
4/8
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1449 times.
✗ Branch 11 not taken.
 1449 d->Luu.noalias() = time_step_ / Scalar(4.) *
277
1/2
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
 1449 (d->ddli_ddu[0] + Scalar(3.) * d->ddli_ddu[2]);
278
4/8
✓ Branch 1 taken 1449 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1449 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1449 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1449 times.
✗ Branch 11 not taken.
 1449 d->Lxu.noalias() = time_step_ / Scalar(4.) *
279
1/2
✓ Branch 2 taken 1449 times.
✗ Branch 3 not taken.
 2898 (d->ddli_dxdu[0] + Scalar(3.) * d->ddli_dxdu[2]);
280 } else {
281
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Fx.noalias() = time_step_ / Scalar(6.) *
282
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->dki_dx[0] + Scalar(2.) * d->dki_dx[1] +
283
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 1452 Scalar(2.) * d->dki_dx[2] + d->dki_dx[3]);
284
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Fu.noalias() = time_step_ / Scalar(6.) *
285
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->dki_du[0] + Scalar(2.) * d->dki_du[1] +
286
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 1452 Scalar(2.) * d->dki_du[2] + d->dki_du[3]);
287
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Lx.noalias() = time_step_ / Scalar(6.) *
288
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->dli_dx[0] + Scalar(2.) * d->dli_dx[1] +
289
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 1452 Scalar(2.) * d->dli_dx[2] + d->dli_dx[3]);
290
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Lu.noalias() = time_step_ / Scalar(6.) *
291
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->dli_du[0] + Scalar(2.) * d->dli_du[1] +
292
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 1452 Scalar(2.) * d->dli_du[2] + d->dli_du[3]);
293
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Lxx.noalias() = time_step_ / Scalar(6.) *
294
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->ddli_ddx[0] + Scalar(2.) * d->ddli_ddx[1] +
295
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 1452 Scalar(2.) * d->ddli_ddx[2] + d->ddli_ddx[3]);
296
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Luu.noalias() = time_step_ / Scalar(6.) *
297
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->ddli_ddu[0] + Scalar(2.) * d->ddli_ddu[1] +
298
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 1452 Scalar(2.) * d->ddli_ddu[2] + d->ddli_ddu[3]);
299
6/12
✓ Branch 1 taken 1452 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1452 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1452 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1452 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1452 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1452 times.
✗ Branch 17 not taken.
 1452 d->Lxu.noalias() = time_step_ / Scalar(6.) *
300
1/2
✓ Branch 2 taken 1452 times.
✗ Branch 3 not taken.
 1452 (d->ddli_dxdu[0] + Scalar(2.) * d->ddli_dxdu[1] +
301
1/2
✓ Branch 3 taken 1452 times.
✗ Branch 4 not taken.
 2904 Scalar(2.) * d->ddli_dxdu[2] + d->ddli_dxdu[3]);
302 }
303 4327 d->Gx = k0_data->Gx;
304 4327 d->Hx = k0_data->Hx;
305 4327 d->Gu.conservativeResize(differential_->get_ng(), nu_);
306 4327 d->Hu.conservativeResize(differential_->get_nh(), nu_);
307
2/4
✓ Branch 4 taken 4327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4327 times.
✗ Branch 8 not taken.
 4327 control_->multiplyByJacobian(u0_data, k0_data->Gu, d->Gu);
308
2/4
✓ Branch 4 taken 4327 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4327 times.
✗ Branch 8 not taken.
 4327 control_->multiplyByJacobian(u0_data, k0_data->Hu, d->Hu);
309
310
2/4
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4327 times.
✗ Branch 7 not taken.
 4327 state_->JintegrateTransport(x, d->dx, d->Fx, second);
311
3/6
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4327 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4327 times.
✗ Branch 10 not taken.
 4327 state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
312
2/4
✓ Branch 3 taken 4327 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4327 times.
✗ Branch 7 not taken.
 4327 state_->JintegrateTransport(x, d->dx, d->Fu, second);
313 4327 }
314
315 template <typename Scalar>
316 394 void IntegratedActionModelRKTpl<Scalar>::calcDiff(
317 const std::shared_ptr<ActionDataAbstract>& data,
318 const Eigen::Ref<const VectorXs>& x) {
319
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 394 times.
 394 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
320 ✗ throw_pretty(
321 "Invalid argument: " << "x has wrong dimension (it should be " +
322 std::to_string(state_->get_nx()) + ")");
323 }
324 394 Data* d = static_cast<Data*>(data.get());
325
326 const std::shared_ptr<DifferentialActionDataAbstract>& k0_data =
327 394 d->differential[0];
328 394 differential_->calcDiff(k0_data, x);
329 394 d->Lx = k0_data->Lx;
330 394 d->Lxx = k0_data->Lxx;
331 394 d->Gx = k0_data->Gx;
332 394 d->Hx = k0_data->Hx;
333 394 }
334
335 template <typename Scalar>
336 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
337 31557 IntegratedActionModelRKTpl<Scalar>::createData() {
338
1/2
✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
 31557 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
339 }
340
341 template <typename Scalar>
342 template <typename NewScalar>
343 ✗ IntegratedActionModelRKTpl<NewScalar> IntegratedActionModelRKTpl<Scalar>::cast()
344 const {
345 typedef IntegratedActionModelRKTpl<NewScalar> ReturnType;
346 ✗ if (control_) {
347 ✗ ReturnType ret(differential_->template cast<NewScalar>(),
348 ✗ control_->template cast<NewScalar>(), rk_type_,
349 ✗ scalar_cast<NewScalar>(time_step_), with_cost_residual_);
350 ✗ return ret;
351 ✗ } else {
352 ✗ ReturnType ret(differential_->template cast<NewScalar>(), rk_type_,
353 ✗ scalar_cast<NewScalar>(time_step_), with_cost_residual_);
354 ✗ return ret;
355 }
356 }
357
358 template <typename Scalar>
359 4531 bool IntegratedActionModelRKTpl<Scalar>::checkData(
360 const std::shared_ptr<ActionDataAbstract>& data) {
361 4531 std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
362
1/2
✓ Branch 1 taken 4531 times.
✗ Branch 2 not taken.
 4531 if (data != NULL) {
363
2/2
✓ Branch 0 taken 13616 times.
✓ Branch 1 taken 4531 times.
 18147 for (std::size_t i = 0; i < ni_; ++i) {
364
2/4
✓ Branch 4 taken 13616 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 13616 times.
 13616 if (!differential_->checkData(d->differential[i])) {
365 ✗ return false;
366 }
367 }
368 4531 return true;
369 } else {
370 ✗ return false;
371 }
372 4531 }
373
374 template <typename Scalar>
375 5520 void IntegratedActionModelRKTpl<Scalar>::quasiStatic(
376 const std::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
377 const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
378 const Scalar tol) {
379
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5520 times.
 5520 if (static_cast<std::size_t>(u.size()) != nu_) {
380 ✗ throw_pretty(
381 "Invalid argument: " << "u has wrong dimension (it should be " +
382 std::to_string(nu_) + ")");
383 }
384
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 5520 times.
 5520 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
385 ✗ throw_pretty(
386 "Invalid argument: " << "x has wrong dimension (it should be " +
387 std::to_string(state_->get_nx()) + ")");
388 }
389
390 5520 Data* d = static_cast<Data*>(data.get());
391 const std::shared_ptr<ControlParametrizationDataAbstract>& u0_data =
392 5520 d->control[0];
393
1/2
✓ Branch 2 taken 5520 times.
✗ Branch 3 not taken.
 5520 u0_data->w *= Scalar(0.);
394
1/2
✓ Branch 5 taken 5520 times.
✗ Branch 6 not taken.
 5520 differential_->quasiStatic(d->differential[0], u0_data->w, x, maxiter, tol);
395
1/2
✓ Branch 4 taken 5520 times.
✗ Branch 5 not taken.
 5520 control_->params(u0_data, Scalar(0.), u0_data->w);
396 5520 u = u0_data->u;
397 5520 }
398
399 template <typename Scalar>
400 791241 std::size_t IntegratedActionModelRKTpl<Scalar>::get_ni() const {
401 791241 return ni_;
402 }
403
404 template <typename Scalar>
405 184 void IntegratedActionModelRKTpl<Scalar>::print(std::ostream& os) const {
406 184 os << "IntegratedActionModelRK {dt=" << time_step_ << ", " << *differential_
407 184 << "}";
408 184 }
409
410 template <typename Scalar>
411 832 void IntegratedActionModelRKTpl<Scalar>::set_rk_type(const RKType rktype) {
412
3/4
✓ Branch 0 taken 231 times.
✓ Branch 1 taken 299 times.
✓ Branch 2 taken 302 times.
✗ Branch 3 not taken.
 832 switch (rktype) {
413 231 case two:
414 231 ni_ = 2;
415 231 rk_c_.resize(ni_);
416 231 rk_c_[0] = Scalar(0.);
417 231 rk_c_[1] = Scalar(0.5);
418 231 break;
419 299 case three:
420 299 ni_ = 3;
421 299 rk_c_.resize(ni_);
422 299 rk_c_[0] = Scalar(0.);
423 299 rk_c_[1] = Scalar(1. / 3.);
424 299 rk_c_[2] = Scalar(2. / 3.);
425 299 break;
426 302 case four:
427 302 ni_ = 4;
428 302 rk_c_.resize(ni_);
429 302 rk_c_[0] = Scalar(0.);
430 302 rk_c_[1] = Scalar(0.5);
431 302 rk_c_[2] = Scalar(0.5);
432 302 rk_c_[3] = Scalar(1.);
433 302 break;
434 }
435 832 }
436
437 } // namespace crocoddyl
438