GCC Code Coverage Report

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