GCC Code Coverage Report

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