def in_collision_other(self, other_manager, return_names=False, return_data=False): """ Check if any object from this manager collides with any object from another manager. Parameters ---------- other_manager: CollisionManager, another collision manager object return_names: bool, If true, a set is returned containing the names of all pairs of objects in collision. return_data: bool, If true, a list of ContactData is returned as well Returns ------- is_collision: bool, True if a collision occurred between any pair of objects and False otherwise names: set of 2-tup, The set of pairwise collisions. Each tuple contains two names (first from this manager, second from the other_manager) indicating that the two corresponding objects are in collision. contacts: list of ContactData, All contacts detected """ cdata = fcl.CollisionData() if return_names or return_data: cdata = fcl.CollisionData( request=fcl.CollisionRequest( num_max_contacts=100000, enable_contact=True)) self._manager.collide(other_manager._manager, cdata, fcl.defaultCollisionCallback) result = cdata.result.is_collision objs_in_collision = set() contact_data = [] if return_names or return_data: for contact in cdata.result.contacts: reverse = False names = (self._extract_name(contact.o1), other_manager._extract_name(contact.o2)) if names[0] is None: names = (self._extract_name(contact.o2), other_manager._extract_name(contact.o1)) reverse = True if return_names: objs_in_collision.add(names) if return_data: if reverse: names = reversed(names) contact_data.append(ContactData(names, contact)) if return_names and return_data: return result, objs_in_collision, contact_data elif return_names: return result, objs_in_collision elif return_data: return result, contact_data else: return result
def isValid(self, state): # print("collision") sample = np.array([state[0], state[1], 0]) # sample = np.array([state().getX(), state().getY(), state().getYaw()]) req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) rdata = fcl.CollisionData(request = req) cyl = fcl.Cylinder(0.01, 2) t = fcl.Transform(sample) agent = fcl.CollisionObject(cyl, t) self.fcl_manager.collide(agent, rdata, fcl.defaultCollisionCallback) # if(rdata.result.is_collision): # print("state: ", sample, " collision: ", rdata.result.is_collision) # print ('Collision between manager 1 and agent?: {}'.format(rdata.result.is_collision)) # print( 'Contacts:') # for c in rdata.result.contacts: # print( '\tO1: {}, O2: {}'.format(c.o1, c.o2)) self.count += 1 if(rdata.result.is_collision): self.collision_count += 1 # self.states_bad.append(sample) # else: # self.states_ok.append(sample) # return not rdata.result.is_collision return True
def predict(self, X, distance=True): labels = torch.FloatTensor(len(X), len(self.obs_managers)) dists = torch.FloatTensor(len(X), len( self.obs_managers)) if distance else None req = fcl.CollisionRequest(num_max_contacts=1000 if distance else 1, enable_contact=distance) for i, cfg in enumerate(X): self.robot.update_polygons(cfg) self.robot_manager.update() assert len(self.robot_manager.getObjects()) == self.robot.dof for cat, obs_mng in enumerate(self.obs_managers): rdata = fcl.CollisionData(request=req) self.robot_manager.collide(obs_mng, rdata, fcl.defaultCollisionCallback) in_collision = rdata.result.is_collision labels[i, cat] = 1 if in_collision else -1 if distance: ddata = fcl.DistanceData() self.robot_manager.distance(obs_mng, ddata, fcl.defaultDistanceCallback) depths = torch.FloatTensor( [c.penetration_depth for c in rdata.result.contacts]) dists[i, cat] = depths.abs().max( ) if in_collision else -ddata.result.min_distance if distance: return labels, dists else: return labels
def __init__(self, dx, dy, dz): self.dx = dx self.dy = dy self.dz = dz self.num_edges = 12 self.fcl_shape = fcl.Box(dx, dy, dz) self.request = fcl.CollisionRequest() self.result = fcl.CollisionResult()
def __init__(self, radius, length, approx_faces=8): self.r = radius self.l = length # the number of faces to use in the polyherdron approximation self.nfac = approx_faces self.num_edges = 3 * self.nfac self.fcl_shape = fcl.Cylinder(radius, length) self.request = fcl.CollisionRequest() self.result = fcl.CollisionResult()
def in_collision_internal(self, return_names=False, return_data=False): """ Check if any pair of objects in the manager collide with one another. Parameters ---------- return_names : bool If true, a set is returned containing the names of all pairs of objects in collision. return_data : bool If true, a list of ContactData is returned as well Returns ------- is_collision : bool True if a collision occurred between any pair of objects and False otherwise names : set of 2-tup The set of pairwise collisions. Each tuple contains two names in alphabetical order indicating that the two corresponding objects are in collision. contacts : list of ContactData All contacts detected """ cdata = fcl.CollisionData() if return_names or return_data: cdata = fcl.CollisionData(request=fcl.CollisionRequest( num_max_contacts=100000, enable_contact=True)) self._manager.collide(cdata, fcl.defaultCollisionCallback) result = cdata.result.is_collision objs_in_collision = set() contact_data = [] if return_names or return_data: for contact in cdata.result.contacts: names = (self._extract_name(contact.o1), self._extract_name(contact.o2)) if return_names: objs_in_collision.add(tuple(sorted(names))) if return_data: contact_data.append(ContactData(names, contact)) if return_names and return_data: return result, objs_in_collision, contact_data elif return_names: return result, objs_in_collision elif return_data: return result, contact_data else: return result
def checkCollision(self): g1 = fcl.Sphere(self.Radius) t1 = fcl.Transform() o1 = fcl.CollisionObject(g1, t1) g2 = fcl.sphere(self.Radius) t2 = fcl.Transform() o2 = fcl.CollisionObject(g2, t2) request = fcl.CollisionRequest() result = fcl.CollisionResult() ret = fcl.collide(o1, o2, request, result) return result, ret #result.is_collide is the required function
def collision_check(self): #world = ['blue_box', 'white_sphere', 'red_cylinder', 'green_box', 'turquoise_box', 'blue_cylinder', 'white_box', 'fetch'] robot = fcl.Cylinder(0.4, 1) tR = fcl.Transform(self.quaternion[7], self.translation[7]) print self.translation[7] oR = fcl.CollisionObject(robot, tR) ob0 = fcl.Box(0.3, 1, 0.8) tr0 = fcl.Transform(self.quaternion[0], self.translation[0]) self.obj[0] = fcl.CollisionObject(ob0, tr0) ob1 = fcl.Sphere(0.5) tr1 = fcl.Transform(self.quaternion[1], self.translation[1]) self.obj[1] = fcl.CollisionObject(ob1, tr1) ob2 = fcl.Cylinder(0.5, 1) tr2 = fcl.Transform(self.quaternion[2], self.translation[2]) self.obj[2] = fcl.CollisionObject(ob2, tr2) ob3 = fcl.Box(0.5, 1.4, 0.8) tr3 = fcl.Transform(self.quaternion[3], self.translation[3]) self.obj[3] = fcl.CollisionObject(ob3, tr3) ob4 = fcl.Box(1, 5, 1) tr4 = fcl.Transform(self.quaternion[4], self.translation[4]) self.obj[4] = fcl.CollisionObject(ob4, tr4) ob5 = fcl.Cylinder(0.5, 1) tr5 = fcl.Transform(self.quaternion[5], self.translation[5]) self.obj[5] = fcl.CollisionObject(ob5, tr5) ob6 = fcl.Box(5, 1, 1) tr6 = fcl.Transform(self.quaternion[6], self.translation[6]) self.obj[6] = fcl.CollisionObject(ob6, tr6) request = fcl.CollisionRequest() result = fcl.CollisionResult() for i in range(7): self.ret[i] = fcl.collide(oR, self.obj[i], request, result) # ret = fcl.continuousCollide(oR, tR, o_wb, t_wb, request, result) if self.ret[i]: print "--------------- YES ", self.ret[ i], " --------------------" else: print "--------------- NO ", self.ret[ i], " -------------------"
def in_collision_single(self, mesh, transform=None, return_names=False): ''' Check a single object for collisions against all objects in the manager. Parameters ---------- mesh: Trimesh object, the geometry of the collision object transform: (4,4) float, homogenous transform matrix for the object return_names : bool, If true, a set is returned containing the names of all objects in collision with the provided object Returns ------- is_collision: bool, True if a collision occurs and False otherwise names: set of str, The set of names of objects that collided with the provided one ''' if transform is None: transform = np.eye(4) # Create FCL data b = fcl.BVHModel() b.beginModel(len(mesh.vertices), len(mesh.faces)) b.addSubModel(mesh.vertices, mesh.faces) b.endModel() t = fcl.Transform(transform[:3, :3], transform[:3, 3]) o = fcl.CollisionObject(b, t) # Collide with manager's objects cdata = fcl.CollisionData() if return_names: cdata = fcl.CollisionData(request=fcl.CollisionRequest( num_max_contacts=100000, enable_contact=True)) self._manager.collide(o, cdata, fcl.defaultCollisionCallback) result = cdata.result.is_collision # If we want to return the objects that were collision, collect them. if return_names: objs_in_collision = set() for contact in cdata.result.contacts: cg = contact.o1 if cg == b: cg = contact.o2 name = self._extract_name(cg) objs_in_collision.add(name) return result, objs_in_collision else: return result
def checkLidarCollistion(lidar, verts, tris): mesh = fcl.BVHModel() mesh.beginModel(len(verts), len(tris)) mesh.addSubModel(verts, tris) mesh.endModel() mesh_obj = fcl.CollisionObject(mesh) objs = [mesh_obj, lidar] manager = fcl.DynamicAABBTreeCollisionManager() manager.registerObjects(objs) manager.setup() crequest = fcl.CollisionRequest(enable_contact=True) drequest = fcl.DistanceRequest(enable_nearest_points=True) cdata = fcl.CollisionData(crequest, fcl.CollisionResult()) ddata = fcl.DistanceData(drequest, fcl.DistanceResult()) manager.collide(cdata, fcl.defaultCollisionCallback) manager.distance(ddata, fcl.defaultDistanceCallback) minimum_distance = ddata.result.min_distance return minimum_distance
def isValidPoint(self, s_q): # check if the sampled point is inside the world" if not (self.x_min[0] < s_q[0] < self.x_max[0] and self.x_min[1] < s_q[1] < self.x_max[1] and self.v_min[0] < s_q[2] < self.v_max[0] and self.v_min[1] < s_q[3] < self.v_max[1]): return False # 一对many fclPoint = self.point2fcl(s_q) req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) rdata = fcl.CollisionData(request=req) self.fcl_manager.collide(fclPoint, rdata, fcl.defaultCollisionCallback) # print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision)) # print('Contacts:') # for c in rdata.result.contacts: # print('\tO1: {}, O2: {}'.format(c.o1, c.o2)) return not rdata.result.is_collision
def CheckCollison(rotate_direction, verts, tris, stick, rota_degree, trans_x, trans_y, tolerance): global previous_minimum_distance mesh = fcl.BVHModel() mesh.beginModel(len(verts), len(tris)) mesh.addSubModel(verts, tris) mesh.endModel() mesh_obj = fcl.CollisionObject(mesh) objs = [mesh_obj, stick] manager = fcl.DynamicAABBTreeCollisionManager() manager.registerObjects(objs) manager.setup() crequest = fcl.CollisionRequest(enable_contact=True) drequest = fcl.DistanceRequest(enable_nearest_points=True) cdata = fcl.CollisionData(crequest, fcl.CollisionResult()) ddata = fcl.DistanceData(drequest, fcl.DistanceResult()) manager.collide(cdata, fcl.defaultCollisionCallback) manager.distance(ddata, fcl.defaultDistanceCallback) #print("Is collision: ", cdata.result.is_collision) minimum_distance = ddata.result.min_distance nearest_point = ddata.result.nearest_points[0][:2] nearest_point_stick = ddata.result.nearest_points[1] nearest_point_stick = Transform(rota_degree, trans_x, trans_y, nearest_point_stick) collision = -10000 if previous_minimum_distance != -10000: if minimum_distance == -1: collision = -1 print("The tip of the stick is inside the soft tissues.") elif minimum_distance >= tolerance: collision = 0 print("No collision") elif minimum_distance >= 0 and minimum_distance < tolerance and minimum_distance < previous_minimum_distance: print("Collision") collision = 1 # if nearest_point in verts: # print("nearest point of the stick:", nearest_point_stick) tmp = previous_minimum_distance previous_minimum_distance = minimum_distance return nearest_point, collision, minimum_distance
def in_collision_other(self, other_manager, return_names=False): ''' Check if any object from this manager collides with any object from another manager. Parameters ---------- other_manager: CollisionManager, another collision manager object return_names: bool, If true, a set is returned containing the names of all pairs of objects in collision. Returns ------- is_collision: bool, True if a collision occured between any pair of objects and False otherwise names: set of 2-tup, The set of pairwise collisions. Each tuple contains two names (first from this manager, second from the other_manager) indicating that the two correspoinding objects are in collision. ''' cdata = fcl.CollisionData() if return_names: cdata = fcl.CollisionData(request=fcl.CollisionRequest(num_max_contacts=100000, enable_contact=True)) self._manager.collide(other_manager._manager, cdata, fcl.defaultCollisionCallback) result = cdata.result.is_collision if return_names: objs_in_collision = set() for contact in cdata.result.contacts: name1, name2 = self._extract_name( contact.o1), other_manager._extract_name(contact.o2) if name1 is None: name1, name2 = self._extract_name( contact.o2), other_manager._extract_name(contact.o1) objs_in_collision.add((name1, name2)) return result, objs_in_collision else: return result
def setUp(self): verts = np.array([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]]) mesh = fcl.BVHModel() mesh.beginModel(len(verts), len(tris)) mesh.addSubModel(verts, tris) mesh.endModel() self.geometry = { 'box': fcl.Box(1.0, 1.0, 1.0), 'sphere': fcl.Sphere(1.0), 'cone': fcl.Cone(1.0, 1.0), 'cylinder': fcl.Cylinder(1.0, 1.0), 'mesh': mesh } self.crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) self.drequest = fcl.DistanceRequest(enable_nearest_points=True) self.x_axis_rot = np.array([[1.0, 0.0, 0.0], [0.0, 0.0, -1.0], [0.0, 1.0, 0.0]])
def in_collision_internal(self, return_names=False): ''' Check if any pair of objects in the manager collide with one another. Parameters ---------- return_names : bool If true, a set is returned containing the names of all pairs of objects in collision. Returns ------- is_collision: bool, True if a collision occured between any pair of objects and False otherwise names: set of 2-tup, The set of pairwise collisions. Each tuple contains two names in alphabetical order indicating that the two correspoinding objects are in collision. ''' cdata = fcl.CollisionData() if return_names: cdata = fcl.CollisionData(request=fcl.CollisionRequest( num_max_contacts=100000, enable_contact=True)) self._manager.collide(cdata, fcl.defaultCollisionCallback) result = cdata.result.is_collision if return_names: objs_in_collision = set() for contact in cdata.result.contacts: name1, name2 = (self._extract_name(contact.o1), self._extract_name(contact.o2)) names = tuple(sorted((name1, name2))) objs_in_collision.add(names) return result, objs_in_collision else: return result
def isValid(self, q_set): # check validity for multiple points. # will be used for piecewize path consited of multiple points # for q in q_set: # if not self.isValidPoint(q): # return False # return True if len(q_set.shape) < 2: return self.isValidPoint(q_set) manager_q = fcl.DynamicAABBTreeCollisionManager() qs = [] for q in q_set: qs.append(self.point2fcl(q)) manager_q.registerObjects(qs) req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) rdata = fcl.CollisionData(request=req) self.fcl_manager.collide(manager_q, rdata, fcl.defaultCollisionCallback) # print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision)) # print('Contacts:') # for c in rdata.result.contacts: # print('\tO1: {}, O2: {}'.format(c.o1, c.o2)) return not rdata.result.is_collision
def in_collision_single(self, mesh, transform=None, return_names=False, return_data=False): """ Check a single object for collisions against all objects in the manager. Parameters ---------- mesh: Trimesh object, the geometry of the collision object transform: (4,4) float, homogenous transform matrix return_names: bool, If true, a set is returned containing the names of all objects in collision with the object return_data: bool, If true, a list of ContactData is returned as well Returns ------- is_collision: bool, True if a collision occurs and False otherwise names: set of str, The set of names of objects that collided with the provided one contacts: list of ContactData, All contacts detected """ if transform is None: transform = np.eye(4) # Create FCL data b = self._get_BVH(mesh) t = fcl.Transform(transform[:3, :3], transform[:3, 3]) o = fcl.CollisionObject(b, t) # Collide with manager's objects cdata = fcl.CollisionData() if return_names or return_data: cdata = fcl.CollisionData(request=fcl.CollisionRequest( num_max_contacts=100000, enable_contact=True)) self._manager.collide(o, cdata, fcl.defaultCollisionCallback) result = cdata.result.is_collision # If we want to return the objects that were collision, collect them. objs_in_collision = set() contact_data = [] if return_names or return_data: for contact in cdata.result.contacts: cg = contact.o1 if cg == b: cg = contact.o2 name = self._extract_name(cg) names = (name, '__external') if cg == contact.o2: names = reversed(names) if return_names: objs_in_collision.add(name) if return_data: contact_data.append(ContactData(names, contact)) if return_names and return_data: return result, objs_in_collision, contact_data elif return_names: return result, objs_in_collision elif return_data: return result, contact_data else: return result
sys.path.append(os.environ['CODE_BASE'] + '/catkin_ws/src/config/src') from helper import helper, roshelper from scipy.spatial import ConvexHull from scipy.spatial import Delaunay from shapely.geometry import Polygon from tf.transformations import quaternion_from_euler import util from stl import mesh import os import tf import trimesh from copy import deepcopy import fcl import rospy request = fcl.CollisionRequest() result = fcl.CollisionResult() def initialize_collision_object(tris, verts): m = fcl.BVHModel() m.beginModel(len(verts), len(tris)) m.addSubModel(verts, tris) m.endModel() t = fcl.Transform() return fcl.CollisionObject(m, t) def is_collision(body1, body2): return fcl.collide(body1, body2, request, result) class Body(object): def __init__(self, mesh_name):
def generate_one(robot, obs_num, folder, label_type='binary', num_class=None, num_points=8000, env_id='', vis=True): obstacles = [] types = ['rect', 'circle'] link_length = robot.link_length[0].item() for i in range(obs_num): obs_t = types[randint(2)] if types[0] in obs_t: # rectangle, size = 0.5-3.5, pos = -7~7 while True: s = rand(2) * 3 + 0.5 if obs_num <= 2: p = rand(2) * 10 - 5 else: p = rand(2) * 14 - 7 if any(p - s / 2 > link_length) or any( p + s / 2 < -link_length): # the near-origin area is clear break elif types[1] in obs_t: # circle, size = 0.25-2, pos = -7~7 while True: s = rand() * 1.75 + 0.25 if obs_num <= 2: p = rand(2) * 10 - 5 else: p = rand(2) * 14 - 7 if np.linalg.norm(p) > s + link_length: break obstacles.append((obs_t, p, s)) fcl_obs = [FCLObstacle(*param) for param in obstacles] fcl_collision_obj = [fobs.cobj for fobs in fcl_obs] # geom2instnum = {id(g): i for i, (_, g) in enumerate(fcl_obs)} cfgs = torch.rand((num_points, robot.dof), dtype=torch.float32) cfgs = cfgs * (robot.limits[:, 1] - robot.limits[:, 0]) + robot.limits[:, 0] if label_type == 'binary': labels = torch.zeros(num_points, 1, dtype=torch.float) dists = torch.zeros(num_points, 1, dtype=torch.float) obs_managers = [fcl.DynamicAABBTreeCollisionManager()] obs_managers[0].registerObjects(fcl_collision_obj) obs_managers[0].setup() elif label_type == 'instance': labels = torch.zeros(num_points, len(obstacles), dtype=torch.float) dists = torch.zeros(num_points, len(obstacles), dtype=torch.float) obs_managers = [fcl.DynamicAABBTreeCollisionManager() for _ in fcl_obs] for mng, cobj in zip(obs_managers, fcl_collision_obj): mng.registerObjects([cobj]) elif label_type == 'class': labels = torch.zeros(num_points, num_class, dtype=torch.float) dists = torch.zeros(num_points, num_class, dtype=torch.float) obs_managers = [ fcl.DynamicAABBTreeCollisionManager() for _ in range(num_class) ] obj_by_cls = [[] for _ in range(num_class)] for obj in fcl_obs: obj_by_cls[obj.category].append(obj.cobj) for mng, obj_group in zip(obs_managers, obj_by_cls): mng.registerObjects(obj_group) robot_links = robot.update_polygons(cfgs[0]) robot_manager = fcl.DynamicAABBTreeCollisionManager() robot_manager.registerObjects(robot_links) robot_manager.setup() for mng in obs_managers: mng.setup() req = fcl.CollisionRequest(num_max_contacts=1000, enable_contact=True) times = [] st = time() for i, cfg in enumerate(cfgs): st1 = time() robot.update_polygons(cfg) robot_manager.update() assert len(robot_manager.getObjects()) == robot.dof for cat, obs_mng in enumerate(obs_managers): rdata = fcl.CollisionData(request=req) robot_manager.collide(obs_mng, rdata, fcl.defaultCollisionCallback) in_collision = rdata.result.is_collision ddata = fcl.DistanceData() robot_manager.distance(obs_mng, ddata, fcl.defaultDistanceCallback) depths = torch.FloatTensor( [c.penetration_depth for c in rdata.result.contacts]) labels[i, cat] = 1 if in_collision else -1 dists[i, cat] = depths.abs().max( ) if in_collision else -ddata.result.min_distance end1 = time() times.append(end1 - st1) end = time() times = np.array(times) print('std: {}, mean {}, avg {}'.format(times.std(), times.mean(), (end - st) / len(cfgs))) in_collision = (labels == 1).sum(1) > 0 if label_type == 'binary': labels = labels.squeeze_(1) dists = dists.squeeze_(1) print('env_id {}, {} collisons, {} free'.format( env_id, torch.sum(in_collision == 1), torch.sum(in_collision == 0))) if torch.sum(in_collision == 1) == 0: print('0 Collision. You may want to regenerate env {}obs{}'.format( obs_num, env_id)) dataset = { 'data': cfgs, 'label': labels, 'dist': dists, 'obs': obstacles, 'robot': robot.__class__, 'rparam': [ robot.link_length, robot.link_width, robot.dof, ] } torch.save( dataset, os.path.join( folder, '2d_{}dof_{}obs_{}_{}.pt'.format(robot.dof, obs_num, label_type, env_id))) if vis: create_plots(robot, obstacles, cfg=None) plt.savefig( os.path.join( folder, '2d_{}dof_{}obs_{}_{}.png'.format(robot.dof, obs_num, label_type, env_id))) plt.close() return
# [0.0, 0.0, 1.0]]) R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) T = np.array([1.0, 1.865, 0]) g1 = fcl.Box(1, 2, 3) t1 = fcl.Transform() o1 = fcl.CollisionObject(g1, t1) # g2 = fcl.Cone(1,3) g2 = fcl.Cylinder(0.01, 1000) t2 = fcl.Transform() o2 = fcl.CollisionObject(g2, t2) # request = fcl.DistanceRequest(gjk_solver_type=fcl.GJKSolverType.GST_INDEP) # result = fcl.DistanceResult() request = fcl.CollisionRequest(enable_contact=True) result = fcl.CollisionResult() # ret = fcl.distance(o1, o2, request, result) # ret = fcl.collide(o1, o2, request, result) size = 50, 50 yy, xx = torch.meshgrid(torch.linspace(-5, 5, size[0]), torch.linspace(-5, 5, size[1])) grid_points = torch.stack([xx, yy], axis=2).reshape((-1, 2)) grid_labels = torch.zeros_like(grid_points)[:, 0] for i, (x, y) in enumerate(grid_points): print(x, y) o2.setTranslation([x, y, 0]) fcl.update() ret = fcl.collide(o1, o2, request, result)
def collision(ball1, ball2, ball3, ball4): geom1 = fcl.Sphere(1.0) geom2 = fcl.Sphere(1.0) geom3 = fcl.Sphere(1.0) geom4 = fcl.Sphere(1.0) T1 = [ball1.pos.x, ball1.pos.y, ball1.pos.z] t1 = fcl.Transform(T1) obj1 = fcl.CollisionObject(geom1, t1) T2 = [ball2.pos.x, ball2.pos.y, ball2.pos.z] t2 = fcl.Transform(T2) obj2 = fcl.CollisionObject(geom2, t2) T3 = [ball3.pos.x, ball3.pos.y, ball3.pos.z] t3 = fcl.Transform(T3) obj3 = fcl.CollisionObject(geom3, t3) T4 = [ball4.pos.x, ball4.pos.y, ball4.pos.z] t4 = fcl.Transform(T4) obj4 = fcl.CollisionObject(geom4, t4) geoms = [geom1, geom2, geom3, geom4] objs = [obj1, obj2, obj3, obj4] names = ['obj1', 'obj2', 'obj3', 'obj4'] # Create map from geometry IDs to objects geom_id_to_obj = {id(geom): obj for geom, obj in zip(geoms, objs)} # Create map from geometry IDs to string names geom_id_to_name = {id(geom): name for geom, name in zip(geoms, names)} # Create manager manager = fcl.DynamicAABBTreeCollisionManager() manager.registerObjects(objs) manager.setup() # Create collision request structure crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) cdata = fcl.CollisionData(crequest, fcl.CollisionResult()) # Run collision request manager.collide(cdata, fcl.defaultCollisionCallback) # Extract collision data from contacts and use that to infer set of # objects that are in collision objs_in_collision = set() for contact in cdata.result.contacts: # Extract collision geometries that are in contact coll_geom_0 = contact.o1 coll_geom_1 = contact.o2 # Get their names coll_names = [ geom_id_to_name[id(coll_geom_0)], geom_id_to_name[id(coll_geom_1)] ] coll_names = tuple(sorted(coll_names)) objs_in_collision.add(coll_names) for coll_pair in objs_in_collision: print('Object {} in collision with object {}!'.format( coll_pair[0], coll_pair[1])) return objs_in_collision
# # Managed internal (sub-n^2) distance checking # #===================================================================== # ddata = fcl.DistanceData() # manager1.distance(ddata, fcl.defaultDistanceCallback) # print('Closest distance within manager 1?: {}'.format(ddata.result.min_distance)) #===================================================================== # Managed one to many collision checking #===================================================================== # req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) # rdata = fcl.CollisionData(request = req) # manager1.collide(fcl.CollisionObject(m), rdata, fcl.defaultCollisionCallback) # print('Collision between manager 1 and Mesh?: {}'.format(rdata.result.is_collision)) # print('Contacts:') # for c in rdata.result.contacts: # print('\tO1: {}, O2: {}, depth: {}'.format(c.o1, c.o2, c.penetration_depth)) #===================================================================== # Managed many to many collision checking #===================================================================== req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) rdata = fcl.CollisionData(request=req) manager1.collide(manager2, rdata, fcl.defaultCollisionCallback) print('Collision between manager 1 and manager 2?: {}'.format( rdata.result.is_collision)) print('Contacts:') for c in rdata.result.contacts: print('\tO1: {}, O2: {}, depth: {}, pos: {}, normal: {}'.format( c.o1, c.o2, c.penetration_depth, c.pos, c.normal)) print(1 - 2 / np.sqrt(5))