Directory: | ./ |
---|---|
File: | include/crocoddyl/core/numdiff/action.hxx |
Date: | 2025-02-24 23:41:29 |
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 |