factor out fcl; using pythonocc for collision detection

src/pyolp_robotics/OCC_functions.py

@@ -79,7 +79,7 @@
     topods_Face,
     TopoDS_Iterator,
     TopoDS_Shape,
-    TopoDS_Shell,
+    TopoDS_Shell, TopoDS_Vertex,
 )
 
 from OCC.Extend.ShapeFactory import *
@@ -91,10 +91,7 @@
 from OCCUtils.Topology import Topo
 
 import collections
-import fcl
 import time
-from itertools import combinations
-import numpy as np
 import math
 
 def create_vec_2pts(from_pnt, to_pnt):
@@ -544,7 +541,7 @@ def TopExplorer(shape, to_find=None, filter_pts=0.01):
         topexp_vertex.Init(shape, TopAbs_VERTEX)
         vertices = []
         while topexp_vertex.More():
-            vert = topods_Vertex(topexp_vertex.Current())
+            vert = TopoDS_Vertex(topexp_vertex.Current())
             point = BRep_Tool.Pnt(vert)
             vertices.append(point)
             topexp_vertex.Next()
@@ -783,88 +780,3 @@ def mesh_from_brep(occ_brep, theLinDeflection=0.8):
     )
     return triangles
 
-def print_collision_result(o1_name, o2_name, result):
-    print("Collision between {} and {}:".format(o1_name, o2_name))
-    print("-" * 30)
-    print("Collision?: {}".format(result.is_collision))
-    print("Number of contacts: {}".format(len(result.contacts)))
-    print("")
-
-def fcl_collision_object_from_shape(shape1, shape2):
-    """
-    create a fcl.BVHModel instance from the `shape` TopoDS_Shape
-    """
-    triangles = mesh_from_brep(shape1)
-    triangles = np.array(triangles)
-    # Create mesh geometry
-    _mesh1 = fcl.BVHModel()
-    n_tris = len(triangles)
-    _mesh1.beginModel(n_tris, n_tris * 3)
-    for tri in triangles:
-        x, y, z = tri
-        _mesh1.addTriangle(x, y, z)
-    _mesh1.endModel()
-
-    triangles2 = mesh_from_brep(shape2)
-    triangles2 = np.array(triangles2)
-    # Create mesh geometry
-    _mesh2 = fcl.BVHModel()
-    n_tris = len(triangles2)
-    _mesh2.beginModel(n_tris, n_tris * 3)
-    for tri in triangles2:
-        x, y, z = tri
-        _mesh2.addTriangle(x, y, z)
-    _mesh2.endModel()
-
-    req = fcl.CollisionRequest(enable_contact=True)
-    res = fcl.CollisionResult()
-
-    n_contacts = fcl.collide(
-    fcl.CollisionObject(_mesh1, fcl.Transform()),
-    fcl.CollisionObject(_mesh2, fcl.Transform()),
-    req,
-    res,
-    )
-    print_collision_result("_mesh1", "_mesh2", res)
-    return res.is_collision
-
-def fcl_collisions_collection_shapes(shapes, stop_at_first=True):
-    continue_searching = True
-    shapes_colliding = []
-    for two_solids in combinations(shapes, 2):
-        if continue_searching:
-            basis = two_solids[0]
-            cutter = two_solids[1]
-            collision = fcl_collision_object_from_shape(basis, cutter) 
-        if collision is True :
-            shapes_colliding.append(basis)
-            shapes_colliding.append(cutter)
-            if stop_at_first :
-                continue_searching = False
-    return collision, shapes_colliding
-
-def check_two_part_collisions(part1, part2):
-    collision = False
-    basis = part1
-    cutter = part2
-    result = BRepAlgoAPI_Section(basis, cutter).Shape()
-    if result.NbChildren() > 0:
-        collision = True
-    return collision
-
-def check_collections_collisions(shapes, stop_at_first=True):
-    continue_searching = True
-    collision = False
-    shapes_colliding = []
-    for two_solids in combinations(shapes, 2):
-        if continue_searching:
-            basis = two_solids[0]
-            cutter = two_solids[1]
-            result = BRepAlgoAPI_Section(basis, cutter).Shape()
-            if result.NbChildren() > 0:
-                collision = True
-                shapes_colliding.append(basis)
-                shapes_colliding.append(cutter)
-                if stop_at_first :
-                    continue_searching = False
-    return collision, shapes_colliding

src/pyolp_robotics/collision.py

