()
| 168 | |
| 169 | |
| 170 | def test_frame_translation(): |
| 171 | fr_name1 = "larm_shoulder2_body" |
| 172 | fr_id1 = model.getFrameId(fr_name1) |
| 173 | |
| 174 | space = manifolds.MultibodyConfiguration(model) |
| 175 | ndx = space.ndx |
| 176 | x0 = space.neutral() |
| 177 | d = np.random.randn(space.ndx) * 0.1 |
| 178 | d[6:] = 0.0 |
| 179 | x0 = space.integrate(x0, d) |
| 180 | u0 = np.zeros(nu) |
| 181 | q0 = x0[:nq] |
| 182 | |
| 183 | pin.framesForwardKinematics(model, rdata, q0) |
| 184 | target_pos = rdata.oMf[fr_id1].translation |
| 185 | |
| 186 | fun = aligator.FrameTranslationResidual(ndx, nu, model, target_pos, fr_id1) |
| 187 | assert fr_id1 == fun.frame_id |
| 188 | |
| 189 | fdata = fun.createData() |
| 190 | fun.evaluate(x0, u0, fdata) |
| 191 | |
| 192 | assert np.allclose(fdata.value, 0.0) |
| 193 | |
| 194 | fun.computeJacobians(x0, u0, fdata) |
| 195 | |
| 196 | fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS) |
| 197 | fdata2 = fun_fd.createData() |
| 198 | fun_fd.evaluate(x0, u0, fdata2) |
| 199 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 200 | # just check dims for now |
| 201 | assert fdata.Jx.shape == fdata2.Jx.shape |
| 202 | |
| 203 | for i in range(100): |
| 204 | x0 = sample_gauss(space) |
| 205 | fun.evaluate(x0, fdata) |
| 206 | fun.computeJacobians(x0, fdata) |
| 207 | fun_fd.evaluate(x0, u0, fdata2) |
| 208 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 209 | assert np.allclose(fdata.Jx, fdata2.Jx, atol=ATOL) |
| 210 | |
| 211 | |
| 212 | def test_frame_velocity(): |
nothing calls this directly
no test coverage detected