diff --git a/src/pyolp_robotics/OCC_functions.py b/src/pyolp_robotics/OCC_functions.py index 6c311c5..07a7198 100644 --- a/src/pyolp_robotics/OCC_functions.py +++ b/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 diff --git a/src/pyolp_robotics/collision.py b/src/pyolp_robotics/collision.py new file mode 100644 index 0000000..4a9d8f3 --- /dev/null +++ b/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 [] diff --git a/src/pyolp_robotics/ikpy_planner.py b/src/pyolp_robotics/ikpy_planner.py index 0b7be79..4e7fb67 100644 --- a/src/pyolp_robotics/ikpy_planner.py +++ b/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]) diff --git a/tests/self_collision.py b/tests/self_collision.py index 526c26f..f8730d7 100644 --- a/tests/self_collision.py +++ b/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() \ No newline at end of file +start_display()