@@ -0,0 +1,189 @@
+from itertools import combinations
+
+# import fcl
+import numpy as np
+from OCC.Core.AIS import AIS_Shape
+from OCC.Core.BRepAlgoAPI import BRepAlgoAPI_Section
+from OCC.Core.BRepBuilderAPI import BRepBuilderAPI_Transform
+from OCC.Core.BRepExtrema import BRepExtrema_ShapeProximity
+from OCC.Core.BRepMesh import BRepMesh_IncrementalMesh
+from OCC.Core.TopoDS import TopoDS_Shape
+
+from pyolp_robotics.OCC_functions import mesh_from_brep
+
+
+## FCL based code
+
+def print_collision_result(o1_name, o2_name, result):
+    print("Collision between {} and {}:".format(o1_name, o2_name))
+    print("-" * 30)
+    print("Collision?: {}".format(result.is_collision))
+    print("Number of contacts: {}".format(len(result.contacts)))
+    print("")
+
+
+def fcl_collision_object_from_shape(shape1, shape2):
+    """
+    create a fcl.BVHModel instance from the `shape` TopoDS_Shape
+    """
+    triangles = mesh_from_brep(shape1)
+    triangles = np.array(triangles)
+    # Create mesh geometry
+    _mesh1 = fcl.BVHModel()
+    n_tris = len(triangles)
+    _mesh1.beginModel(n_tris, n_tris * 3)
+    for tri in triangles:
+        x, y, z = tri
+        _mesh1.addTriangle(x, y, z)
+    _mesh1.endModel()
+
+    triangles2 = mesh_from_brep(shape2)
+    triangles2 = np.array(triangles2)
+    # Create mesh geometry
+    _mesh2 = fcl.BVHModel()
+    n_tris = len(triangles2)
+    _mesh2.beginModel(n_tris, n_tris * 3)
+    for tri in triangles2:
+        x, y, z = tri
+        _mesh2.addTriangle(x, y, z)
+    _mesh2.endModel()
+
+    req = fcl.CollisionRequest(enable_contact=True)
+    res = fcl.CollisionResult()
+
+    n_contacts = fcl.collide(
+        fcl.CollisionObject(_mesh1, fcl.Transform()),
+        fcl.CollisionObject(_mesh2, fcl.Transform()),
+        req,
+        res,
+    )
+    print_collision_result("_mesh1", "_mesh2", res)
+    return res.is_collision
+
+
+def fcl_collisions_collection_shapes(shapes, stop_at_first=True):
+    continue_searching = True
+    shapes_colliding = []
+    for two_solids in combinations(shapes, 2):
+        if continue_searching:
+            basis = two_solids[0]
+            cutter = two_solids[1]
+            collision = fcl_collision_object_from_shape(basis, cutter)
+        if collision is True:
+            shapes_colliding.append(basis)
+            shapes_colliding.append(cutter)
+            if stop_at_first:
+                continue_searching = False
+    return collision, shapes_colliding
+
+
+def check_two_part_collisions(part1, part2):
+    collision = False
+    basis = part1
+    cutter = part2
+    result = BRepAlgoAPI_Section(basis, cutter).Shape()
+    if result.NbChildren() > 0:
+        collision = True
+    return collision
+
+
+def check_collections_collisions(shapes, stop_at_first=True):
+    continue_searching = True
+    collision = False
+    shapes_colliding = []
+    for two_solids in combinations(shapes, 2):
+        if continue_searching:
+            basis = two_solids[0]
+            cutter = two_solids[1]
+            result = BRepAlgoAPI_Section(basis, cutter).Shape()
+            if result.NbChildren() > 0:
+                collision = True
+                shapes_colliding.append(basis)
+                shapes_colliding.append(cutter)
+                if stop_at_first:
+                    continue_searching = False
+    return collision, shapes_colliding
+
+
+## OCC handling collisions
+
+class Ais_Collision:
+    def __init__(self,
+                 ais_a: TopoDS_Shape,
+                 ais_b: TopoDS_Shape,
+                 get_collision_a=True,
+                 get_collision_b=True,
+                 defl=1e01,
+                 tol=0.1
+                 ):
+
+        """Use AIS objects for interference judgment
+
+        Parameters
+        ----------
+        `ais_a` : AIS_Shape
+            Interference object A
+        `ais_b` : AIS_Shape
+            Interference object B
+        `get_collision_a` : bool, optional
+            Whether to return the faces where collisions occur in A, default value: True
+        `get_collision_b` : bool, optional
+            Whether to return the faces where collisions occur in B, default value: True
+
+        Returns
+        -------
+        _type_
+            The faces where collisions occurred (including results from A and B)
+        """
+        # # Obtain the TopoDS of the AIS object
+        _shape_a = ais_a.Shape()
+        _shape_b = ais_b.Shape()
+        # Obtain the Trsf of the AIS object
+        trsf_a = ais_a.Transformation()
+        trsf_b = ais_b.Transformation()
+        # Move the original shape
+        self.shape_a = BRepBuilderAPI_Transform(_shape_a, trsf_a).Shape()
+        self.shape_b = BRepBuilderAPI_Transform(_shape_b, trsf_b).Shape()
+        self.defl = defl
+        self.tol = tol
+        self.get_collision_a = get_collision_a
+        self.get_collision_b = get_collision_b
+        # Generate mesh
+        self._compute_triangulation()
+
+    def _compute_triangulation(self):
+        self.mesh_a = BRepMesh_IncrementalMesh(self.shape_a, self.defl)
+        self.mesh_a.Perform()
+        self.mesh_b = BRepMesh_IncrementalMesh(self.shape_b, self.defl)
+        self.mesh_b.Perform()
+
+    def check_collision(self):
+        # Collision check
+        self.proximity = BRepExtrema_ShapeProximity(self.shape_a, self.shape_b, self.tol)
+        self.proximity.Perform()
+        return self.proximity.IsDone()
+
+    def get_collision_sets(self):
+        if self.proximity.IsDone():
+            # Get the collision part of shape_a
+            if self.get_collision_a:
+                overlaps1 = self.proximity.OverlapSubShapes1()
+                face_indices1 = overlaps1.Keys()
+                collision_face_a = []
+                for ind in face_indices1:
+                    face = self.proximity.GetSubShape1(ind)
+                    collision_face_a.append(face)
+
+            # Get the collision part of shape_b
+            if self.get_collision_b:
+                overlaps2 = self.proximity.OverlapSubShapes2()
+                face_indices2 = overlaps2.Keys()
+                collision_face_b = []
+                for ind in face_indices2:
+                    face = self.proximity.GetSubShape2(ind)
+                    collision_face_b.append(face)
+            # Return the faces where interference occurred
+            if collision_face_a or collision_face_b:
+                return [collision_face_a, collision_face_b]
+        else:
+            return []

