GCC Code Coverage Report

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