GCC Code Coverage Report

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