GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/action.hxx
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 143 0.0%
Functions: 0 28 0.0%
Branches: 0 330 0.0%
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 ✗ 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 ✗ model->get_ng(), model->get_nh(), model->get_ng_T(),
20 ✗ model->get_nh_T()),
21 ✗ model_(model),
22 ✗ e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())),
23 ✗ with_gauss_approx_(with_gauss_approx) {
24 ✗ e_hess_ = sqrt(Scalar(2.0) * e_jac_);
25 ✗ this->set_u_lb(model_->get_u_lb());
26 ✗ this->set_u_ub(model_->get_u_ub());
27 ✗ }
28
29 template <typename Scalar>
30 ✗ 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 ✗ 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 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
44 ✗ model_->calc(d->data_0, x, u);
45 ✗ data->xnext = d->data_0->xnext;
46 ✗ data->cost = d->data_0->cost;
47 ✗ d->g = d->data_0->g;
48 ✗ d->h = d->data_0->h;
49 ✗ }
50
51 template <typename Scalar>
52 ✗ void ActionModelNumDiffTpl<Scalar>::calc(
53 const std::shared_ptr<ActionDataAbstract>& data,
54 const Eigen::Ref<const VectorXs>& x) {
55 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
61 ✗ model_->calc(d->data_0, x);
62 ✗ data->xnext = d->data_0->xnext;
63 ✗ data->cost = d->data_0->cost;
64 ✗ d->g = d->data_0->g;
65 ✗ d->h = d->data_0->h;
66 ✗ }
67
68 template <typename Scalar>
69 ✗ 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 ✗ 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 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
83
84 ✗ const VectorXs& x0 = d->data_0->xnext;
85 ✗ const Scalar c0 = d->data_0->cost;
86 ✗ data->xnext = d->data_0->xnext;
87 ✗ data->cost = d->data_0->cost;
88 ✗ const VectorXs& g0 = d->g;
89 ✗ const VectorXs& h0 = d->h;
90 ✗ const std::size_t ndx = model_->get_state()->get_ndx();
91 ✗ const std::size_t nu = model_->get_nu();
92 ✗ const std::size_t ng = model_->get_ng();
93 ✗ const std::size_t nh = model_->get_nh();
94 ✗ d->Gx.conservativeResize(ng, ndx);
95 ✗ d->Gu.conservativeResize(ng, nu);
96 ✗ d->Hx.conservativeResize(nh, ndx);
97 ✗ d->Hu.conservativeResize(nh, nu);
98 ✗ d->du.setZero();
99
100 ✗ assertStableStateFD(x);
101
102 // Computing the d action(x,u) / dx
103 ✗ model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
104 ✗ d->x_norm = d->dx.norm();
105 ✗ d->dx.setZero();
106 ✗ d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
107 ✗ for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
108 ✗ d->dx(ix) = d->xh_jac;
109 ✗ model_->get_state()->integrate(x, d->dx, d->xp);
110 ✗ model_->calc(d->data_x[ix], d->xp, u);
111 // dynamics
112 ✗ model_->get_state()->diff(x0, d->data_x[ix]->xnext, d->Fx.col(ix));
113 // cost
114 ✗ data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
115 ✗ 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 ✗ data->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
120 ✗ data->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
121 ✗ d->dx(ix) = Scalar(0.);
122 }
123 ✗ data->Fx /= d->xh_jac;
124
125 // Computing the d action(x,u) / du
126 ✗ d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
127 ✗ for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
128 ✗ d->du(iu) = d->uh_jac;
129 ✗ model_->calc(d->data_u[iu], x, u + d->du);
130 // dynamics
131 ✗ model_->get_state()->diff(x0, d->data_u[iu]->xnext, d->Fu.col(iu));
132 // cost
133 ✗ data->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
134 ✗ 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 ✗ d->Gu.col(iu) = (d->data_u[iu]->g - g0) / d->uh_jac;
139 ✗ d->Hu.col(iu) = (d->data_u[iu]->h - h0) / d->uh_jac;
140 ✗ d->du(iu) = Scalar(0.);
141 }
142 ✗ 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 ✗ 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 ✗ }
236
237 template <typename Scalar>
238 ✗ void ActionModelNumDiffTpl<Scalar>::calcDiff(
239 const std::shared_ptr<ActionDataAbstract>& data,
240 const Eigen::Ref<const VectorXs>& x) {
241 ✗ 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 ✗ Data* d = static_cast<Data*>(data.get());
247
248 ✗ const Scalar c0 = d->data_0->cost;
249 ✗ data->xnext = d->data_0->xnext;
250 ✗ data->cost = d->data_0->cost;
251 ✗ const VectorXs& g0 = d->g;
252 ✗ const VectorXs& h0 = d->h;
253 ✗ const std::size_t ndx = model_->get_state()->get_ndx();
254 ✗ d->Gx.conservativeResize(model_->get_ng_T(), ndx);
255 ✗ d->Hx.conservativeResize(model_->get_nh_T(), ndx);
256
257 ✗ assertStableStateFD(x);
258
259 // Computing the d action(x,u) / dx
260 ✗ model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
261 ✗ d->x_norm = d->dx.norm();
262 ✗ d->dx.setZero();
263 ✗ d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
264 ✗ for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
265 ✗ d->dx(ix) = d->xh_jac;
266 ✗ model_->get_state()->integrate(x, d->dx, d->xp);
267 ✗ model_->calc(d->data_x[ix], d->xp);
268 // cost
269 ✗ data->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
270 ✗ 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 ✗ d->Gx.col(ix) = (d->data_x[ix]->g - g0) / d->xh_jac;
275 ✗ d->Hx.col(ix) = (d->data_x[ix]->h - h0) / d->xh_jac;
276 ✗ 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 ✗ if (get_with_gauss_approx()) {
316 ✗ data->Lxx = d->Rx.transpose() * d->Rx;
317 }
318 ✗ }
319
320 template <typename Scalar>
321 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
322 ✗ ActionModelNumDiffTpl<Scalar>::createData() {
323 ✗ 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 ✗ ActionModelNumDiffTpl<Scalar>::get_model() const {
346 ✗ return model_;
347 }
348
349 template <typename Scalar>
350 ✗ const Scalar ActionModelNumDiffTpl<Scalar>::get_disturbance() const {
351 ✗ 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_ = sqrt(Scalar(2.0) * e_jac_);
361 ✗ }
362
363 template <typename Scalar>
364 ✗ bool ActionModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
365 ✗ 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 ✗ void ActionModelNumDiffTpl<Scalar>::assertStableStateFD(
375 const Eigen::Ref<const VectorXs>& /** x */) {
376 // do nothing in the general case
377 ✗ }
378
379 } // namespace crocoddyl
380