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