| 47 | |
| 48 | |
| 49 | def ode_finite_difference(dyn: dynamics.ODEAbstract, space, x, u, eps=1e-8): |
| 50 | assert isinstance(dyn, dynamics.ODEAbstract) |
| 51 | ndx = space.ndx |
| 52 | Jx = np.zeros((ndx, ndx)) |
| 53 | dx = np.zeros(ndx) |
| 54 | data = dyn.createData() |
| 55 | dyn.forward(x, u, data) |
| 56 | # distance to origin |
| 57 | _dx = space.difference(space.neutral(), x) |
| 58 | ex = eps * max(1.0, np.linalg.norm(_dx)) |
| 59 | f = data.xdot.copy() |
| 60 | for i in range(ndx): |
| 61 | dx[i] = ex |
| 62 | dyn.forward(space.integrate(x, dx), u, data) |
| 63 | fp = data.xdot.copy() |
| 64 | dyn.forward(space.integrate(x, -dx), u, data) |
| 65 | fm = data.xdot.copy() |
| 66 | Jx[:, i] = (fp - fm) / (2 * ex) |
| 67 | dx[i] = 0.0 |
| 68 | |
| 69 | nu = u.shape[0] |
| 70 | eu = eps * max(1.0, np.linalg.norm(u)) |
| 71 | Ju = np.zeros((ndx, nu)) |
| 72 | du = np.zeros(nu) |
| 73 | data = dyn.createData() |
| 74 | for i in range(nu): |
| 75 | du[i] = eu |
| 76 | dyn.forward(x, u + du, data) |
| 77 | fp[:] = data.xdot |
| 78 | Ju[:, i] = (fp - f) / eu |
| 79 | du[i] = 0.0 |
| 80 | |
| 81 | return Jx, Ju |
| 82 | |
| 83 | |
| 84 | def cost_finite_grad(costmodel, space, x, u, eps=1e-8): |