GCC Code Coverage Report

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