()
| 251 | |
| 252 | |
| 253 | def test_fly_high(): |
| 254 | fr_name1 = "larm_shoulder2_body" |
| 255 | fr_id1 = model.getFrameId(fr_name1) |
| 256 | space = manifolds.MultibodyPhaseSpace(model) |
| 257 | fun = aligator.FlyHighResidual(space.ndx, model, fr_id1, 0.1, nu) |
| 258 | data = fun.createData() |
| 259 | data2 = fun.createData() |
| 260 | Jx_nd = data.Jx.copy() |
| 261 | np.set_printoptions(precision=2, linewidth=250) |
| 262 | |
| 263 | for _ in range(10): |
| 264 | x0 = sample_gauss(space) |
| 265 | fun.evaluate(x0, data) |
| 266 | fun.computeJacobians(x0, data) |
| 267 | |
| 268 | ei = np.zeros(space.ndx) |
| 269 | for i in range(space.ndx): |
| 270 | ei[i] = FD_EPS |
| 271 | x0_p = space.integrate(x0, ei) |
| 272 | fun.evaluate(x0_p, data2) |
| 273 | Jx_nd[:, i] = (data2.value - data.value) / FD_EPS |
| 274 | ei[i] = 0.0 |
| 275 | |
| 276 | err_Jx = data.Jx - Jx_nd |
| 277 | print(err_Jx, err_Jx.max()) |
| 278 | assert np.allclose(data.Jx, Jx_nd, atol=ATOL) |
| 279 | |
| 280 | |
| 281 | def test_frame_collision(): |
nothing calls this directly
no test coverage detected