()
| 159 | |
| 160 | |
| 161 | def test_step_balls(): |
| 162 | mass = [12.0] |
| 163 | length = [1.5] |
| 164 | model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass) |
| 165 | |
| 166 | q = pin.neutral(model) |
| 167 | q[2] = length[0] / 2.0 + 1.0 |
| 168 | v = np.zeros(model.nv) |
| 169 | tau = np.zeros(model.nv) |
| 170 | dt = 1e-3 |
| 171 | sim = simple.Simulator(model, data, geom_model, geom_data) |
| 172 | sim.step(q, v, tau, dt) |
| 173 | assert sim.constraints_problem.getNumberOfContacts() == 0 |
| 174 | |
| 175 | q[2] = 0.0 |
| 176 | v = np.zeros(model.nv) |
| 177 | sim.step(q, v, tau, dt) |
| 178 | assert sim.constraints_problem.getNumberOfContacts() == 1 |
| 179 | assert np.linalg.norm(sim.vnew) < 1e-6 |
| 180 | mass = [12.0, 0.1] |
| 181 | length = [1.5, 0.5] |
| 182 | model, data, geom_model, geom_data = setBallsAndHalfSpace(length, mass) |
| 183 | sim = simple.Simulator(model, data, geom_model, geom_data) |
| 184 | |
| 185 | q = pin.neutral(model) |
| 186 | q[0] = 0.0 |
| 187 | q[7] = length[1] / 2.0 + 1.0 |
| 188 | q[2] = 0.0 |
| 189 | q[9] = 0.0 |
| 190 | v = np.zeros(model.nv) |
| 191 | tau = np.zeros(model.nv) |
| 192 | sim.step(q, v, tau, dt) |
| 193 | assert np.linalg.norm(sim.vnew) < 1e-6 |
| 194 | num_contacts = sim.constraints_problem.getNumberOfContacts() |
| 195 | assert num_contacts == 2 |
| 196 | lam = sim.constraints_problem.frictional_point_constraints_forces()[ |
| 197 | : 3 * num_contacts |
| 198 | ] |
| 199 | assert len(lam) == 3 * num_contacts |
| 200 | sim.step(q, v, tau, dt) |
| 201 | assert True |
| 202 | |
| 203 | |
| 204 | def test_step_balls_with_constraints(): |
nothing calls this directly
no test coverage detected