()
| 346 | |
| 347 | |
| 348 | def test_composite_limits(): |
| 349 | model = pin.Model() |
| 350 | geom_model = pin.GeometryModel() |
| 351 | a = 0.1 |
| 352 | m = 3.8 |
| 353 | composite = pin.JointModelComposite(2) |
| 354 | composite.addJoint(pin.JointModelRX()) |
| 355 | composite.addJoint(pin.JointModelRY()) |
| 356 | joint = model.addJoint(0, composite, pin.SE3.Identity(), "ball") |
| 357 | model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity()) |
| 358 | ball_shape = Sphere(a / 2) |
| 359 | geom_ball = pin.GeometryObject("ball", joint, joint, pin.SE3.Identity(), ball_shape) |
| 360 | geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) |
| 361 | ball_id = geom_model.addGeometryObject(geom_ball) |
| 362 | q0 = pin.neutral(model) |
| 363 | model.lowerPositionLimit = q0.copy() |
| 364 | model.upperPositionLimit = q0.copy() |
| 365 | sim = simple.Simulator(model, geom_model) |
| 366 | v = np.zeros(model.nv) |
| 367 | tau = np.zeros(model.nv) |
| 368 | dt = 1e-3 |
| 369 | sim.step(q0, v, tau, dt) |
| 370 | assert sim.constraints_problem.getNumberOfContacts() == 0 |
| 371 | assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq |
| 372 | assert np.linalg.norm(sim.vnew) < 1e-6 |
| 373 | model = pin.Model() |
| 374 | geom_model = pin.GeometryModel() |
| 375 | a = 0.1 |
| 376 | m = 3.8 |
| 377 | composite = pin.JointModelComposite(2) |
| 378 | composite.addJoint(pin.JointModelPX()) |
| 379 | composite.addJoint(pin.JointModelPY()) |
| 380 | joint = model.addJoint(0, composite, pin.SE3.Identity(), "ball") |
| 381 | model.appendBodyToJoint(joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity()) |
| 382 | ball_shape = Sphere(a / 2) |
| 383 | geom_ball = pin.GeometryObject("ball", joint, joint, pin.SE3.Identity(), ball_shape) |
| 384 | geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) |
| 385 | ball_id = geom_model.addGeometryObject(geom_ball) |
| 386 | q0 = pin.neutral(model) |
| 387 | model.lowerPositionLimit = q0.copy() |
| 388 | model.upperPositionLimit = q0.copy() |
| 389 | sim = simple.Simulator(model, geom_model) |
| 390 | v = np.zeros(model.nv) |
| 391 | tau = np.zeros(model.nv) |
| 392 | dt = 1e-3 |
| 393 | sim.step(q0, v, tau, dt) |
| 394 | assert sim.constraints_problem.getNumberOfContacts() == 0 |
| 395 | assert sim.constraints_problem.constraints_problem_size() == 2 * model.nq |
| 396 | assert np.linalg.norm(sim.vnew) < 1e-6 |
| 397 | |
| 398 | |
| 399 | def test_mujoco_humanoid_limits(): |
nothing calls this directly
no test coverage detected