()
| 143 | |
| 144 | |
| 145 | def test_acceleration(): |
| 146 | x, d, x0 = sample_gauss(space) |
| 147 | force_size = 3 |
| 148 | nu = force_size * nk |
| 149 | u0 = np.random.randn(nu) |
| 150 | |
| 151 | contact_names = ["c1", "c2", "c3", "c4"] |
| 152 | contact_states = [True, False, True, True] |
| 153 | contact_poses = [ |
| 154 | np.array([0.2, 0.1, 0.0]), |
| 155 | np.array([0.2, 0.0, 0.0]), |
| 156 | np.array([0.0, 0.1, 0.0]), |
| 157 | np.array([0.0, 0.0, 0]), |
| 158 | ] |
| 159 | contact_map = aligator.ContactMap(contact_names, contact_states, contact_poses) |
| 160 | |
| 161 | fun = aligator.CentroidalAccelerationResidual( |
| 162 | ndx, nu, mass, gravity, contact_map, force_size |
| 163 | ) |
| 164 | fdata = fun.createData() |
| 165 | fun.evaluate(x0, u0, fdata) |
| 166 | |
| 167 | comddot = np.zeros(3) |
| 168 | for i in range(nk): |
| 169 | if fun.contact_map.contact_states[i]: |
| 170 | comddot += u0[i * force_size : i * force_size + 3] |
| 171 | |
| 172 | comddot /= mass |
| 173 | comddot += gravity |
| 174 | |
| 175 | assert np.allclose(fdata.value, comddot) |
| 176 | |
| 177 | fun_fd = aligator.FiniteDifferenceHelper(space, fun, FD_EPS) |
| 178 | fdata2 = fun_fd.createData() |
| 179 | fun_fd.evaluate(x0, u0, fdata2) |
| 180 | assert np.allclose(fdata.value, fdata2.value) |
| 181 | |
| 182 | fun_fd.computeJacobians(x0, u0, fdata2) |
| 183 | J_fd = fdata2.Jx |
| 184 | J_fd_u = fdata2.Ju |
| 185 | assert fdata.Jx.shape == J_fd.shape |
| 186 | assert fdata.Ju.shape == J_fd_u.shape |
| 187 | |
| 188 | for i in range(100): |
| 189 | du = np.random.randn(nu) * 0.1 |
| 190 | u1 = u0 + du |
| 191 | fun.evaluate(x0, u1, fdata) |
| 192 | fun.computeJacobians(x0, u1, fdata) |
| 193 | fun_fd.evaluate(x0, u1, fdata2) |
| 194 | fun_fd.computeJacobians(x0, u1, fdata2) |
| 195 | assert np.allclose(fdata.Ju, fdata2.Ju, THRESH) |
| 196 | |
| 197 | force_size = 6 |
| 198 | nu = force_size * nk |
| 199 | u0 = np.random.randn(nu) |
| 200 | |
| 201 | fun = aligator.CentroidalAccelerationResidual( |
| 202 | ndx, nu, mass, gravity, contact_map, force_size |
nothing calls this directly
no test coverage detected