()
| 210 | |
| 211 | |
| 212 | def test_frame_velocity(): |
| 213 | fr_name1 = "larm_shoulder2_body" |
| 214 | fr_id1 = model.getFrameId(fr_name1) |
| 215 | space = manifolds.MultibodyPhaseSpace(model) |
| 216 | |
| 217 | x0 = sample_gauss(space) |
| 218 | q0, v0 = x0[: model.nq], x0[model.nq :] |
| 219 | u0 = np.zeros(nu) |
| 220 | |
| 221 | pin.forwardKinematics(model, rdata, q0, v0) |
| 222 | ref_type = pin.LOCAL |
| 223 | v_ref = pin.getFrameVelocity(model, rdata, fr_id1, ref_type) |
| 224 | |
| 225 | fun = aligator.FrameVelocityResidual(space.ndx, nu, model, v_ref, fr_id1, ref_type) |
| 226 | assert fr_id1 == fun.frame_id |
| 227 | assert np.allclose(v_ref, fun.getReference()) |
| 228 | |
| 229 | fdata = fun.createData() |
| 230 | fun.evaluate(x0, fdata) |
| 231 | print("v_ref=", v_ref) |
| 232 | |
| 233 | assert np.allclose(fdata.value, 0.0) |
| 234 | |
| 235 | fun.evaluate(x0, fdata) |
| 236 | fun.computeJacobians(x0, fdata) |
| 237 | |
| 238 | fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS) |
| 239 | fdata2 = fun_fd.createData() |
| 240 | fun_fd.evaluate(x0, u0, fdata2) |
| 241 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 242 | assert fdata.Jx.shape == fdata2.Jx.shape |
| 243 | |
| 244 | for i in range(100): |
| 245 | x0 = sample_gauss(space) |
| 246 | fun.evaluate(x0, fdata) |
| 247 | fun.computeJacobians(x0, fdata) |
| 248 | fun_fd.evaluate(x0, u0, fdata2) |
| 249 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 250 | assert np.allclose(fdata.Jx, fdata2.Jx, atol=ATOL) |
| 251 | |
| 252 | |
| 253 | def test_fly_high(): |
nothing calls this directly
no test coverage detected