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 test_many_objects(self): manager = fcl.DynamicAABBTreeCollisionManager() objs = [ fcl.CollisionObject(self.geometry['sphere']), fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, -5.0]))), fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, 5.0]))) ] manager.registerObjects(objs) manager.setup() self.assertTrue(len(manager.getObjects()) == 3) # Many-to-many, internal cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager.collide(cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision) objs[1].setTranslation(np.array([0.0, 0.0, -0.3])) manager.update(objs[1]) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager.collide(cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision) objs[1].setTranslation(np.array([0.0, 0.0, -5.0])) manager.update(objs[1]) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager.collide(cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision) objs[2].setTranslation(np.array([0.0, 0.0, 0.3])) manager.update(objs[2]) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager.collide(cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision) objs[2].setTranslation(np.array([0.0, 0.0, 5.0])) manager.update(objs[2]) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager.collide(cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision)
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 test_pairwise_collisions(self): result = fcl.CollisionResult() box = fcl.CollisionObject(self.geometry['box']) cone = fcl.CollisionObject(self.geometry['cone']) mesh = fcl.CollisionObject(self.geometry['mesh']) result = fcl.CollisionResult() ret = fcl.collide(box, cone, self.crequest, result) self.assertTrue(ret > 0) self.assertTrue(result.is_collision) result = fcl.CollisionResult() ret = fcl.collide(box, mesh, self.crequest, result) self.assertTrue(ret > 0) self.assertTrue(result.is_collision) result = fcl.CollisionResult() ret = fcl.collide(cone, mesh, self.crequest, result) self.assertTrue(ret > 0) self.assertTrue(result.is_collision) cone.setTranslation(np.array([0.0, 0.0, -0.6])) result = fcl.CollisionResult() ret = fcl.collide(box, cone, self.crequest, result) self.assertTrue(ret > 0) self.assertTrue(result.is_collision) result = fcl.CollisionResult() ret = fcl.collide(cone, mesh, self.crequest, result) self.assertTrue(ret == 0) self.assertFalse(result.is_collision) cone.setTranslation(np.array([0.0, -0.9, 0.0])) cone.setRotation(self.x_axis_rot) result = fcl.CollisionResult() ret = fcl.collide(box, cone, self.crequest, result) self.assertTrue(ret > 0) self.assertTrue(result.is_collision) cone.setTranslation(np.array([0.0, -1.1, 0.0])) result = fcl.CollisionResult() ret = fcl.collide(box, cone, self.crequest, result) self.assertTrue(ret == 0) self.assertFalse(result.is_collision)
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 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 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
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): # self.mesh = mesh.Mesh.from_file(os.environ["CODE_BASE"] + "/catkin_ws/src/" + mesh_name)
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
def test_managed_collisions(self): manager1 = fcl.DynamicAABBTreeCollisionManager() manager2 = fcl.DynamicAABBTreeCollisionManager() manager3 = fcl.DynamicAABBTreeCollisionManager() objs1 = [ fcl.CollisionObject(self.geometry['box']), fcl.CollisionObject(self.geometry['cylinder']) ] objs2 = [ fcl.CollisionObject(self.geometry['cone'], fcl.Transform(np.array([0.0, 0.0, 5.0]))), fcl.CollisionObject(self.geometry['cylinder'], fcl.Transform(np.array([0.0, 0.0, -5.0]))) ] objs3 = [ fcl.CollisionObject(self.geometry['mesh']), fcl.CollisionObject(self.geometry['box'], fcl.Transform(np.array([0.0, 0.0, -5.0]))) ] manager1.registerObjects(objs1) manager2.registerObjects(objs2) manager3.registerObjects(objs3) manager1.setup() manager2.setup() manager3.setup() self.assertTrue(len(manager1.getObjects()) == 2) self.assertTrue(len(manager2.getObjects()) == 2) self.assertTrue(len(manager3.getObjects()) == 2) # One-to-many o1 = fcl.CollisionObject(self.geometry['box']) o2 = fcl.CollisionObject(self.geometry['cylinder'], fcl.Transform(np.array([0.0, 0.0, -4.6]))) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager1.collide(o1, cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager1.collide(o2, cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager2.collide(o1, cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager2.collide(o2, cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision) # Many-to-many, internal cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager1.collide(cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager2.collide(cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision) # Many-to-many, grouped cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager1.collide(manager2, cdata, fcl.defaultCollisionCallback) self.assertFalse(cdata.result.is_collision) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager2.collide(manager3, cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision) cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult()) manager1.collide(manager3, cdata, fcl.defaultCollisionCallback) self.assertTrue(cdata.result.is_collision)
box1 = fcl.CollisionObject(fcl.Box(1, 1, 0.0), Transform) #x,y,z length center at the origin #box.setTranslation(np.array([0.0, 1.05000, 0.0])) #useful #box.setRotation(rotation) #useful #other options: setTransform setQuatRotation rota_degree = 0 rotation = np.array([[np.cos(rota_degree/180*np.pi), -np.sin(rota_degree/180*np.pi), 0.0], [np.sin(rota_degree/180*np.pi), np.cos(rota_degree/180*np.pi), 0.0], [0.0, 0.0, 1.0]]) translation = np.array([1.4999, 0, 0.0]) Transform = fcl.Transform(rotation, translation) box2 = fcl.CollisionObject(fcl.Box(3, 3, 0.0), Transform) #x,y,z length center at the origin cylinder = fcl.CollisionObject(fcl.Cylinder(1, 0.0),Transform) #radius = 1 objs = [box1,cylinder] manager = fcl.DynamicAABBTreeCollisionManager() manager.registerObjects(objs) manager.setup() crequest = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) cdata = fcl.CollisionData(crequest, fcl.CollisionResult()) manager.collide(cdata, fcl.defaultCollisionCallback) print(cdata.result.contacts) for contact in cdata.result.contacts: print(contact.pos) print(contact.normal) print(contact.penetration_depth)