()
| 476 | if has_erd: |
| 477 | |
| 478 | def setSolo(): |
| 479 | # robot model |
| 480 | robot = erd.load("solo12") |
| 481 | model = robot.model.copy() |
| 482 | model.qref = np.array( |
| 483 | [ |
| 484 | 0.09906518, |
| 485 | 0.20099078, |
| 486 | 0.32502457, |
| 487 | 0.19414175, |
| 488 | -0.00524735, |
| 489 | -0.97855773, |
| 490 | 0.06860185, |
| 491 | 0.00968163, |
| 492 | 0.60963582, |
| 493 | -1.61206407, |
| 494 | -0.02543309, |
| 495 | 0.66709088, |
| 496 | -1.50870083, |
| 497 | 0.32405118, |
| 498 | -1.15305599, |
| 499 | 1.56867351, |
| 500 | -0.39097222, |
| 501 | -1.29675892, |
| 502 | 1.39741073, |
| 503 | ] |
| 504 | ) |
| 505 | model.qref = pin.normalize(model, model.qref) |
| 506 | |
| 507 | # Create height field ground |
| 508 | def ground(xy): |
| 509 | return ( |
| 510 | np.sin(xy[0] * 3) / 5 |
| 511 | + np.cos(xy[1] ** 2 * 3) / 20 |
| 512 | + np.sin(xy[1] * xy[0] * 5) / 10 |
| 513 | ) |
| 514 | |
| 515 | xg = np.arange(-2, 2, 0.02) |
| 516 | nx = xg.shape[0] |
| 517 | xy_g = np.meshgrid(xg, xg) |
| 518 | xy_g = np.stack(xy_g) |
| 519 | elev_g = np.zeros((nx, nx)) |
| 520 | elev_g[:, :] = ground(xy_g) |
| 521 | sx = xg[-1] - xg[0] |
| 522 | sy = xg[-1] - xg[0] |
| 523 | elev_g[:, :] = elev_g[::-1, :] |
| 524 | |
| 525 | # Create geometry model |
| 526 | geom_model = robot.collision_model |
| 527 | hfield = HeightFieldAABB(sx, sy, elev_g, np.min(elev_g)) |
| 528 | Mhfield = pin.SE3.Identity() |
| 529 | ground = pin.GeometryObject("ground", 0, Mhfield, hfield) |
| 530 | ground_id = geom_model.addGeometryObject(ground) |
| 531 | |
| 532 | # Add collision pairs between the ground and the robot |
| 533 | geom_model.removeAllCollisionPairs() |
| 534 | |
| 535 | a = 0.01910275 |
no test coverage detected