Coverage for trimesh/collision.py: 85%
263 statements
« prev ^ index » next coverage.py v7.14.1, created at 2026-06-24 04:40 +0000
1import collections
3import numpy as np
5try:
6 # pip install python-fcl
7 import fcl
8except BaseException:
9 fcl = None
12# fcl caps result.contacts at num_max_contacts per fcl.collide() call.
13# with our custom callback that becomes a per-pair cap: for return_names
14# we only need to know each pair collided so 1 suffices, but for
15# return_data the caller wants the full contact list so the cap must be
16# bigger than the worst-case contact count for any single pair — picked
17# high enough to be effectively unlimited for realistic geometry
18COLLISION_PER_PAIR_CAP = 100000
21def _fcl_collide_callback(o1, o2, cdata):
22 # fcl.defaultCollisionCallback halts BVH traversal once cumulative
23 # contacts hit num_max_contacts — dropping any later colliding pairs
24 fcl.collide(o1=o1, o2=o2, request=cdata.request, result=cdata.result)
25 return False
28def _fcl_collision_data(return_names, return_data):
29 # build (cdata, callback) sized for what the caller actually needs
30 if not (return_names or return_data):
31 return fcl.CollisionData(), fcl.defaultCollisionCallback
32 # one contact per pair is enough to identify the pair — only ask
33 # fcl for many contacts if the caller wants the contact data itself
34 request = fcl.CollisionRequest(
35 num_max_contacts=COLLISION_PER_PAIR_CAP if return_data else 1,
36 enable_contact=True,
37 )
38 return fcl.CollisionData(request=request), _fcl_collide_callback
41class ContactData:
42 """
43 Data structure for holding information about a collision contact.
44 """
46 def __init__(self, names, contact):
47 """
48 Initialize a ContactData.
50 Parameters
51 ----------
52 names : list of str
53 The names of the two objects in order.
54 contact : fcl.Contact
55 The contact in question.
56 """
57 self.names = set(names)
58 self._inds = {names[0]: contact.b1, names[1]: contact.b2}
59 self._normal = contact.normal
60 self._point = contact.pos
61 self._depth = contact.penetration_depth
63 @property
64 def normal(self):
65 """
66 The 3D intersection normal for this contact.
68 Returns
69 -------
70 normal : (3,) float
71 The intersection normal.
72 """
73 return self._normal
75 @property
76 def point(self):
77 """
78 The 3D point of intersection for this contact.
80 Returns
81 -------
82 point : (3,) float
83 The intersection point.
84 """
85 return self._point
87 @property
88 def depth(self):
89 """
90 The penetration depth of the 3D point of intersection for this contact.
92 Returns
93 -------
94 depth : float
95 The penetration depth.
96 """
97 return self._depth
99 def index(self, name):
100 """
101 Returns the index of the face in contact for the mesh with
102 the given name.
104 Parameters
105 ----------
106 name : str
107 The name of the target object.
109 Returns
110 -------
111 index : int
112 The index of the face in collision
113 """
114 return self._inds[name]
117class DistanceData:
118 """
119 Data structure for holding information about a distance query.
120 """
122 def __init__(self, names, result):
123 """
124 Initialize a DistanceData.
126 Parameters
127 ----------
128 names : list of str
129 The names of the two objects in order.
130 contact : fcl.DistanceResult
131 The distance query result.
132 """
133 self.names = set(names)
134 self._inds = {names[0]: result.b1, names[1]: result.b2}
135 self._points = {
136 names[0]: result.nearest_points[0],
137 names[1]: result.nearest_points[1],
138 }
139 self._distance = result.min_distance
141 @property
142 def distance(self):
143 """
144 Returns the distance between the two objects.
146 Returns
147 -------
148 distance : float
149 The euclidean distance between the objects.
150 """
151 return self._distance
153 def index(self, name):
154 """
155 Returns the index of the closest face for the mesh with
156 the given name.
158 Parameters
159 ----------
160 name : str
161 The name of the target object.
163 Returns
164 -------
165 index : int
166 The index of the face in collisoin.
167 """
168 return self._inds[name]
170 def point(self, name):
171 """
172 The 3D point of closest distance on the mesh with the given name.
174 Parameters
175 ----------
176 name : str
177 The name of the target object.
179 Returns
180 -------
181 point : (3,) float
182 The closest point.
183 """
184 return self._points[name]
187class CollisionManager:
188 """
189 A mesh-mesh collision manager.
190 """
192 def __init__(self):
193 """
194 Initialize a mesh-mesh collision manager.
195 """
196 if fcl is None:
197 raise ValueError("No FCL Available! Please install the python-fcl library")
198 # {name: {geom:, obj}}
199 self._objs = {}
200 # {id(bvh) : str, name}
201 # unpopulated values will return None
202 self._names = collections.defaultdict(lambda: None)
204 self._manager = fcl.DynamicAABBTreeCollisionManager()
205 self._manager.setup()
207 def add_object(self, name, mesh, transform=None):
208 """
209 Add an object to the collision manager.
211 If an object with the given name is already in the manager,
212 replace it.
214 Parameters
215 ----------
216 name : str
217 An identifier for the object
218 mesh : Trimesh object
219 The geometry of the collision object
220 transform : (4,4) float
221 Homogeneous transform matrix for the object
222 """
224 # if no transform passed, assume identity transform
225 if transform is None:
226 transform = np.eye(4)
227 transform = np.asanyarray(transform, dtype=np.float32)
228 if transform.shape != (4, 4):
229 raise ValueError("transform must be (4,4)!")
231 # create BVH/Convex
232 geom = self._get_fcl_obj(mesh)
234 # create the FCL transform from (4,4) matrix
235 t = fcl.Transform(transform[:3, :3], transform[:3, 3])
236 o = fcl.CollisionObject(geom, t)
238 # Add collision object to set
239 if name in self._objs:
240 self._manager.unregisterObject(self._objs[name])
241 self._objs[name] = {"obj": o, "geom": geom}
242 # store the name of the geometry
243 self._names[id(geom)] = name
245 self._manager.registerObject(o)
246 self._manager.update()
247 return o
249 def remove_object(self, name):
250 """
251 Delete an object from the collision manager.
253 Parameters
254 ----------
255 name : str
256 The identifier for the object
257 """
258 if name in self._objs:
259 self._manager.unregisterObject(self._objs[name]["obj"])
260 self._manager.update(self._objs[name]["obj"])
261 # remove objects from _objs
262 geom_id = id(self._objs.pop(name)["geom"])
263 # remove names
264 self._names.pop(geom_id)
265 else:
266 raise ValueError(f"{name} not in collision manager!")
268 def set_transform(self, name, transform):
269 """
270 Set the transform for one of the manager's objects.
271 This replaces the prior transform.
273 Parameters
274 ----------
275 name : str
276 An identifier for the object already in the manager
277 transform : (4,4) float
278 A new homogeneous transform matrix for the object
279 """
280 if name in self._objs:
281 o = self._objs[name]["obj"]
282 o.setRotation(transform[:3, :3])
283 o.setTranslation(transform[:3, 3])
284 self._manager.update(o)
285 else:
286 raise ValueError(f"{name} not in collision manager!")
288 def in_collision_single(
289 self, mesh, transform=None, return_names=False, return_data=False
290 ):
291 """
292 Check a single object for collisions against all objects in the
293 manager.
295 Parameters
296 ----------
297 mesh : Trimesh object
298 The geometry of the collision object
299 transform : (4,4) float
300 Homogeneous transform matrix
301 return_names : bool
302 If true, a set is returned containing the names
303 of all objects in collision with the object
304 return_data : bool
305 If true, a list of ContactData is returned as well
307 Returns
308 ------------
309 is_collision : bool
310 True if a collision occurs and False otherwise
311 names : set of str
312 [OPTIONAL] The set of names of objects that collided with the
313 provided one
314 contacts : list of ContactData
315 [OPTIONAL] All contacts detected
316 """
317 if transform is None:
318 transform = np.eye(4)
320 # create BVH/Convex
321 geom = self._get_fcl_obj(mesh)
323 # create the FCL transform from (4,4) matrix
324 t = fcl.Transform(transform[:3, :3], transform[:3, 3])
325 o = fcl.CollisionObject(geom, t)
327 cdata, callback = _fcl_collision_data(return_names, return_data)
328 self._manager.collide(o, cdata, callback)
329 result = cdata.result.is_collision
331 # If we want to return the objects that were collision, collect them.
332 objs_in_collision = set()
333 contact_data = []
334 if return_names or return_data:
335 for contact in cdata.result.contacts:
336 cg = contact.o1
337 if cg == geom:
338 cg = contact.o2
339 name = self._extract_name(cg)
341 names = (name, "__external")
342 if cg == contact.o2:
343 names = tuple(reversed(names))
345 if return_names:
346 objs_in_collision.add(name)
347 if return_data:
348 contact_data.append(ContactData(names, contact))
350 if return_names and return_data:
351 return result, objs_in_collision, contact_data
352 elif return_names:
353 return result, objs_in_collision
354 elif return_data:
355 return result, contact_data
356 else:
357 return result
359 def in_collision_internal(self, return_names=False, return_data=False):
360 """
361 Check if any pair of objects in the manager collide with one another.
363 Parameters
364 ----------
365 return_names : bool
366 If true, a set is returned containing the names
367 of all pairs of objects in collision.
368 return_data : bool
369 If true, a list of ContactData is returned as well
371 Returns
372 -------
373 is_collision : bool
374 True if a collision occurred between any pair of objects
375 and False otherwise
376 names : set of 2-tup
377 The set of pairwise collisions. Each tuple
378 contains two names in alphabetical order indicating
379 that the two corresponding objects are in collision.
380 contacts : list of ContactData
381 All contacts detected
382 """
383 cdata, callback = _fcl_collision_data(return_names, return_data)
384 self._manager.collide(cdata, callback)
386 result = cdata.result.is_collision
388 objs_in_collision = set()
389 contact_data = []
390 if return_names or return_data:
391 for contact in cdata.result.contacts:
392 names = (self._extract_name(contact.o1), self._extract_name(contact.o2))
394 if return_names:
395 objs_in_collision.add(tuple(sorted(names)))
396 if return_data:
397 contact_data.append(ContactData(names, contact))
399 if return_names and return_data:
400 return result, objs_in_collision, contact_data
401 elif return_names:
402 return result, objs_in_collision
403 elif return_data:
404 return result, contact_data
405 else:
406 return result
408 def in_collision_other(self, other_manager, return_names=False, return_data=False):
409 """
410 Check if any object from this manager collides with any object
411 from another manager.
413 Parameters
414 -------------------
415 other_manager : CollisionManager
416 Another collision manager object
417 return_names : bool
418 If true, a set is returned containing the names
419 of all pairs of objects in collision.
420 return_data : bool
421 If true, a list of ContactData is returned as well
423 Returns
424 -------------
425 is_collision : bool
426 True if a collision occurred between any pair of objects
427 and False otherwise
428 names : set of 2-tup
429 The set of pairwise collisions. Each tuple
430 contains two names (first from this manager,
431 second from the other_manager) indicating
432 that the two corresponding objects are in collision.
433 contacts : list of ContactData
434 All contacts detected
435 """
436 cdata, callback = _fcl_collision_data(return_names, return_data)
437 self._manager.collide(other_manager._manager, cdata, callback)
438 result = cdata.result.is_collision
440 objs_in_collision = set()
441 contact_data = []
442 if return_names or return_data:
443 for contact in cdata.result.contacts:
444 reverse = False
445 names = (
446 self._extract_name(contact.o1),
447 other_manager._extract_name(contact.o2),
448 )
449 if names[0] is None:
450 names = (
451 self._extract_name(contact.o2),
452 other_manager._extract_name(contact.o1),
453 )
454 reverse = True
456 if return_names:
457 objs_in_collision.add(names)
458 if return_data:
459 if reverse:
460 names = tuple(reversed(names))
461 contact_data.append(ContactData(names, contact))
463 if return_names and return_data:
464 return result, objs_in_collision, contact_data
465 elif return_names:
466 return result, objs_in_collision
467 elif return_data:
468 return result, contact_data
469 else:
470 return result
472 def min_distance_single(
473 self, mesh, transform=None, return_name=False, return_data=False
474 ):
475 """
476 Get the minimum distance between a single object and any
477 object in the manager.
479 Parameters
480 ---------------
481 mesh : Trimesh object
482 The geometry of the collision object
483 transform : (4,4) float
484 Homogeneous transform matrix for the object
485 return_names : bool
486 If true, return name of the closest object
487 return_data : bool
488 If true, a DistanceData object is returned as well
490 Returns
491 -------------
492 distance : float
493 Min distance between mesh and any object in the manager
494 name : str
495 The name of the object in the manager that was closest
496 data : DistanceData
497 Extra data about the distance query
498 """
499 if transform is None:
500 transform = np.eye(4)
502 # create BVH/Convex
503 geom = self._get_fcl_obj(mesh)
505 # create the FCL transform from (4,4) matrix
506 t = fcl.Transform(transform[:3, :3], transform[:3, 3])
507 o = fcl.CollisionObject(geom, t)
509 # Collide with manager's objects
510 ddata = fcl.DistanceData(fcl.DistanceRequest(enable_signed_distance=True))
511 if return_data:
512 ddata = fcl.DistanceData(
513 fcl.DistanceRequest(
514 enable_nearest_points=True, enable_signed_distance=True
515 ),
516 fcl.DistanceResult(),
517 )
519 self._manager.distance(o, ddata, fcl.defaultDistanceCallback)
521 distance = ddata.result.min_distance
523 # If we want to return the objects that were collision, collect them.
524 name, data = None, None
525 if return_name or return_data:
526 cg = ddata.result.o1
527 if cg == geom:
528 cg = ddata.result.o2
530 name = self._extract_name(cg)
532 names = (name, "__external")
533 if cg == ddata.result.o2:
534 names = tuple(reversed(names))
535 data = DistanceData(names, ddata.result)
537 if return_name and return_data:
538 return distance, name, data
539 elif return_name:
540 return distance, name
541 elif return_data:
542 return distance, data
543 else:
544 return distance
546 def min_distance_internal(self, name=None, return_names=False, return_data=False):
547 """
548 Get the minimum distance between objects in the manager.
550 If name is provided, computes the minimum distance between the
551 specified object and any other object in the manager.
552 If name is None, computes the minimum distance between any pair
553 of objects in the manager.
555 Parameters
556 -------------
557 name : str or None
558 If provided, the identifier for the object already in the manager
559 to compute distances from. If None, computes distances between
560 all pairs of objects.
561 return_names : bool
562 If true, a 2-tuple is returned containing the names
563 of the closest objects.
564 return_data : bool
565 If true, a DistanceData object is returned as well
567 Returns
568 -----------
569 distance : float
570 Min distance between objects
571 names : (2,) str
572 The names of the closest objects
573 data : DistanceData
574 Extra data about the distance query
575 """
576 ddata = fcl.DistanceData(fcl.DistanceRequest(enable_signed_distance=True))
577 if return_data:
578 ddata = fcl.DistanceData(
579 fcl.DistanceRequest(
580 enable_nearest_points=True,
581 enable_signed_distance=True,
582 ),
583 fcl.DistanceResult(),
584 )
586 # If name is provided, compute distance from that object to others
587 if name is not None:
588 if name not in self._objs:
589 raise ValueError(f"{name} not in collision manager!")
590 obj = self._objs[name]["obj"]
591 # remove object from manager temporarily
592 self._manager.unregisterObject(obj)
593 self._manager.update(obj)
595 # compute distance
596 self._manager.distance(obj, ddata, fcl.defaultDistanceCallback)
598 # add it back to the manager
599 self._manager.registerObject(obj)
600 self._manager.update()
602 else:
603 # Compute distance between any pair of objects
604 self._manager.distance(ddata, fcl.defaultDistanceCallback)
606 distance = ddata.result.min_distance
608 names, data = None, None
609 if return_names or return_data:
610 names = (
611 self._extract_name(ddata.result.o1),
612 self._extract_name(ddata.result.o2),
613 )
614 data = DistanceData(names, ddata.result)
615 names = tuple(sorted(names))
617 if return_names and return_data:
618 return distance, names, data
619 elif return_names:
620 return distance, names
621 elif return_data:
622 return distance, data
623 else:
624 return distance
626 def min_distance_other(self, other_manager, return_names=False, return_data=False):
627 """
628 Get the minimum distance between any pair of objects,
629 one in each manager.
631 Parameters
632 ----------
633 other_manager : CollisionManager
634 Another collision manager object
635 return_names : bool
636 If true, a 2-tuple is returned containing
637 the names of the closest objects.
638 return_data : bool
639 If true, a DistanceData object is returned as well
641 Returns
642 -----------
643 distance : float
644 The min distance between a pair of objects,
645 one from each manager.
646 names : 2-tup of str
647 A 2-tuple containing two names (first from this manager,
648 second from the other_manager) indicating
649 the two closest objects.
650 data : DistanceData
651 Extra data about the distance query
652 """
653 ddata = fcl.DistanceData(fcl.DistanceRequest(enable_signed_distance=True))
654 if return_data:
655 ddata = fcl.DistanceData(
656 fcl.DistanceRequest(
657 enable_nearest_points=True,
658 enable_signed_distance=True,
659 ),
660 fcl.DistanceResult(),
661 )
663 self._manager.distance(other_manager._manager, ddata, fcl.defaultDistanceCallback)
665 distance = ddata.result.min_distance
667 names, data = None, None
668 if return_names or return_data:
669 reverse = False
670 names = (
671 self._extract_name(ddata.result.o1),
672 other_manager._extract_name(ddata.result.o2),
673 )
674 if names[0] is None:
675 reverse = True
676 names = (
677 self._extract_name(ddata.result.o2),
678 other_manager._extract_name(ddata.result.o1),
679 )
681 dnames = tuple(names)
682 if reverse:
683 dnames = tuple(reversed(dnames))
684 data = DistanceData(dnames, ddata.result)
686 if return_names and return_data:
687 return distance, names, data
688 elif return_names:
689 return distance, names
690 elif return_data:
691 return distance, data
692 else:
693 return distance
695 def _get_fcl_obj(self, mesh):
696 """
697 Get a BVH or Convex for a mesh.
699 Parameters
700 -------------
701 mesh : Trimesh
702 Mesh to create BVH/Convex for
704 Returns
705 --------------
706 obj : fcl.BVHModel or fcl.Convex
707 BVH/Convex object of source mesh
708 """
710 if mesh.is_convex:
711 obj = mesh_to_convex(mesh)
712 else:
713 obj = mesh_to_BVH(mesh)
714 return obj
716 def _extract_name(self, geom):
717 """
718 Retrieve the name of an object from the manager by its
719 CollisionObject, or return None if not found.
721 Parameters
722 -----------
723 geom : CollisionObject or BVHModel
724 Input model
726 Returns
727 ------------
728 names : hashable
729 Name of input geometry
730 """
731 return self._names[id(geom)]
734def mesh_to_BVH(mesh):
735 """
736 Create a BVHModel object from a Trimesh object
738 Parameters
739 -----------
740 mesh : Trimesh
741 Input geometry
743 Returns
744 ------------
745 bvh : fcl.BVHModel
746 BVH of input geometry
747 """
748 bvh = fcl.BVHModel()
749 bvh.beginModel(num_tris_=len(mesh.faces), num_vertices_=len(mesh.vertices))
750 bvh.addSubModel(verts=mesh.vertices, triangles=mesh.faces)
751 bvh.endModel()
752 return bvh
755def mesh_to_convex(mesh):
756 """
757 Create a Convex object from a Trimesh object
759 Parameters
760 -----------
761 mesh : Trimesh
762 Input geometry
764 Returns
765 ------------
766 convex : fcl.Convex
767 Convex of input geometry
768 """
769 fs = np.concatenate(
770 (3 * np.ones((len(mesh.faces), 1), dtype=np.int64), mesh.faces), axis=1
771 )
772 return fcl.Convex(mesh.vertices, len(fs), fs.flatten())
775def scene_to_collision(scene):
776 """
777 Create collision objects from a trimesh.Scene object.
779 Parameters
780 ------------
781 scene : trimesh.Scene
782 Scene to create collision objects for
784 Returns
785 ------------
786 manager : CollisionManager
787 CollisionManager for objects in scene
788 objects: {node name: CollisionObject}
789 Collision objects for nodes in scene
790 """
791 manager = CollisionManager()
792 objects = {}
793 for node in scene.graph.nodes_geometry:
794 T, geometry = scene.graph[node]
795 objects[node] = manager.add_object(
796 name=node, mesh=scene.geometry[geometry], transform=T
797 )
798 return manager, objects