src/pyolp_robotics/ikpy_planner.py

@@ -9,6 +9,8 @@
 from math import pi, sqrt, sin, cos, atan2, radians, degrees
 
 import pyolp_robotics.OCC_functions as occ
+import pyolp_robotics.collision
+
 
 class Planner(object):
     def __init__(self, robot) -> None:
@@ -192,7 +194,7 @@ def get_pose_collision_free(self, ax3, config_initial, timeout=100, selfcollisio
                 all_shapes.append(self.collision_environment)
             else :
                 all_shapes = [self.collision_environment, self.robot.get_compound()]
-            collision_non_ang_checked, shapes_colliding = occ.check_collections_collisions(all_shapes)
+            collision_non_ang_checked, shapes_colliding = pyolp_robotics.collision.check_collections_collisions(all_shapes)
             if not collision_non_ang_checked:
                 # config = self.rebase_config_domain(config_ikpy[-1])
                 articular_limit_ok = self.check_articular_limits(config_ikpy[-1])

tests/self_collision.py

@@ -1,28 +1,47 @@
 import os
 import sys
-sys.path.append(os.path.realpath(os.curdir)) 
+
+from OCC.Core.AIS import AIS_Shape
+
+from pyolp_robotics.collision import check_two_part_collisions, Ais_Collision
+
+sys.path.append(os.path.realpath(os.curdir))
 
 import pyolp_robotics.OCC_functions as occ
 from pyolp_robotics.robots.ur import UR10
 
 from OCC.Display.SimpleGui import init_display
+
 display, start_display, add_menu, add_function_to_menu = init_display()
 
 rob = UR10(mesh=False)
 rob.initialise_robot()
 rob.set_tool("grinding")
 
-config_collision =  [-2.13173836, -1.29331347,  0.7562257,   0.53708777, -2.13173836,  0.]
-
+config_collision = [-2.13173836, -1.29331347, 0.7562257, 0.53708777, -2.13173836, 0.]
 rob.forward_k(config_collision)
-display.DisplayShape(rob.get_compound(), transparency=0.8)
-
-axe_with_tool =  rob.get_axes()[-1]
-for axe in rob.get_axes()[:-1]:
-    collision = occ.check_two_part_collisions(axe, axe_with_tool)
-    if collision:
-        display.DisplayShape(axe, color="red")
-        display.DisplayShape(axe_with_tool, color="red")
-
+
+joints_ais = display.DisplayShape(rob.axes, transparency=0.8)
+
+axe_with_tool = rob.get_axes()[-1]
+yyy = display.DisplayShape(axe_with_tool, color="black")[0]
+
+for nr_jnt, axe in enumerate(joints_ais):
+    if axe.Shape().IsSame(axe_with_tool):
+        print("tool / tool clash, skipping")
+
+    else:
+        collide = Ais_Collision(axe, yyy)
+        does_collide = collide.check_collision()
+
+        if does_collide:
+            # TODO:
+            faces_a, faces_b = collide.get_collision_sets()
+            print(f"joint: {nr_jnt} interferes with tool")
+            display.DisplayShape(faces_a, color="red")
+            display.DisplayShape(faces_b, color="green")
+            print(f" len collision sets: {len(faces_a)}, {len(faces_b)}")
+            print("if of similar length, that's suspicious")
+
 display.FitAll()
-start_display() 
+start_display()