Add the right collision pairs of a model, given qref. qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered to always be in coll
(model, geom_model, qref)
| 160 | |
| 161 | |
| 162 | def addSystemCollisionPairs(model, geom_model, qref): |
| 163 | """ |
| 164 | Add the right collision pairs of a model, given qref. |
| 165 | qref is here as a `T-pose`. The function uses this pose to determine which objects are in collision |
| 166 | in this ref pose. If objects are in collision, they are not added as collision pairs, as they are considered |
| 167 | to always be in collision. |
| 168 | """ |
| 169 | data = model.createData() |
| 170 | geom_data = geom_model.createData() |
| 171 | pin.updateGeometryPlacements(model, data, geom_model, geom_data, qref) |
| 172 | geom_model.removeAllCollisionPairs() |
| 173 | num_col_pairs = 0 |
| 174 | for i in range(len(geom_model.geometryObjects)): |
| 175 | for j in range(i + 1, len(geom_model.geometryObjects)): |
| 176 | # Don't add collision pair if same object |
| 177 | if i != j: |
| 178 | gobj_i: pin.GeometryObject = geom_model.geometryObjects[i] |
| 179 | gobj_j: pin.GeometryObject = geom_model.geometryObjects[j] |
| 180 | if gobj_i.name == "floor" or gobj_j.name == "floor": |
| 181 | num_col_pairs += 1 |
| 182 | col_pair = pin.CollisionPair(i, j) |
| 183 | geom_model.addCollisionPair(col_pair) |
| 184 | else: |
| 185 | if gobj_i.parentJoint != gobj_j.parentJoint: |
| 186 | # Compute collision between the geometries. Only add the collision pair if there is no collision. |
| 187 | M1 = geom_data.oMg[i] |
| 188 | M2 = geom_data.oMg[j] |
| 189 | colreq = hppfcl.CollisionRequest() |
| 190 | colreq.security_margin = 1e-2 # 1cm of clearance |
| 191 | colres = hppfcl.CollisionResult() |
| 192 | hppfcl.collide( |
| 193 | gobj_i.geometry, M1, gobj_j.geometry, M2, colreq, colres |
| 194 | ) |
| 195 | if not colres.isCollision(): |
| 196 | num_col_pairs += 1 |
| 197 | col_pair = pin.CollisionPair(i, j) |
| 198 | geom_model.addCollisionPair(col_pair) |
| 199 | |
| 200 | |
| 201 | def addFloor(geom_model: pin.GeometryModel, visual_model: pin.GeometryModel): |
no outgoing calls
no test coverage detected