| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-2022, 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 |
528 |
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 |
✓✗ | 528 |
: Base(model, control, time_step, with_cost_residual) { |
22 |
✓✗ | 528 |
set_rk_type(rktype); |
23 |
528 |
} |
|
24 |
|||
25 |
template <typename Scalar> |
||
26 |
268 |
IntegratedActionModelRKTpl<Scalar>::IntegratedActionModelRKTpl( |
|
27 |
boost::shared_ptr<DifferentialActionModelAbstract> model, |
||
28 |
const RKType rktype, const Scalar time_step, const bool with_cost_residual) |
||
29 |
✓✗ | 268 |
: Base(model, time_step, with_cost_residual) { |
30 |
✓✗ | 268 |
set_rk_type(rktype); |
31 |
268 |
} |
|
32 |
|||
33 |
template <typename Scalar> |
||
34 |
1596 |
IntegratedActionModelRKTpl<Scalar>::~IntegratedActionModelRKTpl() {} |
|
35 |
|||
36 |
template <typename Scalar> |
||
37 |
23480 |
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 |
✗✓ | 23480 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
41 |
throw_pretty("Invalid argument: " |
||
42 |
<< "x has wrong dimension (it should be " + |
||
43 |
std::to_string(state_->get_nx()) + ")"); |
||
44 |
} |
||
45 |
✗✓ | 23480 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
46 |
throw_pretty("Invalid argument: " |
||
47 |
<< "u has wrong dimension (it should be " + |
||
48 |
std::to_string(nu_) + ")"); |
||
49 |
} |
||
50 |
23480 |
const std::size_t nv = state_->get_nv(); |
|
51 |
23480 |
Data* d = static_cast<Data*>(data.get()); |
|
52 |
|||
53 |
const boost::shared_ptr<DifferentialActionDataAbstract>& k0_data = |
||
54 |
23480 |
d->differential[0]; |
|
55 |
const boost::shared_ptr<ControlParametrizationDataAbstract>& u0_data = |
||
56 |
23480 |
d->control[0]; |
|
57 |
23480 |
control_->calc(u0_data, rk_c_[0], u); |
|
58 |
23480 |
d->ws[0] = u0_data->w; |
|
59 |
✓✗ | 23480 |
differential_->calc(k0_data, x, d->ws[0]); |
60 |
23480 |
d->y[0] = x; |
|
61 |
✓✗✓✗ |
23480 |
d->ki[0].head(nv) = d->y[0].tail(nv); |
62 |
✓✗ | 23480 |
d->ki[0].tail(nv) = k0_data->xout; |
63 |
23480 |
d->integral[0] = k0_data->cost; |
|
64 |
✓✓ | 72526 |
for (std::size_t i = 1; i < ni_; ++i) { |
65 |
const boost::shared_ptr<DifferentialActionDataAbstract>& ki_data = |
||
66 |
49046 |
d->differential[i]; |
|
67 |
const boost::shared_ptr<ControlParametrizationDataAbstract>& ui_data = |
||
68 |
49046 |
d->control[i]; |
|
69 |
✓✗✓✗ ✓✗ |
49046 |
d->dx_rk[i].noalias() = time_step_ * rk_c_[i] * d->ki[i - 1]; |
70 |
✓✗✓✗ |
49046 |
state_->integrate(x, d->dx_rk[i], d->y[i]); |
71 |
49046 |
control_->calc(ui_data, rk_c_[i], u); |
|
72 |
49046 |
d->ws[i] = ui_data->w; |
|
73 |
✓✗✓✗ |
49046 |
differential_->calc(ki_data, d->y[i], d->ws[i]); |
74 |
✓✗✓✗ |
49046 |
d->ki[i].head(nv) = d->y[i].tail(nv); |
75 |
✓✗ | 49046 |
d->ki[i].tail(nv) = ki_data->xout; |
76 |
49046 |
d->integral[i] = ki_data->cost; |
|
77 |
} |
||
78 |
|||
79 |
✓✓ | 23480 |
if (ni_ == 2) { |
80 |
✓✗ | 6438 |
d->dx = d->ki[1] * time_step_; |
81 |
6438 |
d->cost = d->integral[1] * time_step_; |
|
82 |
✓✓ | 17042 |
} else if (ni_ == 3) { |
83 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8518 |
d->dx = (d->ki[0] + Scalar(3.) * d->ki[2]) * time_step_ / Scalar(4.); |
84 |
8518 |
d->cost = (d->integral[0] + Scalar(3.) * d->integral[2]) * time_step_ / |
|
85 |
Scalar(4.); |
||
86 |
} else { |
||
87 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
8524 |
d->dx = |
88 |
✓✗✓✗ |
8524 |
(d->ki[0] + Scalar(2.) * d->ki[1] + Scalar(2.) * d->ki[2] + d->ki[3]) * |
89 |
8524 |
time_step_ / Scalar(6.); |
|
90 |
8524 |
d->cost = (d->integral[0] + Scalar(2.) * d->integral[1] + |
|
91 |
8524 |
Scalar(2.) * d->integral[2] + d->integral[3]) * |
|
92 |
8524 |
time_step_ / Scalar(6.); |
|
93 |
} |
||
94 |
✓✗✓✗ |
23480 |
state_->integrate(x, d->dx, d->xnext); |
95 |
23480 |
d->g = k0_data->g; |
|
96 |
23480 |
d->h = k0_data->h; |
|
97 |
✓✗ | 23480 |
if (with_cost_residual_) { |
98 |
23480 |
d->r = k0_data->r; |
|
99 |
} |
||
100 |
23480 |
} |
|
101 |
|||
102 |
template <typename Scalar> |
||
103 |
7172 |
void IntegratedActionModelRKTpl<Scalar>::calc( |
|
104 |
const boost::shared_ptr<ActionDataAbstract>& data, |
||
105 |
const Eigen::Ref<const VectorXs>& x) { |
||
106 |
✗✓ | 7172 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
107 |
throw_pretty("Invalid argument: " |
||
108 |
<< "x has wrong dimension (it should be " + |
||
109 |
std::to_string(state_->get_nx()) + ")"); |
||
110 |
} |
||
111 |
7172 |
Data* d = static_cast<Data*>(data.get()); |
|
112 |
|||
113 |
const boost::shared_ptr<DifferentialActionDataAbstract>& k0_data = |
||
114 |
7172 |
d->differential[0]; |
|
115 |
7172 |
differential_->calc(k0_data, x); |
|
116 |
7172 |
d->dx.setZero(); |
|
117 |
7172 |
d->xnext = x; |
|
118 |
7172 |
d->cost = k0_data->cost; |
|
119 |
7172 |
d->g = k0_data->g; |
|
120 |
7172 |
d->h = k0_data->h; |
|
121 |
✓✗ | 7172 |
if (with_cost_residual_) { |
122 |
7172 |
d->r = k0_data->r; |
|
123 |
} |
||
124 |
7172 |
} |
|
125 |
|||
126 |
template <typename Scalar> |
||
127 |
4139 |
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 |
✗✓ | 4139 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
131 |
throw_pretty("Invalid argument: " |
||
132 |
<< "x has wrong dimension (it should be " + |
||
133 |
std::to_string(state_->get_nx()) + ")"); |
||
134 |
} |
||
135 |
✗✓ | 4139 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
136 |
throw_pretty("Invalid argument: " |
||
137 |
<< "u has wrong dimension (it should be " + |
||
138 |
std::to_string(nu_) + ")"); |
||
139 |
} |
||
140 |
4139 |
const std::size_t nv = state_->get_nv(); |
|
141 |
4139 |
const std::size_t nu = control_->get_nu(); |
|
142 |
4139 |
Data* d = static_cast<Data*>(data.get()); |
|
143 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗ |
4139 |
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 |
✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗ |
4139 |
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 |
✓✓ | 16581 |
for (std::size_t i = 0; i < ni_; ++i) { |
154 |
✓✗✓✗ |
12442 |
differential_->calcDiff(d->differential[i], d->y[i], d->ws[i]); |
155 |
} |
||
156 |
|||
157 |
const boost::shared_ptr<DifferentialActionDataAbstract>& k0_data = |
||
158 |
4139 |
d->differential[0]; |
|
159 |
const boost::shared_ptr<ControlParametrizationDataAbstract>& u0_data = |
||
160 |
4139 |
d->control[0]; |
|
161 |
✓✗ | 4139 |
d->dki_dx[0].bottomRows(nv) = k0_data->Fx; |
162 |
✓✗✓✗ |
12417 |
control_->multiplyByJacobian( |
163 |
✓✗ | 4139 |
u0_data, k0_data->Fu, |
164 |
4139 |
d->dki_du[0].bottomRows(nv)); // dki_du = dki_dw * dw_du |
|
165 |
|||
166 |
4139 |
d->dli_dx[0] = k0_data->Lx; |
|
167 |
✓✗ | 12417 |
control_->multiplyJacobianTransposeBy( |
168 |
✓✗ | 4139 |
u0_data, k0_data->Lu, |
169 |
4139 |
d->dli_du[0]); // dli_du = dli_dw * dw_du |
|
170 |
|||
171 |
4139 |
d->ddli_ddx[0] = k0_data->Lxx; |
|
172 |
4139 |
d->ddli_ddw[0] = k0_data->Luu; |
|
173 |
✓✗✓✗ |
12417 |
control_->multiplyByJacobian( |
174 |
4139 |
u0_data, d->ddli_ddw[0], |
|
175 |
4139 |
d->ddli_dwdu[0]); // ddli_dwdu = ddli_ddw * dw_du |
|
176 |
✓✗✓✗ |
12417 |
control_->multiplyJacobianTransposeBy( |
177 |
4139 |
u0_data, d->ddli_dwdu[0], |
|
178 |
4139 |
d->ddli_ddu[0]); // ddli_ddu = dw_du.T * ddli_dwdu |
|
179 |
4139 |
d->ddli_dxdw[0] = k0_data->Lxu; |
|
180 |
✓✗✓✗ |
12417 |
control_->multiplyByJacobian( |
181 |
4139 |
u0_data, d->ddli_dxdw[0], |
|
182 |
4139 |
d->ddli_dxdu[0]); // ddli_dxdu = ddli_dxdw * dw_du |
|
183 |
|||
184 |
✓✓ | 12442 |
for (std::size_t i = 1; i < ni_; ++i) { |
185 |
const boost::shared_ptr<DifferentialActionDataAbstract>& ki_data = |
||
186 |
8303 |
d->differential[i]; |
|
187 |
const boost::shared_ptr<ControlParametrizationDataAbstract>& ui_data = |
||
188 |
8303 |
d->control[i]; |
|
189 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->dyi_dx[i].noalias() = d->dki_dx[i - 1] * rk_c_[i] * time_step_; |
190 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->dyi_du[i].noalias() = d->dki_du[i - 1] * rk_c_[i] * time_step_; |
191 |
✓✗✓✗ ✓✗ |
8303 |
state_->JintegrateTransport(x, d->dx_rk[i], d->dyi_dx[i], second); |
192 |
✓✗✓✗ ✓✗✓✗ |
8303 |
state_->Jintegrate(x, d->dx_rk[i], d->dyi_dx[i], d->dyi_dx[i], first, |
193 |
addto); |
||
194 |
✓✗✓✗ ✓✗ |
8303 |
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 |
✓✗ | 8303 |
Eigen::Block<MatrixXs> dkvi_dq = d->dki_dx[i].bottomLeftCorner(nv, nv); |
199 |
✓✗ | 8303 |
Eigen::Block<MatrixXs> dkvi_dv = d->dki_dx[i].bottomRightCorner(nv, nv); |
200 |
✓✗ | 8303 |
Eigen::Block<MatrixXs> dkqi_du = d->dki_du[i].topLeftCorner(nv, nu); |
201 |
✓✗ | 8303 |
Eigen::Block<MatrixXs> dkvi_du = d->dki_du[i].bottomLeftCorner(nv, nu); |
202 |
✓✗ | 8303 |
const Eigen::Block<MatrixXs> dki_dqi = ki_data->Fx.bottomLeftCorner(nv, nv); |
203 |
const Eigen::Block<MatrixXs> dki_dvi = |
||
204 |
✓✗ | 8303 |
ki_data->Fx.bottomRightCorner(nv, nv); |
205 |
✓✗ | 8303 |
const Eigen::Block<MatrixXs> dqi_dq = d->dyi_dx[i].topLeftCorner(nv, nv); |
206 |
✓✗ | 8303 |
const Eigen::Block<MatrixXs> dqi_dv = d->dyi_dx[i].topRightCorner(nv, nv); |
207 |
✓✗ | 8303 |
const Eigen::Block<MatrixXs> dvi_dq = d->dyi_dx[i].bottomLeftCorner(nv, nv); |
208 |
✓✗ | 8303 |
const Eigen::Block<MatrixXs> dvi_dv = |
209 |
8303 |
d->dyi_dx[i].bottomRightCorner(nv, nv); |
|
210 |
✓✗ | 8303 |
const Eigen::Block<MatrixXs> dqi_du = d->dyi_du[i].topLeftCorner(nv, nu); |
211 |
✓✗ | 8303 |
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 |
✓✗✓✗ ✓✗ |
8303 |
d->dki_dx[i].topRows(nv) = d->dyi_dx[i].bottomRows(nv); |
215 |
✓✗✓✗ ✓✗ |
8303 |
dkvi_dq.noalias() = dki_dqi * dqi_dq; |
216 |
✓✓ | 8303 |
if (i == 1) { |
217 |
✓✗✓✗ |
4139 |
dkvi_dv = time_step_ / Scalar(2.) * dki_dqi; |
218 |
} else { |
||
219 |
✓✗✓✗ ✓✗ |
4164 |
dkvi_dv.noalias() = dki_dqi * dqi_dv; |
220 |
} |
||
221 |
✓✗✓✗ ✓✗ |
8303 |
dkvi_dq.noalias() += dki_dvi * dvi_dq; |
222 |
✓✗✓✗ ✓✗ |
8303 |
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 |
✓✗ | 8303 |
dkqi_du = dvi_du; |
226 |
✓✗✓✗ ✓✗ |
8303 |
dkvi_du.noalias() = dki_dqi * dqi_du; |
227 |
✓✗✓✗ ✓✗ |
8303 |
dkvi_du.noalias() += dki_dvi * dvi_du; |
228 |
|||
229 |
✓✗✓✗ ✓✗✓✗ |
16606 |
control_->multiplyByJacobian(ui_data, ki_data->Fu, |
230 |
8303 |
d->dki_du[i].bottomRows(nv), |
|
231 |
addto); // dfi_du = dki_dw * dw_du |
||
232 |
|||
233 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->dli_dx[i].noalias() = ki_data->Lx.transpose() * d->dyi_dx[i]; |
234 |
✓✗✓✗ ✓✗ |
16606 |
control_->multiplyJacobianTransposeBy(ui_data, ki_data->Lu, |
235 |
8303 |
d->dli_du[i]); // dli_du = Lu * dw_du |
|
236 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->dli_du[i].noalias() += ki_data->Lx.transpose() * d->dyi_du[i]; |
237 |
|||
238 |
✓✗✓✗ ✓✗ |
8303 |
d->Lxx_partialx[i].noalias() = ki_data->Lxx * d->dyi_dx[i]; |
239 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->ddli_ddx[i].noalias() = d->dyi_dx[i].transpose() * d->Lxx_partialx[i]; |
240 |
|||
241 |
✓✗✓✗ ✓✗ |
16606 |
control_->multiplyByJacobian(ui_data, ki_data->Lxu, |
242 |
8303 |
d->Lxu_i[i]); // Lxu = Lxw * dw_du |
|
243 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->Luu_partialx[i].noalias() = d->Lxu_i[i].transpose() * d->dyi_du[i]; |
244 |
✓✗✓✗ ✓✗ |
8303 |
d->Lxx_partialu[i].noalias() = ki_data->Lxx * d->dyi_du[i]; |
245 |
✓✗✓✗ |
24909 |
control_->multiplyByJacobian( |
246 |
✓✗ | 8303 |
ui_data, ki_data->Luu, |
247 |
8303 |
d->ddli_dwdu[i]); // ddli_dwdu = ddli_ddw * dw_du |
|
248 |
✓✗✓✗ ✓✗ |
24909 |
control_->multiplyJacobianTransposeBy( |
249 |
8303 |
ui_data, d->ddli_dwdu[i], |
|
250 |
8303 |
d->ddli_ddu[i]); // ddli_ddu = dw_du.T * ddli_dwdu |
|
251 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
8303 |
d->ddli_ddu[i].noalias() += d->Luu_partialx[i].transpose() + |
252 |
8303 |
d->Luu_partialx[i] + |
|
253 |
✓✗✓✗ |
8303 |
d->dyi_du[i].transpose() * d->Lxx_partialu[i]; |
254 |
|||
255 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->ddli_dxdw[i].noalias() = d->dyi_dx[i].transpose() * ki_data->Lxu; |
256 |
✓✗✓✗ ✓✗ |
24909 |
control_->multiplyByJacobian( |
257 |
8303 |
ui_data, d->ddli_dxdw[i], |
|
258 |
8303 |
d->ddli_dxdu[i]); // ddli_dxdu = ddli_dxdw * dw_du |
|
259 |
✓✗✓✗ ✓✗✓✗ |
8303 |
d->ddli_dxdu[i].noalias() += d->dyi_dx[i].transpose() * d->Lxx_partialu[i]; |
260 |
} |
||
261 |
|||
262 |
✓✓ | 4139 |
if (ni_ == 2) { |
263 |
✓✗✓✗ |
1364 |
d->Fx.noalias() = time_step_ * d->dki_dx[1]; |
264 |
✓✗✓✗ |
1364 |
d->Fu.noalias() = time_step_ * d->dki_du[1]; |
265 |
✓✗✓✗ |
1364 |
d->Lx.noalias() = time_step_ * d->dli_dx[1]; |
266 |
✓✗✓✗ |
1364 |
d->Lu.noalias() = time_step_ * d->dli_du[1]; |
267 |
✓✗✓✗ |
1364 |
d->Lxx.noalias() = time_step_ * d->ddli_ddx[1]; |
268 |
✓✗✓✗ |
1364 |
d->Luu.noalias() = time_step_ * d->ddli_ddu[1]; |
269 |
✓✗✓✗ |
1364 |
d->Lxu.noalias() = time_step_ * d->ddli_dxdu[1]; |
270 |
✓✓ | 2775 |
} else if (ni_ == 3) { |
271 |
✓✗✓✗ ✓✗ |
2772 |
d->Fx.noalias() = |
272 |
✓✗✓✗ |
2772 |
time_step_ / Scalar(4.) * (d->dki_dx[0] + Scalar(3.) * d->dki_dx[2]); |
273 |
✓✗✓✗ ✓✗ |
2772 |
d->Fu.noalias() = |
274 |
✓✗✓✗ |
2772 |
time_step_ / Scalar(4.) * (d->dki_du[0] + Scalar(3.) * d->dki_du[2]); |
275 |
✓✗✓✗ ✓✗ |
2772 |
d->Lx.noalias() = |
276 |
✓✗✓✗ |
2772 |
time_step_ / Scalar(4.) * (d->dli_dx[0] + Scalar(3.) * d->dli_dx[2]); |
277 |
✓✗✓✗ ✓✗ |
2772 |
d->Lu.noalias() = |
278 |
✓✗✓✗ |
2772 |
time_step_ / Scalar(4.) * (d->dli_du[0] + Scalar(3.) * d->dli_du[2]); |
279 |
✓✗✓✗ ✓✗✓✗ |
1386 |
d->Lxx.noalias() = time_step_ / Scalar(4.) * |
280 |
✓✗ | 1386 |
(d->ddli_ddx[0] + Scalar(3.) * d->ddli_ddx[2]); |
281 |
✓✗✓✗ ✓✗✓✗ |
1386 |
d->Luu.noalias() = time_step_ / Scalar(4.) * |
282 |
✓✗ | 1386 |
(d->ddli_ddu[0] + Scalar(3.) * d->ddli_ddu[2]); |
283 |
✓✗✓✗ ✓✗✓✗ |
1386 |
d->Lxu.noalias() = time_step_ / Scalar(4.) * |
284 |
✓✗ | 2772 |
(d->ddli_dxdu[0] + Scalar(3.) * d->ddli_dxdu[2]); |
285 |
} else { |
||
286 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Fx.noalias() = time_step_ / Scalar(6.) * |
287 |
✓✗ | 1389 |
(d->dki_dx[0] + Scalar(2.) * d->dki_dx[1] + |
288 |
✓✗ | 1389 |
Scalar(2.) * d->dki_dx[2] + d->dki_dx[3]); |
289 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Fu.noalias() = time_step_ / Scalar(6.) * |
290 |
✓✗ | 1389 |
(d->dki_du[0] + Scalar(2.) * d->dki_du[1] + |
291 |
✓✗ | 1389 |
Scalar(2.) * d->dki_du[2] + d->dki_du[3]); |
292 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Lx.noalias() = time_step_ / Scalar(6.) * |
293 |
✓✗ | 1389 |
(d->dli_dx[0] + Scalar(2.) * d->dli_dx[1] + |
294 |
✓✗ | 1389 |
Scalar(2.) * d->dli_dx[2] + d->dli_dx[3]); |
295 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Lu.noalias() = time_step_ / Scalar(6.) * |
296 |
✓✗ | 1389 |
(d->dli_du[0] + Scalar(2.) * d->dli_du[1] + |
297 |
✓✗ | 1389 |
Scalar(2.) * d->dli_du[2] + d->dli_du[3]); |
298 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Lxx.noalias() = time_step_ / Scalar(6.) * |
299 |
✓✗ | 1389 |
(d->ddli_ddx[0] + Scalar(2.) * d->ddli_ddx[1] + |
300 |
✓✗ | 1389 |
Scalar(2.) * d->ddli_ddx[2] + d->ddli_ddx[3]); |
301 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Luu.noalias() = time_step_ / Scalar(6.) * |
302 |
✓✗ | 1389 |
(d->ddli_ddu[0] + Scalar(2.) * d->ddli_ddu[1] + |
303 |
✓✗ | 1389 |
Scalar(2.) * d->ddli_ddu[2] + d->ddli_ddu[3]); |
304 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
1389 |
d->Lxu.noalias() = time_step_ / Scalar(6.) * |
305 |
✓✗ | 1389 |
(d->ddli_dxdu[0] + Scalar(2.) * d->ddli_dxdu[1] + |
306 |
✓✗ | 2778 |
Scalar(2.) * d->ddli_dxdu[2] + d->ddli_dxdu[3]); |
307 |
} |
||
308 |
4139 |
d->Gx = k0_data->Gx; |
|
309 |
4139 |
d->Hx = k0_data->Hx; |
|
310 |
4139 |
d->Gu.resize(differential_->get_ng(), nu_); |
|
311 |
4139 |
d->Hu.resize(differential_->get_nh(), nu_); |
|
312 |
✓✗✓✗ |
4139 |
control_->multiplyByJacobian(u0_data, k0_data->Gu, d->Gu); |
313 |
✓✗✓✗ |
4139 |
control_->multiplyByJacobian(u0_data, k0_data->Hu, d->Hu); |
314 |
|||
315 |
✓✗✓✗ |
4139 |
state_->JintegrateTransport(x, d->dx, d->Fx, second); |
316 |
✓✗✓✗ ✓✗ |
4139 |
state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto); |
317 |
✓✗✓✗ |
4139 |
state_->JintegrateTransport(x, d->dx, d->Fu, second); |
318 |
4139 |
} |
|
319 |
|||
320 |
template <typename Scalar> |
||
321 |
377 |
void IntegratedActionModelRKTpl<Scalar>::calcDiff( |
|
322 |
const boost::shared_ptr<ActionDataAbstract>& data, |
||
323 |
const Eigen::Ref<const VectorXs>& x) { |
||
324 |
✗✓ | 377 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
325 |
throw_pretty("Invalid argument: " |
||
326 |
<< "x has wrong dimension (it should be " + |
||
327 |
std::to_string(state_->get_nx()) + ")"); |
||
328 |
} |
||
329 |
377 |
Data* d = static_cast<Data*>(data.get()); |
|
330 |
|||
331 |
const boost::shared_ptr<DifferentialActionDataAbstract>& k0_data = |
||
332 |
377 |
d->differential[0]; |
|
333 |
377 |
differential_->calcDiff(k0_data, x); |
|
334 |
377 |
d->Lx = k0_data->Lx; |
|
335 |
377 |
d->Lxx = k0_data->Lxx; |
|
336 |
377 |
d->Gx = k0_data->Gx; |
|
337 |
377 |
d->Hx = k0_data->Hx; |
|
338 |
377 |
} |
|
339 |
|||
340 |
template <typename Scalar> |
||
341 |
boost::shared_ptr<ActionDataAbstractTpl<Scalar> > |
||
342 |
29538 |
IntegratedActionModelRKTpl<Scalar>::createData() { |
|
343 |
✓✗ | 29538 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); |
344 |
} |
||
345 |
|||
346 |
template <typename Scalar> |
||
347 |
4334 |
bool IntegratedActionModelRKTpl<Scalar>::checkData( |
|
348 |
const boost::shared_ptr<ActionDataAbstract>& data) { |
||
349 |
8668 |
boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data); |
|
350 |
✓✗ | 4334 |
if (data != NULL) { |
351 |
✓✓ | 17358 |
for (std::size_t i = 0; i < ni_; ++i) { |
352 |
✓✗✗✓ |
13024 |
if (!differential_->checkData(d->differential[i])) { |
353 |
return false; |
||
354 |
} |
||
355 |
} |
||
356 |
4334 |
return true; |
|
357 |
} else { |
||
358 |
return false; |
||
359 |
} |
||
360 |
} |
||
361 |
|||
362 |
template <typename Scalar> |
||
363 |
5280 |
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 |
✗✓ | 5280 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
368 |
throw_pretty("Invalid argument: " |
||
369 |
<< "u has wrong dimension (it should be " + |
||
370 |
std::to_string(nu_) + ")"); |
||
371 |
} |
||
372 |
✗✓ | 5280 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
373 |
throw_pretty("Invalid argument: " |
||
374 |
<< "x has wrong dimension (it should be " + |
||
375 |
std::to_string(state_->get_nx()) + ")"); |
||
376 |
} |
||
377 |
|||
378 |
5280 |
Data* d = static_cast<Data*>(data.get()); |
|
379 |
const boost::shared_ptr<ControlParametrizationDataAbstract>& u0_data = |
||
380 |
5280 |
d->control[0]; |
|
381 |
✓✗ | 5280 |
u0_data->w *= 0.; |
382 |
✓✗ | 5280 |
differential_->quasiStatic(d->differential[0], u0_data->w, x, maxiter, tol); |
383 |
✓✗ | 5280 |
control_->params(u0_data, 0., u0_data->w); |
384 |
5280 |
u = u0_data->u; |
|
385 |
5280 |
} |
|
386 |
|||
387 |
template <typename Scalar> |
||
388 |
740562 |
std::size_t IntegratedActionModelRKTpl<Scalar>::get_ni() const { |
|
389 |
740562 |
return ni_; |
|
390 |
} |
||
391 |
|||
392 |
template <typename Scalar> |
||
393 |
176 |
void IntegratedActionModelRKTpl<Scalar>::print(std::ostream& os) const { |
|
394 |
176 |
os << "IntegratedActionModelRK {dt=" << time_step_ << ", " << *differential_ |
|
395 |
176 |
<< "}"; |
|
396 |
176 |
} |
|
397 |
|||
398 |
template <typename Scalar> |
||
399 |
796 |
void IntegratedActionModelRKTpl<Scalar>::set_rk_type(const RKType rktype) { |
|
400 |
✓✓✓✗ |
796 |
switch (rktype) { |
401 |
221 |
case two: |
|
402 |
221 |
ni_ = 2; |
|
403 |
221 |
rk_c_.resize(ni_); |
|
404 |
221 |
rk_c_[0] = Scalar(0.); |
|
405 |
221 |
rk_c_[1] = Scalar(0.5); |
|
406 |
221 |
break; |
|
407 |
286 |
case three: |
|
408 |
286 |
ni_ = 3; |
|
409 |
286 |
rk_c_.resize(ni_); |
|
410 |
286 |
rk_c_[0] = Scalar(0.); |
|
411 |
286 |
rk_c_[1] = Scalar(1. / 3.); |
|
412 |
286 |
rk_c_[2] = Scalar(2. / 3.); |
|
413 |
286 |
break; |
|
414 |
289 |
case four: |
|
415 |
289 |
ni_ = 4; |
|
416 |
289 |
rk_c_.resize(ni_); |
|
417 |
289 |
rk_c_[0] = Scalar(0.); |
|
418 |
289 |
rk_c_[1] = Scalar(0.5); |
|
419 |
289 |
rk_c_[2] = Scalar(0.5); |
|
420 |
289 |
rk_c_[3] = Scalar(1.); |
|
421 |
289 |
break; |
|
422 |
} |
||
423 |
796 |
} |
|
424 |
|||
425 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |