Coverage for trimesh/collision.py: 85%

1import collections

3import numpy as np

5try:

6 # pip install python-fcl

7 import fcl

8except BaseException:

9 fcl = None

12# contact cap per pair

13COLLISION_PER_PAIR_CAP = 100000

16def _fcl_collide_callback(o1, o2, cdata):

17 # unlike fcl.defaultCollisionCallback never halt traversal

18 # early that would silently drop later colliding pairs

19 fcl.collide(o1=o1, o2=o2, request=cdata.request, result=cdata.result)

20 return False

23def _fcl_collision_data(return_names, return_data):

24 # build (cdata, callback) sized for what the caller actually needs

25 if not (return_names or return_data):

26 return fcl.CollisionData(), fcl.defaultCollisionCallback

27 # one contact identifies a pair only ask for more if the

28 # caller wants the contact data itself

29 request = fcl.CollisionRequest(

30 num_max_contacts=COLLISION_PER_PAIR_CAP if return_data else 1,

31 enable_contact=True,

32 )

33 return fcl.CollisionData(request=request), _fcl_collide_callback

36class ContactData:

37 """

38 Data structure for holding information about a collision contact.

39 """

41 def __init__(self, names, contact):

42 """

43 Initialize a ContactData.

45 Parameters

46 ----------

47 names : list of str

48 The names of the two objects in order.

49 contact : fcl.Contact

50 The contact in question.

51 """

52 self.names = set(names)

53 self._inds = {names[0]: contact.b1, names[1]: contact.b2}

54 self._normal = contact.normal

55 self._point = contact.pos

56 self._depth = contact.penetration_depth

58 @property

59 def normal(self):

60 """

61 The 3D intersection normal for this contact.

63 Returns

64 -------

65 normal : (3,) float

66 The intersection normal.

67 """

68 return self._normal

70 @property

71 def point(self):

72 """

73 The 3D point of intersection for this contact.

75 Returns

76 -------

77 point : (3,) float

78 The intersection point.

79 """

80 return self._point

82 @property

83 def depth(self):

84 """

85 The penetration depth of the 3D point of intersection for this contact.

87 Returns

88 -------

89 depth : float

90 The penetration depth.

91 """

92 return self._depth

94 def index(self, name):

95 """

96 Returns the index of the face in contact for the mesh with

97 the given name.

99 Parameters

100 ----------

101 name : str

102 The name of the target object.

103

104 Returns

105 -------

106 index : int

107 The index of the face in collision

108 """

109 return self._inds[name]

110

111

112class DistanceData:

113 """

114 Data structure for holding information about a distance query.

115 """

116

117 def __init__(self, names, result):

118 """

119 Initialize a DistanceData.

120

121 Parameters

122 ----------

123 names : list of str

124 The names of the two objects in order.

125 contact : fcl.DistanceResult

126 The distance query result.

127 """

128 self.names = set(names)

129 self._inds = {names[0]: result.b1, names[1]: result.b2}

130 self._points = {

131 names[0]: result.nearest_points[0],

132 names[1]: result.nearest_points[1],

133 }

134 self._distance = result.min_distance

135

136 @property

137 def distance(self):

138 """

139 Returns the distance between the two objects.

140

141 Returns

142 -------

143 distance : float

144 The euclidean distance between the objects.

145 """

146 return self._distance

147

148 def index(self, name):

149 """

150 Returns the index of the closest face for the mesh with

151 the given name.

152

153 Parameters

154 ----------

155 name : str

156 The name of the target object.

157

158 Returns

159 -------

160 index : int

161 The index of the face in collisoin.

162 """

163 return self._inds[name]

164

165 def point(self, name):

166 """

167 The 3D point of closest distance on the mesh with the given name.

168

169 Parameters

170 ----------

171 name : str

172 The name of the target object.

173

174 Returns

175 -------

176 point : (3,) float

177 The closest point.

178 """

179 return self._points[name]

180

181

182class CollisionManager:

183 """

184 A mesh-mesh collision manager.

185 """

186

187 def __init__(self):

188 """

189 Initialize a mesh-mesh collision manager.

190 """

191 if fcl is None:

192 raise ValueError("No FCL Available! Please install the python-fcl library")

193 # {name: {geom:, obj}}

194 self._objs = {}

195 # {id(bvh) : str, name}

196 # unpopulated values will return None

197 self._names = collections.defaultdict(lambda: None)

198

199 self._manager = fcl.DynamicAABBTreeCollisionManager()

200 self._manager.setup()

201

202 def add_object(self, name, mesh, transform=None):

203 """

204 Add an object to the collision manager.

205

206 If an object with the given name is already in the manager,

207 replace it.

208

209 Parameters

210 ----------

211 name : str

212 An identifier for the object

213 mesh : Trimesh object

214 The geometry of the collision object

215 transform : (4,4) float

216 Homogeneous transform matrix for the object

217 """

218

219 # if no transform passed, assume identity transform

220 if transform is None:

221 transform = np.eye(4)

222 transform = np.asanyarray(transform, dtype=np.float32)

223 if transform.shape != (4, 4):

224 raise ValueError("transform must be (4,4)!")

225

226 # create BVH/Convex

227 geom = self._get_fcl_obj(mesh)

228

229 # create the FCL transform from (4,4) matrix

230 t = fcl.Transform(transform[:3, :3], transform[:3, 3])

231 o = fcl.CollisionObject(geom, t)

232

233 # Add collision object to set

234 if name in self._objs:

235 self._manager.unregisterObject(self._objs[name])

236 self._objs[name] = {"obj": o, "geom": geom}

237 # store the name of the geometry

238 self._names[id(geom)] = name

239

240 self._manager.registerObject(o)

241 self._manager.update()

242 return o

243

244 def remove_object(self, name):

245 """

246 Delete an object from the collision manager.

247

248 Parameters

249 ----------

250 name : str

251 The identifier for the object

252 """

253 if name in self._objs:

254 self._manager.unregisterObject(self._objs[name]["obj"])

255 self._manager.update(self._objs[name]["obj"])

256 # remove objects from _objs

257 geom_id = id(self._objs.pop(name)["geom"])

258 # remove names

259 self._names.pop(geom_id)

260 else:

261 raise ValueError(f"{name} not in collision manager!")

262

263 def set_transform(self, name, transform):

264 """

265 Set the transform for one of the manager's objects.

266 This replaces the prior transform.

267

268 Parameters

269 ----------

270 name : str

271 An identifier for the object already in the manager

272 transform : (4,4) float

273 A new homogeneous transform matrix for the object

274 """

275 if name in self._objs:

276 o = self._objs[name]["obj"]

277 o.setRotation(transform[:3, :3])

278 o.setTranslation(transform[:3, 3])

279 self._manager.update(o)

280 else:

281 raise ValueError(f"{name} not in collision manager!")

282

283 def in_collision_single(

284 self, mesh, transform=None, return_names=False, return_data=False

285 ):

286 """

287 Check a single object for collisions against all objects in the

288 manager.

289

290 Parameters

291 ----------

292 mesh : Trimesh object

293 The geometry of the collision object

294 transform : (4,4) float

295 Homogeneous transform matrix

296 return_names : bool

297 If true, a set is returned containing the names

298 of all objects in collision with the object

299 return_data : bool

300 If true, a list of ContactData is returned as well

301

302 Returns

303 ------------

304 is_collision : bool

305 True if a collision occurs and False otherwise

306 names : set of str

307 [OPTIONAL] The set of names of objects that collided with the

308 provided one

309 contacts : list of ContactData

310 [OPTIONAL] All contacts detected

311 """

312 if transform is None:

313 transform = np.eye(4)

314

315 # create BVH/Convex

316 geom = self._get_fcl_obj(mesh)

317

318 # create the FCL transform from (4,4) matrix

319 t = fcl.Transform(transform[:3, :3], transform[:3, 3])

320 o = fcl.CollisionObject(geom, t)

321

322 cdata, callback = _fcl_collision_data(return_names, return_data)

323 self._manager.collide(o, cdata, callback)

324 result = cdata.result.is_collision

325

326 # If we want to return the objects that were collision, collect them.

327 objs_in_collision = set()

328 contact_data = []

329 if return_names or return_data:

330 for contact in cdata.result.contacts:

331 cg = contact.o1

332 if cg == geom:

333 cg = contact.o2

334 name = self._extract_name(cg)

335

336 names = (name, "__external")

337 if cg == contact.o2:

338 names = tuple(reversed(names))

339

340 if return_names:

341 objs_in_collision.add(name)

342 if return_data:

343 contact_data.append(ContactData(names, contact))

344

345 if return_names and return_data:

346 return result, objs_in_collision, contact_data

347 elif return_names:

348 return result, objs_in_collision

349 elif return_data:

350 return result, contact_data

351 else:

352 return result

353

354 def in_collision_internal(self, return_names=False, return_data=False):

355 """

356 Check if any pair of objects in the manager collide with one another.

357

358 Parameters

359 ----------

360 return_names : bool

361 If true, a set is returned containing the names

362 of all pairs of objects in collision.

363 return_data : bool

364 If true, a list of ContactData is returned as well

365

366 Returns

367 -------

368 is_collision : bool

369 True if a collision occurred between any pair of objects

370 and False otherwise

371 names : set of 2-tup

372 The set of pairwise collisions. Each tuple

373 contains two names in alphabetical order indicating

374 that the two corresponding objects are in collision.

375 contacts : list of ContactData

376 All contacts detected

377 """

378 cdata, callback = _fcl_collision_data(return_names, return_data)

379 self._manager.collide(cdata, callback)

380

381 result = cdata.result.is_collision

382

383 objs_in_collision = set()

384 contact_data = []

385 if return_names or return_data:

386 for contact in cdata.result.contacts:

387 names = (self._extract_name(contact.o1), self._extract_name(contact.o2))

388

389 if return_names:

390 objs_in_collision.add(tuple(sorted(names)))

391 if return_data:

392 contact_data.append(ContactData(names, contact))

393

394 if return_names and return_data:

395 return result, objs_in_collision, contact_data

396 elif return_names:

397 return result, objs_in_collision

398 elif return_data:

399 return result, contact_data

400 else:

401 return result

402

403 def in_collision_other(self, other_manager, return_names=False, return_data=False):

404 """

405 Check if any object from this manager collides with any object

406 from another manager.

407

408 Parameters

409 -------------------

410 other_manager : CollisionManager

411 Another collision manager object

412 return_names : bool

413 If true, a set is returned containing the names

414 of all pairs of objects in collision.

415 return_data : bool

416 If true, a list of ContactData is returned as well

417

418 Returns

419 -------------

420 is_collision : bool

421 True if a collision occurred between any pair of objects

422 and False otherwise

423 names : set of 2-tup

424 The set of pairwise collisions. Each tuple

425 contains two names (first from this manager,

426 second from the other_manager) indicating

427 that the two corresponding objects are in collision.

428 contacts : list of ContactData

429 All contacts detected

430 """

431 cdata, callback = _fcl_collision_data(return_names, return_data)

432 self._manager.collide(other_manager._manager, cdata, callback)

433 result = cdata.result.is_collision

434

435 objs_in_collision = set()

436 contact_data = []

437 if return_names or return_data:

438 for contact in cdata.result.contacts:

439 reverse = False

440 names = (

441 self._extract_name(contact.o1),

442 other_manager._extract_name(contact.o2),

443 )

444 if names[0] is None:

445 names = (

446 self._extract_name(contact.o2),

447 other_manager._extract_name(contact.o1),

448 )

449 reverse = True

450

451 if return_names:

452 objs_in_collision.add(names)

453 if return_data:

454 if reverse:

455 names = tuple(reversed(names))

456 contact_data.append(ContactData(names, contact))

457

458 if return_names and return_data:

459 return result, objs_in_collision, contact_data

460 elif return_names:

461 return result, objs_in_collision

462 elif return_data:

463 return result, contact_data

464 else:

465 return result

466

467 def min_distance_single(

468 self, mesh, transform=None, return_name=False, return_data=False

469 ):

470 """

471 Get the minimum distance between a single object and any

472 object in the manager.

473

474 Parameters

475 ---------------

476 mesh : Trimesh object

477 The geometry of the collision object

478 transform : (4,4) float

479 Homogeneous transform matrix for the object

480 return_names : bool

481 If true, return name of the closest object

482 return_data : bool

483 If true, a DistanceData object is returned as well

484

485 Returns

486 -------------

487 distance : float

488 Min distance between mesh and any object in the manager

489 name : str

490 The name of the object in the manager that was closest

491 data : DistanceData

492 Extra data about the distance query

493 """

494 if transform is None:

495 transform = np.eye(4)

496

497 # create BVH/Convex

498 geom = self._get_fcl_obj(mesh)

499

500 # create the FCL transform from (4,4) matrix

501 t = fcl.Transform(transform[:3, :3], transform[:3, 3])

502 o = fcl.CollisionObject(geom, t)

503

504 # Collide with manager's objects

505 ddata = fcl.DistanceData(fcl.DistanceRequest(enable_signed_distance=True))

506 if return_data:

507 ddata = fcl.DistanceData(

508 fcl.DistanceRequest(

509 enable_nearest_points=True, enable_signed_distance=True

510 ),

511 fcl.DistanceResult(),

512 )

513

514 self._manager.distance(o, ddata, fcl.defaultDistanceCallback)

515

516 distance = ddata.result.min_distance

517

518 # If we want to return the objects that were collision, collect them.

519 name, data = None, None

520 if return_name or return_data:

521 cg = ddata.result.o1

522 if cg == geom:

523 cg = ddata.result.o2

524

525 name = self._extract_name(cg)

526

527 names = (name, "__external")

528 if cg == ddata.result.o2:

529 names = tuple(reversed(names))

530 data = DistanceData(names, ddata.result)

531

532 if return_name and return_data:

533 return distance, name, data

534 elif return_name:

535 return distance, name

536 elif return_data:

537 return distance, data

538 else:

539 return distance

540

541 def min_distance_internal(self, name=None, return_names=False, return_data=False):

542 """

543 Get the minimum distance between objects in the manager.

544

545 If name is provided, computes the minimum distance between the

546 specified object and any other object in the manager.

547 If name is None, computes the minimum distance between any pair

548 of objects in the manager.

549

550 Parameters

551 -------------

552 name : str or None

553 If provided, the identifier for the object already in the manager

554 to compute distances from. If None, computes distances between

555 all pairs of objects.

556 return_names : bool

557 If true, a 2-tuple is returned containing the names

558 of the closest objects.

559 return_data : bool

560 If true, a DistanceData object is returned as well

561

562 Returns

563 -----------

564 distance : float

565 Min distance between objects

566 names : (2,) str

567 The names of the closest objects

568 data : DistanceData

569 Extra data about the distance query

570 """

571 ddata = fcl.DistanceData(fcl.DistanceRequest(enable_signed_distance=True))

572 if return_data:

573 ddata = fcl.DistanceData(

574 fcl.DistanceRequest(

575 enable_nearest_points=True,

576 enable_signed_distance=True,

577 ),

578 fcl.DistanceResult(),

579 )

580

581 # If name is provided, compute distance from that object to others

582 if name is not None:

583 if name not in self._objs:

584 raise ValueError(f"{name} not in collision manager!")

585 obj = self._objs[name]["obj"]

586 # remove object from manager temporarily

587 self._manager.unregisterObject(obj)

588 self._manager.update(obj)

589

590 # compute distance

591 self._manager.distance(obj, ddata, fcl.defaultDistanceCallback)

592

593 # add it back to the manager

594 self._manager.registerObject(obj)

595 self._manager.update()

596

597 else:

598 # Compute distance between any pair of objects

599 self._manager.distance(ddata, fcl.defaultDistanceCallback)

600

601 distance = ddata.result.min_distance

602

603 names, data = None, None

604 if return_names or return_data:

605 names = (

606 self._extract_name(ddata.result.o1),

607 self._extract_name(ddata.result.o2),

608 )

609 data = DistanceData(names, ddata.result)

610 names = tuple(sorted(names))

611

612 if return_names and return_data:

613 return distance, names, data

614 elif return_names:

615 return distance, names

616 elif return_data:

617 return distance, data

618 else:

619 return distance

620

621 def min_distance_other(self, other_manager, return_names=False, return_data=False):

622 """

623 Get the minimum distance between any pair of objects,

624 one in each manager.

625

626 Parameters

627 ----------

628 other_manager : CollisionManager

629 Another collision manager object

630 return_names : bool

631 If true, a 2-tuple is returned containing

632 the names of the closest objects.

633 return_data : bool

634 If true, a DistanceData object is returned as well

635

636 Returns

637 -----------

638 distance : float

639 The min distance between a pair of objects,

640 one from each manager.

641 names : 2-tup of str

642 A 2-tuple containing two names (first from this manager,

643 second from the other_manager) indicating

644 the two closest objects.

645 data : DistanceData

646 Extra data about the distance query

647 """

648 ddata = fcl.DistanceData(fcl.DistanceRequest(enable_signed_distance=True))

649 if return_data:

650 ddata = fcl.DistanceData(

651 fcl.DistanceRequest(

652 enable_nearest_points=True,

653 enable_signed_distance=True,

654 ),

655 fcl.DistanceResult(),

656 )

657

658 self._manager.distance(other_manager._manager, ddata, fcl.defaultDistanceCallback)

659

660 distance = ddata.result.min_distance

661

662 names, data = None, None

663 if return_names or return_data:

664 reverse = False

665 names = (

666 self._extract_name(ddata.result.o1),

667 other_manager._extract_name(ddata.result.o2),

668 )

669 if names[0] is None:

670 reverse = True

671 names = (

672 self._extract_name(ddata.result.o2),

673 other_manager._extract_name(ddata.result.o1),

674 )

675

676 dnames = tuple(names)

677 if reverse:

678 dnames = tuple(reversed(dnames))

679 data = DistanceData(dnames, ddata.result)

680

681 if return_names and return_data:

682 return distance, names, data

683 elif return_names:

684 return distance, names

685 elif return_data:

686 return distance, data

687 else:

688 return distance

689

690 def _get_fcl_obj(self, mesh):

691 """

692 Get a BVH or Convex for a mesh.

693

694 Parameters

695 -------------

696 mesh : Trimesh

697 Mesh to create BVH/Convex for

698

699 Returns

700 --------------

701 obj : fcl.BVHModel or fcl.Convex

702 BVH/Convex object of source mesh

703 """

704

705 if mesh.is_convex:

706 obj = mesh_to_convex(mesh)

707 else:

708 obj = mesh_to_BVH(mesh)

709 return obj

710

711 def _extract_name(self, geom):

712 """

713 Retrieve the name of an object from the manager by its

714 CollisionObject, or return None if not found.

715

716 Parameters

717 -----------

718 geom : CollisionObject or BVHModel

719 Input model

720

721 Returns

722 ------------

723 names : hashable

724 Name of input geometry

725 """

726 return self._names[id(geom)]

727

728

729def mesh_to_BVH(mesh):

730 """

731 Create a BVHModel object from a Trimesh object

732

733 Parameters

734 -----------

735 mesh : Trimesh

736 Input geometry

737

738 Returns

739 ------------

740 bvh : fcl.BVHModel

741 BVH of input geometry

742 """

743 bvh = fcl.BVHModel()

744 bvh.beginModel(num_tris_=len(mesh.faces), num_vertices_=len(mesh.vertices))

745 bvh.addSubModel(verts=mesh.vertices, triangles=mesh.faces)

746 bvh.endModel()

747 return bvh

748

749

750def mesh_to_convex(mesh):

751 """

752 Create a Convex object from a Trimesh object

753

754 Parameters

755 -----------

756 mesh : Trimesh

757 Input geometry

758

759 Returns

760 ------------

761 convex : fcl.Convex

762 Convex of input geometry

763 """

764 fs = np.concatenate(

765 (3 * np.ones((len(mesh.faces), 1), dtype=np.int64), mesh.faces), axis=1

766 )

767 return fcl.Convex(mesh.vertices, len(fs), fs.flatten())

768

769

770def scene_to_collision(scene):

771 """

772 Create collision objects from a trimesh.Scene object.

773

774 Parameters

775 ------------

776 scene : trimesh.Scene

777 Scene to create collision objects for

778

779 Returns

780 ------------

781 manager : CollisionManager

782 CollisionManager for objects in scene

783 objects: {node name: CollisionObject}

784 Collision objects for nodes in scene

785 """

786 manager = CollisionManager()

787 objects = {}

788 for node in scene.graph.nodes_geometry:

789 T, geometry = scene.graph[node]

790 objects[node] = manager.add_object(

791 name=node, mesh=scene.geometry[geometry], transform=T

792 )

793 return manager, objects