()
| 117 | |
| 118 | |
| 119 | def test_frame_placement(): |
| 120 | fr_name1 = "larm_shoulder2_body" |
| 121 | fr_id1 = model.getFrameId(fr_name1) |
| 122 | |
| 123 | space = manifolds.MultibodyConfiguration(model) |
| 124 | ndx = space.ndx |
| 125 | x0 = space.neutral() |
| 126 | d = np.random.randn(space.ndx) * 0.1 |
| 127 | d[6:] = 0.0 |
| 128 | x0 = space.integrate(x0, d) |
| 129 | u0 = np.zeros(nu) |
| 130 | q0 = x0[:nq] |
| 131 | |
| 132 | pin.framesForwardKinematics(model, rdata, q0) |
| 133 | fr_plc1 = rdata.oMf[fr_id1] |
| 134 | |
| 135 | fun = aligator.FramePlacementResidual(ndx, nu, model, fr_plc1, fr_id1) |
| 136 | assert fr_id1 == fun.frame_id |
| 137 | assert fr_plc1 == fun.getReference() |
| 138 | |
| 139 | fdata = fun.createData() |
| 140 | fun.evaluate(x0, fdata) |
| 141 | |
| 142 | assert np.allclose(fdata.value, 0.0) |
| 143 | |
| 144 | fun.computeJacobians(x0, fdata) |
| 145 | J = fdata.Jx[:, :nv] |
| 146 | |
| 147 | pin.computeJointJacobians(model, rdata) |
| 148 | realJ = pin.getFrameJacobian(model, rdata, fr_id1, pin.LOCAL) |
| 149 | assert J.shape == realJ.shape |
| 150 | assert np.allclose(fdata.Jx[:, :nv], realJ) |
| 151 | |
| 152 | fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS) |
| 153 | fdata2 = fun_fd.createData() |
| 154 | fun_fd.evaluate(x0, u0, fdata2) |
| 155 | assert np.allclose(fdata.value, fdata2.value) |
| 156 | |
| 157 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 158 | J_fd = fdata2.Jx[:] |
| 159 | assert fdata.Jx.shape == J_fd.shape |
| 160 | |
| 161 | for i in range(100): |
| 162 | x0 = sample_gauss(space) |
| 163 | fun.evaluate(x0, u0, fdata) |
| 164 | fun.computeJacobians(x0, u0, fdata) |
| 165 | fun_fd.evaluate(x0, u0, fdata2) |
| 166 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 167 | assert np.allclose(fdata.Jx, fdata2.Jx, ATOL) |
| 168 | |
| 169 | |
| 170 | def test_frame_translation(): |
nothing calls this directly
no test coverage detected