| 34 | } |
| 35 | |
| 36 | void addSystemCollisionPairs(const Model & model, GeometryModel & geom_model, const Eigen::VectorXd & qref) |
| 37 | { |
| 38 | Data data(model); |
| 39 | GeometryData geom_data(geom_model); |
| 40 | // TI this function to gain compilation speed on this test |
| 41 | ::pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, qref); |
| 42 | geom_model.removeAllCollisionPairs(); |
| 43 | for (std::size_t i = 0; i < geom_model.geometryObjects.size(); ++i) |
| 44 | { |
| 45 | for (std::size_t j = i; j < geom_model.geometryObjects.size(); ++j) |
| 46 | { |
| 47 | if (i == j) |
| 48 | { |
| 49 | continue; // don't add collision pair if same object |
| 50 | } |
| 51 | const GeometryObject & gobj_i = geom_model.geometryObjects[i]; |
| 52 | const GeometryObject & gobj_j = geom_model.geometryObjects[j]; |
| 53 | if (gobj_i.name == "floor" || gobj_j.name == "floor") |
| 54 | { // if floor, always add a collision pair |
| 55 | geom_model.addCollisionPair(CollisionPair(i, j)); |
| 56 | } |
| 57 | else |
| 58 | { |
| 59 | if (gobj_i.parentJoint == gobj_j.parentJoint) |
| 60 | { // don't add collision pair if same parent |
| 61 | continue; |
| 62 | } |
| 63 | |
| 64 | // run collision detection -- add collision pair if shapes are not colliding |
| 65 | const SE3 M1 = geom_data.oMg[i]; |
| 66 | const SE3 M2 = geom_data.oMg[j]; |
| 67 | |
| 68 | hpp::fcl::CollisionRequest colreq; |
| 69 | colreq.security_margin = 1e-2; // 1cm of clearance |
| 70 | hpp::fcl::CollisionResult colres; |
| 71 | hpp::fcl::collide( |
| 72 | gobj_i.geometry.get(), ::pinocchio::toFclTransform3f(M1), // |
| 73 | gobj_j.geometry.get(), ::pinocchio::toFclTransform3f(M2), // |
| 74 | colreq, colres); |
| 75 | if (!colres.isCollision()) |
| 76 | { |
| 77 | geom_model.addCollisionPair(CollisionPair(i, j)); |
| 78 | } |
| 79 | } |
| 80 | } |
| 81 | } |
| 82 | } |
| 83 | |
| 84 | struct MujocoHumanoidFixture : benchmark::Fixture |
| 85 | { |