| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, |
||
5 |
// New York University, Heriot-Watt University |
||
6 |
// Max Planck Gesellschaft |
||
7 |
// Copyright note valid unless otherwise stated in individual files. |
||
8 |
// All rights reserved. |
||
9 |
/////////////////////////////////////////////////////////////////////////////// |
||
10 |
|||
11 |
#include "crocoddyl/core/numdiff/diff-action.hpp" |
||
12 |
#include "crocoddyl/core/utils/exception.hpp" |
||
13 |
|||
14 |
namespace crocoddyl { |
||
15 |
|||
16 |
template <typename Scalar> |
||
17 |
84 |
DifferentialActionModelNumDiffTpl<Scalar>::DifferentialActionModelNumDiffTpl( |
|
18 |
boost::shared_ptr<Base> model, const bool with_gauss_approx) |
||
19 |
: Base(model->get_state(), model->get_nu(), model->get_nr(), |
||
20 |
168 |
model->get_ng(), model->get_nh()), |
|
21 |
model_(model), |
||
22 |
with_gauss_approx_(with_gauss_approx), |
||
23 |
✓✗ | 84 |
e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) { |
24 |
84 |
e_hess_ = std::sqrt(2.0 * e_jac_); |
|
25 |
✗✓✗✗ |
84 |
if (with_gauss_approx_ && nr_ == 1) |
26 |
throw_pretty("No Gauss approximation possible with nr = 1"); |
||
27 |
84 |
} |
|
28 |
|||
29 |
template <typename Scalar> |
||
30 |
172 |
DifferentialActionModelNumDiffTpl< |
|
31 |
172 |
Scalar>::~DifferentialActionModelNumDiffTpl() {} |
|
32 |
|||
33 |
template <typename Scalar> |
||
34 |
83 |
void DifferentialActionModelNumDiffTpl<Scalar>::calc( |
|
35 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
36 |
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
||
37 |
✗✓ | 83 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
38 |
throw_pretty("Invalid argument: " |
||
39 |
<< "x has wrong dimension (it should be " + |
||
40 |
std::to_string(state_->get_nx()) + ")"); |
||
41 |
} |
||
42 |
✗✓ | 83 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
43 |
throw_pretty("Invalid argument: " |
||
44 |
<< "u has wrong dimension (it should be " + |
||
45 |
std::to_string(nu_) + ")"); |
||
46 |
} |
||
47 |
83 |
Data* d = static_cast<Data*>(data.get()); |
|
48 |
83 |
model_->calc(d->data_0, x, u); |
|
49 |
83 |
data->xout = d->data_0->xout; |
|
50 |
83 |
data->cost = d->data_0->cost; |
|
51 |
83 |
d->g = d->data_0->g; |
|
52 |
83 |
d->h = d->data_0->h; |
|
53 |
83 |
} |
|
54 |
|||
55 |
template <typename Scalar> |
||
56 |
83 |
void DifferentialActionModelNumDiffTpl<Scalar>::calc( |
|
57 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
58 |
const Eigen::Ref<const VectorXs>& x) { |
||
59 |
✗✓ | 83 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
60 |
throw_pretty("Invalid argument: " |
||
61 |
<< "x has wrong dimension (it should be " + |
||
62 |
std::to_string(state_->get_nx()) + ")"); |
||
63 |
} |
||
64 |
83 |
Data* d = static_cast<Data*>(data.get()); |
|
65 |
83 |
model_->calc(d->data_0, x); |
|
66 |
83 |
data->xout = d->data_0->xout; |
|
67 |
83 |
data->cost = d->data_0->cost; |
|
68 |
83 |
d->g = d->data_0->g; |
|
69 |
83 |
d->h = d->data_0->h; |
|
70 |
83 |
} |
|
71 |
|||
72 |
template <typename Scalar> |
||
73 |
83 |
void DifferentialActionModelNumDiffTpl<Scalar>::calcDiff( |
|
74 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
75 |
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
||
76 |
// For details about the finite difference formulas see |
||
77 |
// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf |
||
78 |
✗✓ | 83 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
79 |
throw_pretty("Invalid argument: " |
||
80 |
<< "x has wrong dimension (it should be " + |
||
81 |
std::to_string(state_->get_nx()) + ")"); |
||
82 |
} |
||
83 |
✗✓ | 83 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
84 |
throw_pretty("Invalid argument: " |
||
85 |
<< "u has wrong dimension (it should be " + |
||
86 |
std::to_string(nu_) + ")"); |
||
87 |
} |
||
88 |
83 |
Data* d = static_cast<Data*>(data.get()); |
|
89 |
|||
90 |
83 |
const VectorXs& x0 = d->data_0->xout; |
|
91 |
83 |
const Scalar c0 = d->data_0->cost; |
|
92 |
83 |
const VectorXs& g0 = d->g; |
|
93 |
83 |
const VectorXs& h0 = d->h; |
|
94 |
83 |
const std::size_t ndx = state_->get_ndx(); |
|
95 |
83 |
const std::size_t nu = model_->get_nu(); |
|
96 |
83 |
const std::size_t ng = model_->get_ng(); |
|
97 |
83 |
const std::size_t nh = model_->get_nh(); |
|
98 |
83 |
d->Gx.resize(ng, ndx); |
|
99 |
83 |
d->Gu.resize(ng, nu); |
|
100 |
83 |
d->Hx.resize(nh, ndx); |
|
101 |
83 |
d->Hu.resize(nh, nu); |
|
102 |
83 |
d->du.setZero(); |
|
103 |
|||
104 |
83 |
assertStableStateFD(x); |
|
105 |
|||
106 |
// Computing the d action(x,u) / dx |
||
107 |
✓✗✓✗ ✓✗ |
83 |
model_->get_state()->diff(model_->get_state()->zero(), x, d->dx); |
108 |
83 |
d->x_norm = d->dx.norm(); |
|
109 |
83 |
d->dx.setZero(); |
|
110 |
83 |
d->xh_jac = e_jac_ * std::max(1., d->x_norm); |
|
111 |
✓✓ | 4591 |
for (std::size_t ix = 0; ix < ndx; ++ix) { |
112 |
4508 |
d->dx(ix) = d->xh_jac; |
|
113 |
✓✗✓✗ |
4508 |
model_->get_state()->integrate(x, d->dx, d->xp); |
114 |
✓✗ | 4508 |
model_->calc(d->data_x[ix], d->xp, u); |
115 |
// dynamics |
||
116 |
✓✗✓✗ ✓✗ |
4508 |
data->Fx.col(ix) = (d->data_x[ix]->xout - x0) / d->xh_jac; |
117 |
// constraint |
||
118 |
✓✗✓✗ ✓✗ |
4508 |
data->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac; |
119 |
✓✗✓✗ ✓✗ |
4508 |
data->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac; |
120 |
// cost |
||
121 |
4508 |
data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac; |
|
122 |
✓✗✓✗ ✓✗ |
4508 |
d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac; |
123 |
4508 |
d->dx(ix) = 0.; |
|
124 |
} |
||
125 |
|||
126 |
// Computing the d action(x,u) / du |
||
127 |
83 |
d->uh_jac = e_jac_ * std::max(1., u.norm()); |
|
128 |
✓✓ | 1999 |
for (std::size_t iu = 0; iu < nu; ++iu) { |
129 |
1916 |
d->du(iu) = d->uh_jac; |
|
130 |
✓✗✓✗ |
1916 |
model_->calc(d->data_u[iu], x, u + d->du); |
131 |
// dynamics |
||
132 |
✓✗✓✗ ✓✗ |
1916 |
data->Fu.col(iu) = (d->data_u[iu]->xout - x0) / d->uh_jac; |
133 |
// constraint |
||
134 |
✓✗✓✗ ✓✗ |
1916 |
data->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac; |
135 |
✓✗✓✗ ✓✗ |
1916 |
data->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac; |
136 |
// cost |
||
137 |
1916 |
data->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac; |
|
138 |
✓✗✓✗ ✓✗ |
1916 |
d->Ru.col(iu) = (d->data_u[iu]->r - d->data_0->r) / d->uh_jac; |
139 |
1916 |
d->du(iu) = 0.; |
|
140 |
} |
||
141 |
|||
142 |
#ifdef NDEBUG |
||
143 |
// Computing the d^2 cost(x,u) / dx^2 |
||
144 |
d->xh_hess = e_hess_ * std::max(1., d->x_norm); |
||
145 |
d->xh_hess_pow2 = d->xh_hess * d->xh_hess; |
||
146 |
for (std::size_t ix = 0; ix < ndx; ++ix) { |
||
147 |
d->dx(ix) = d->xh_hess; |
||
148 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
149 |
model_->calc(d->data_x[ix], d->xp, u); |
||
150 |
const Scalar cp = d->data_x[ix]->cost; |
||
151 |
model_->get_state()->integrate(x, -d->dx, d->xp); |
||
152 |
model_->calc(d->data_x[ix], d->xp, u); |
||
153 |
const Scalar cm = d->data_x[ix]->cost; |
||
154 |
data->Lxx(ix, ix) = (cp - 2 * c0 + cm) / d->xh_hess_pow2; |
||
155 |
for (std::size_t jx = ix + 1; jx < ndx; ++jx) { |
||
156 |
d->dx(jx) = d->xh_hess; |
||
157 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
158 |
model_->calc(d->data_x[ix], d->xp, u); |
||
159 |
const Scalar cpp = |
||
160 |
d->data_x[ix] |
||
161 |
->cost; // cost due to positive disturbance in both directions |
||
162 |
d->dx(ix) = 0.; |
||
163 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
164 |
model_->calc(d->data_x[ix], d->xp, u); |
||
165 |
const Scalar czp = |
||
166 |
d->data_x[ix]->cost; // cost due to zero disturance in 'i' and |
||
167 |
// positive disturbance in 'j' direction |
||
168 |
data->Lxx(ix, jx) = (cpp - czp - cp + c0) / d->xh_hess_pow2; |
||
169 |
data->Lxx(jx, ix) = data->Lxx(ix, jx); |
||
170 |
d->dx(ix) = d->xh_hess; |
||
171 |
d->dx(jx) = 0.; |
||
172 |
} |
||
173 |
d->dx(ix) = 0.; |
||
174 |
} |
||
175 |
|||
176 |
// Computing the d^2 cost(x,u) / du^2 |
||
177 |
d->uh_hess = e_hess_ * std::max(1., u.norm()); |
||
178 |
d->uh_hess_pow2 = d->uh_hess * d->uh_hess; |
||
179 |
for (std::size_t iu = 0; iu < nu; ++iu) { |
||
180 |
d->du(iu) = d->uh_hess; |
||
181 |
model_->calc(d->data_u[iu], x, u + d->du); |
||
182 |
const Scalar cp = d->data_u[iu]->cost; |
||
183 |
model_->calc(d->data_u[iu], x, u - d->du); |
||
184 |
const Scalar cm = d->data_u[iu]->cost; |
||
185 |
data->Luu(iu, iu) = (cp - 2 * c0 + cm) / d->uh_hess_pow2; |
||
186 |
for (std::size_t ju = iu + 1; ju < nu; ++ju) { |
||
187 |
d->du(ju) = d->uh_hess; |
||
188 |
model_->calc(d->data_u[iu], x, u + d->du); |
||
189 |
const Scalar cpp = |
||
190 |
d->data_u[iu] |
||
191 |
->cost; // cost due to positive disturbance in both directions |
||
192 |
d->du(iu) = 0.; |
||
193 |
model_->calc(d->data_u[iu], x, u + d->du); |
||
194 |
const Scalar czp = |
||
195 |
d->data_u[iu]->cost; // cost due to zero disturance in 'i' and |
||
196 |
// positive disturbance in 'j' direction |
||
197 |
data->Luu(iu, ju) = (cpp - czp - cp + c0) / d->uh_hess_pow2; |
||
198 |
data->Luu(ju, iu) = data->Luu(iu, ju); |
||
199 |
d->du(iu) = d->uh_hess; |
||
200 |
d->du(ju) = 0.; |
||
201 |
} |
||
202 |
d->du(iu) = 0.; |
||
203 |
} |
||
204 |
|||
205 |
// Computing the d^2 cost(x,u) / dxu |
||
206 |
d->xuh_hess_pow2 = 4. * d->xh_hess * d->uh_hess; |
||
207 |
for (std::size_t ix = 0; ix < ndx; ++ix) { |
||
208 |
for (std::size_t ju = 0; ju < nu; ++ju) { |
||
209 |
d->dx(ix) = d->xh_hess; |
||
210 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
211 |
d->du(ju) = d->uh_hess; |
||
212 |
model_->calc(d->data_x[ix], d->xp, u + d->du); |
||
213 |
const Scalar cpp = d->data_x[ix]->cost; |
||
214 |
model_->calc(d->data_x[ix], d->xp, u - d->du); |
||
215 |
const Scalar cpm = d->data_x[ix]->cost; |
||
216 |
model_->get_state()->integrate(x, -d->dx, d->xp); |
||
217 |
model_->calc(d->data_x[ix], d->xp, u + d->du); |
||
218 |
const Scalar cmp = d->data_x[ix]->cost; |
||
219 |
model_->calc(d->data_x[ix], d->xp, u - d->du); |
||
220 |
const Scalar cmm = d->data_x[ix]->cost; |
||
221 |
data->Lxu(ix, ju) = (cpp - cpm - cmp + cmm) / d->xuh_hess_pow2; |
||
222 |
d->dx(ix) = 0.; |
||
223 |
d->du(ju) = 0.; |
||
224 |
} |
||
225 |
} |
||
226 |
#endif |
||
227 |
|||
228 |
✗✓ | 83 |
if (with_gauss_approx_) { |
229 |
data->Lxx = d->Rx.transpose() * d->Rx; |
||
230 |
data->Lxu = d->Rx.transpose() * d->Ru; |
||
231 |
data->Luu = d->Ru.transpose() * d->Ru; |
||
232 |
} |
||
233 |
83 |
} |
|
234 |
|||
235 |
template <typename Scalar> |
||
236 |
83 |
void DifferentialActionModelNumDiffTpl<Scalar>::calcDiff( |
|
237 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
238 |
const Eigen::Ref<const VectorXs>& x) { |
||
239 |
// For details about the finite difference formulas see |
||
240 |
// http://www.it.uom.gr/teaching/linearalgebra/NumericalRecipiesInC/c5-7.pdf |
||
241 |
✗✓ | 83 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
242 |
throw_pretty("Invalid argument: " |
||
243 |
<< "x has wrong dimension (it should be " + |
||
244 |
std::to_string(state_->get_nx()) + ")"); |
||
245 |
} |
||
246 |
83 |
Data* d = static_cast<Data*>(data.get()); |
|
247 |
|||
248 |
83 |
const Scalar c0 = d->data_0->cost; |
|
249 |
83 |
const VectorXs& g0 = d->g; |
|
250 |
83 |
const VectorXs& h0 = d->h; |
|
251 |
83 |
const std::size_t ndx = state_->get_ndx(); |
|
252 |
83 |
d->Gx.resize(model_->get_ng(), ndx); |
|
253 |
83 |
d->Hx.resize(model_->get_nh(), ndx); |
|
254 |
|||
255 |
83 |
assertStableStateFD(x); |
|
256 |
|||
257 |
// Computing the d action(x,u) / dx |
||
258 |
✓✗✓✗ ✓✗ |
83 |
model_->get_state()->diff(model_->get_state()->zero(), x, d->dx); |
259 |
83 |
d->x_norm = d->dx.norm(); |
|
260 |
83 |
d->dx.setZero(); |
|
261 |
83 |
d->xh_jac = e_jac_ * std::max(1., d->x_norm); |
|
262 |
✓✓ | 4591 |
for (std::size_t ix = 0; ix < ndx; ++ix) { |
263 |
4508 |
d->dx(ix) = d->xh_jac; |
|
264 |
✓✗✓✗ |
4508 |
model_->get_state()->integrate(x, d->dx, d->xp); |
265 |
✓✗ | 4508 |
model_->calc(d->data_x[ix], d->xp); |
266 |
// cost |
||
267 |
4508 |
data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac; |
|
268 |
✓✗✓✗ ✓✗ |
4508 |
d->Rx.col(ix) = (d->data_x[ix]->r - d->data_0->r) / d->xh_jac; |
269 |
// constraint |
||
270 |
✓✗✓✗ ✓✗ |
4508 |
data->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac; |
271 |
✓✗✓✗ ✓✗ |
4508 |
data->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac; |
272 |
4508 |
d->dx(ix) = 0.; |
|
273 |
} |
||
274 |
|||
275 |
#ifdef NDEBUG |
||
276 |
// Computing the d^2 cost(x,u) / dx^2 |
||
277 |
d->xh_hess = e_hess_ * std::max(1., d->x_norm); |
||
278 |
d->xh_hess_pow2 = d->xh_hess * d->xh_hess; |
||
279 |
for (std::size_t ix = 0; ix < ndx; ++ix) { |
||
280 |
// We can apply the same formulas for finite difference as above |
||
281 |
d->dx(ix) = d->xh_hess; |
||
282 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
283 |
model_->calc(d->data_x[ix], d->xp); |
||
284 |
const Scalar cp = d->data_x[ix]->cost; |
||
285 |
model_->get_state()->integrate(x, -d->dx, d->xp); |
||
286 |
model_->calc(d->data_x[ix], d->xp); |
||
287 |
const Scalar cm = d->data_x[ix]->cost; |
||
288 |
data->Lxx(ix, ix) = (cp - 2 * c0 + cm) / d->xh_hess_pow2; |
||
289 |
for (std::size_t jx = ix + 1; jx < ndx; ++jx) { |
||
290 |
d->dx(jx) = d->xh_hess; |
||
291 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
292 |
model_->calc(d->data_x[ix], d->xp); |
||
293 |
const Scalar cpp = |
||
294 |
d->data_x[ix] |
||
295 |
->cost; // cost due to positive disturbance in both directions |
||
296 |
d->dx(ix) = 0.; |
||
297 |
model_->get_state()->integrate(x, d->dx, d->xp); |
||
298 |
model_->calc(d->data_x[ix], d->xp); |
||
299 |
const Scalar czp = |
||
300 |
d->data_x[ix]->cost; // cost due to zero disturance in 'i' and |
||
301 |
// positive disturbance in 'j' direction |
||
302 |
data->Lxx(ix, jx) = (cpp - czp - cp + c0) / d->xh_hess_pow2; |
||
303 |
data->Lxx(jx, ix) = data->Lxx(ix, jx); |
||
304 |
d->dx(ix) = d->xh_hess; |
||
305 |
d->dx(jx) = 0.; |
||
306 |
} |
||
307 |
d->dx(ix) = 0.; |
||
308 |
} |
||
309 |
#endif |
||
310 |
|||
311 |
✗✓ | 83 |
if (with_gauss_approx_) { |
312 |
data->Lxx = d->Rx.transpose() * d->Rx; |
||
313 |
} |
||
314 |
83 |
} |
|
315 |
|||
316 |
template <typename Scalar> |
||
317 |
boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > |
||
318 |
85 |
DifferentialActionModelNumDiffTpl<Scalar>::createData() { |
|
319 |
✓✗ | 85 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this); |
320 |
} |
||
321 |
|||
322 |
template <typename Scalar> |
||
323 |
void DifferentialActionModelNumDiffTpl<Scalar>::quasiStatic( |
||
324 |
const boost::shared_ptr<DifferentialActionDataAbstract>& data, |
||
325 |
Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, |
||
326 |
const std::size_t maxiter, const Scalar tol) { |
||
327 |
Data* d = static_cast<Data*>(data.get()); |
||
328 |
model_->quasiStatic(d->data_0, u, x, maxiter, tol); |
||
329 |
} |
||
330 |
|||
331 |
template <typename Scalar> |
||
332 |
const boost::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >& |
||
333 |
7286 |
DifferentialActionModelNumDiffTpl<Scalar>::get_model() const { |
|
334 |
7286 |
return model_; |
|
335 |
} |
||
336 |
|||
337 |
template <typename Scalar> |
||
338 |
83 |
const Scalar DifferentialActionModelNumDiffTpl<Scalar>::get_disturbance() |
|
339 |
const { |
||
340 |
83 |
return e_jac_; |
|
341 |
} |
||
342 |
|||
343 |
template <typename Scalar> |
||
344 |
void DifferentialActionModelNumDiffTpl<Scalar>::set_disturbance( |
||
345 |
const Scalar disturbance) { |
||
346 |
if (disturbance < 0.) { |
||
347 |
throw_pretty("Invalid argument: " |
||
348 |
<< "Disturbance constant is positive"); |
||
349 |
} |
||
350 |
e_jac_ = disturbance; |
||
351 |
e_hess_ = std::sqrt(2.0 * e_jac_); |
||
352 |
} |
||
353 |
|||
354 |
template <typename Scalar> |
||
355 |
140 |
bool DifferentialActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() { |
|
356 |
140 |
return with_gauss_approx_; |
|
357 |
} |
||
358 |
|||
359 |
template <typename Scalar> |
||
360 |
void DifferentialActionModelNumDiffTpl<Scalar>::print(std::ostream& os) const { |
||
361 |
os << "DifferentialActionModelNumDiffTpl {action=" << *model_ << "}"; |
||
362 |
} |
||
363 |
|||
364 |
template <typename Scalar> |
||
365 |
166 |
void DifferentialActionModelNumDiffTpl<Scalar>::assertStableStateFD( |
|
366 |
const Eigen::Ref<const VectorXs>& /** x */) { |
||
367 |
// TODO(cmastalli): First we need to do it AMNumDiff and then to replicate it. |
||
368 |
166 |
} |
|
369 |
|||
370 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |