()
| 184 | |
| 185 | |
| 186 | def test_inv_dyn(): |
| 187 | model, rcm = createFourBarLinkages() |
| 188 | data = model.createData() |
| 189 | rcd = rcm.createData() |
| 190 | q0 = model.q_init.copy() |
| 191 | v0 = np.zeros(model.nv) |
| 192 | nu = model.nv |
| 193 | B = np.eye(nu) |
| 194 | tau0, _fc = aligator.underactuatedConstrainedInverseDynamics( |
| 195 | model, data, q0, v0, B, [rcm], [rcd] |
| 196 | ) |
| 197 | |
| 198 | # test the resulting acceleration is zero |
| 199 | if ALIGATOR_PINOCCHIO_V4: |
| 200 | pin.initConstraintDynamics(model, data, [rcm], [rcd]) |
| 201 | else: |
| 202 | pin.initConstraintDynamics(model, data, [rcm]) |
| 203 | prox = pin.ProximalSettings(1e-12, 1e-10, 30) |
| 204 | a0 = pin.constraintDynamics(model, data, q0, v0, tau0, [rcm], [rcd], prox) |
| 205 | assert np.allclose(a0, 0.0) |
| 206 | |
| 207 | |
| 208 | def test_constrained_dynamics(): |
nothing calls this directly
no test coverage detected