Use finite differences to compute Jacobians of a `aligator.dynamics.DynamicsModel`. TODO: move to a test utils file
(
dyn: aligator.dynamics.DynamicsModel,
space: manifolds.ManifoldAbstract,
x0,
u0,
y0=None,
eps=EPSILON,
)
| 11 | |
| 12 | |
| 13 | def dynamics_finite_difference( |
| 14 | dyn: aligator.dynamics.DynamicsModel, |
| 15 | space: manifolds.ManifoldAbstract, |
| 16 | x0, |
| 17 | u0, |
| 18 | y0=None, |
| 19 | eps=EPSILON, |
| 20 | ): |
| 21 | """Use finite differences to compute Jacobians |
| 22 | of a `aligator.dynamics.DynamicsModel`. |
| 23 | |
| 24 | TODO: move to a test utils file |
| 25 | """ |
| 26 | if y0 is None: |
| 27 | y0 = x0 |
| 28 | data = dyn.createData() |
| 29 | Jx_nd = np.zeros((dyn.ndx2, dyn.ndx1)) |
| 30 | ei = np.zeros(dyn.ndx1) |
| 31 | dyn.evaluate(x0, u0, y0, data) |
| 32 | r0 = data.value.copy() |
| 33 | for i in range(dyn.ndx1): |
| 34 | ei[i] = eps |
| 35 | xplus = space.integrate(x0, ei) |
| 36 | dyn.evaluate(xplus, u0, y0, data) |
| 37 | Jx_nd[:, i] = (data.value - r0) / eps |
| 38 | ei[i] = 0.0 |
| 39 | |
| 40 | ei = np.zeros(dyn.nu) |
| 41 | Ju_nd = np.zeros((dyn.ndx2, dyn.nu)) |
| 42 | for i in range(dyn.nu): |
| 43 | ei[i] = eps |
| 44 | dyn.evaluate(x0, u0 + ei, y0, data) |
| 45 | Ju_nd[:, i] = (data.value - r0) / eps |
| 46 | ei[i] = 0.0 |
| 47 | |
| 48 | ei = np.zeros(dyn.ndx2) |
| 49 | yplus = y0.copy() |
| 50 | Jy_nd = np.zeros((dyn.ndx2, dyn.ndx2)) |
| 51 | for i in range(dyn.ndx2): |
| 52 | ei[i] = eps |
| 53 | space.integrate(y0, ei, yplus) |
| 54 | dyn.evaluate(x0, u0, yplus, data) |
| 55 | Jy_nd[:, i] = (data.value - r0) / eps |
| 56 | ei[i] = 0.0 |
| 57 | |
| 58 | return Jx_nd, Ju_nd, Jy_nd |
| 59 | |
| 60 | |
| 61 | def explicit_dynamics_finite_difference( |
no test coverage detected