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 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 __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 __init__(self, collision_dict): Collision_Object.__init__(self, collision_dict) if not len(self.params) == 2: raise TypeError(bc.FAIL + 'ERROR: parameters for collision cylinder must be list of 2 floats.' + bc.ENDC) self.r, self.lz = self.params[0], self.params[1] self.g = fcl.Cylinder(self.r, self.lz) self.t = fcl.Transform() self.obj = fcl.CollisionObject(self.g, self.t) self.make_rviz_marker()
def __init__(self, shape, position, size, category=None): self.size = size self.position = position if shape == 'circle': pos_3d = torch.FloatTensor([position[0], position[1], 0]) self.geom = fcl.Cylinder(size, 1000) elif shape == 'rect': pos_3d = torch.FloatTensor([position[0], position[1], 0]) self.geom = fcl.Box(size[0], size[1], 1000) self.cobj = fcl.CollisionObject(self.geom, fcl.Transform(pos_3d)) self.category = category
def _build_tube(curve, rad): """Generates object list from given curve and radius lists. Creates a cylinder with given radius between every pair of [x, y, z] points. Each cylinder is initialized with the given radius and the computed length and given a transformation to move it from the origin (cylinders are initially centered on the origin in every axis) to the points, where the points are in the center of each face of the cylinder. Parameters ---------- curve : list of list of 4x4 numpy arrays (SE3) list of SE3 for each curve, with points given in last column rad : list of float radii of the tubes Returns ------- list of CollisionObject collection of cylinders that discretize the curve/tube """ tube = [] for n in range(len(curve)): for index, from_g in enumerate(curve[n][:-1]): to_g = curve[n][index + 1] from_point = from_g[0:3, 3] to_point = to_g[0:3, 3] vec = [(t - f) for f, t in zip(from_point, to_point)] mid_point = [(f + v / 2) for f, v in zip(from_point, vec)] length = norm(vec) unit_vec = vec / length cyl = fcl.Cylinder(rad[n], length) if unit_vec[2] == -1.0: # if vector is in -z direction unit_quat = [0, 1, 0, 0] # gives 180 degree rotation else: # quat = [1 + dot(init_vec, unit_vec), cross(init_vec, unit_vec)] # where init_vec is the z-axis unit vector [0, 0, 1] quat = [1 + unit_vec[2], -unit_vec[1], unit_vec[0], 0] quat_mag = norm(quat) unit_quat = [q / quat_mag for q in quat] translate = np.array(mid_point) rotate = np.array(unit_quat) transform = fcl.Transform(rotate, translate) obj = fcl.CollisionObject(cyl, transform) tube.append(obj) return tube
def parse_json(object_type, init_objects_file): f = init_objects_file.open() full_json = json.load(f) f.close() list_objects = full_json.get(object_type) collision_objects = [] for o in list_objects: if o.get('mesh'): m = fcl.BVHModel() path = init_objects_file.parent file = path / o.get('filename') vertices, tris = parse_mesh(str(file)) m.beginModel(len(vertices), len(tris)) m.addSubModel(vertices, tris) m.endModel() this_transform = fcl.Transform(o.get('transform')) obj = fcl.CollisionObject(m, this_transform) else: if o.get('shape') == 'Sphere': this_obj = fcl.Sphere(o.get('radius')) this_transform = fcl.Transform(o.get('transform')) elif o.get('shape') == 'Cylinder': this_obj = fcl.Cylinder(o.get('radius'), o.get('length')) direction = o.get('direction') rot = rotate_align(np.asarray(direction)) this_transform = fcl.Transform(rot, o.get('transform')) elif o.get('shape') == 'Box': this_obj = fcl.Box(o.get('x_length'), o.get('y_length'), o.get('z_length')) this_transform = fcl.Transform(o.get('transform')) else: raise NotImplementedError( f'Shape type {o.get("shape")} not supported.') obj = fcl.CollisionObject(this_obj, this_transform) collision_objects.append(obj) return collision_objects
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 point2fcl(self, p): point = fcl.Cylinder(0.01, 1) tf = fcl.Transform([p[0], p[1], 0]) return fcl.CollisionObject(point, tf)
def lidar_configuration(trans_x, trans_y, radius): rotation = np.array([[1,0,0],[0,1,0],[0,0,1]]) translation = np.array([trans_x, trans_y, 0.0]) Transform = fcl.Transform(rotation, translation) lidar = fcl.CollisionObject(fcl.Cylinder(radius, 0), Transform) return lidar
v1 = np.array([1.0, 2.0, 3.0]) v2 = np.array([2.0, 1.0, 3.0]) v3 = np.array([3.0, 2.0, 1.0]) x, y, z = 1.0, 2.0, 3.0 rad, lz = 1.0, 3.0 n = np.array([1.0, 0.0, 0.0]) d = 5.0 t = fcl.TriangleP(v1, v2, v3) # Triangle defined by three points b = fcl.Box(x, y, z) # Axis-aligned box with given side lengths s = fcl.Sphere(rad) # Sphere with given radius e = fcl.Ellipsoid(x, y, z) # Axis-aligned ellipsoid with given radii # c = fcl.Capsule(rad, lz) # Capsule with given radius and height along z-axis # c = fcl.Cone(rad, lz) # Cone with given radius and cylinder height along z-axis c = fcl.Cylinder(rad, lz) # Cylinder with given radius and height along z-axis h = fcl.Halfspace(n, d) # Half-space defined by {x : <n, x> < d} p = fcl.Plane(n, d) # Plane defined by {x : <n, x> = d} verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0], [1.0, 1.0, 2.0]]) tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]]) m = fcl.BVHModel() m.beginModel(len(verts), len(tris)) m.addSubModel(verts, tris) m.endModel() objs1 = [ fcl.CollisionObject( b,
import numpy as np import fcl import torch # R = np.array([[0.0, -1.0, 0.0], # [1.0, 0.0, 0.0], # [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))
self.fkine_backup = torch.stack([t[:, :3, 3] for t in cum_tfs], dim=1) return self.fkine_backup if __name__ == "__main__": lw_data = 0.3 robot = RevolutePlanarRobot(1, dof=7, link_width=lw_data) num_frames = 100 q = 2 * (torch.rand(num_frames, 7) - 0.5) * np.pi / 1.5 points = robot.fkine(q) points = torch.cat([torch.zeros(num_frames, 1, 2), points], axis=1) print(points) robot_links = robot.update_polygons(q[0]) cir_pos = torch.FloatTensor([2, 2, 0]) obs = [fcl.CollisionObject(fcl.Cylinder(1, 1), fcl.Transform(cir_pos))] robot_manager = fcl.DynamicAABBTreeCollisionManager() obs_manager = fcl.DynamicAABBTreeCollisionManager() robot_manager.registerObjects(robot_links) obs_manager.registerObjects(obs) robot_manager.setup() obs_manager.setup() req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) rdata = fcl.CollisionData(request=req) robot_manager.collide(obs_manager, rdata, fcl.defaultCollisionCallback) in_collision = rdata.result.is_collision from matplotlib import pyplot as plt import seaborn as sns sns.set() import matplotlib.patheffects as path_effects
translation = np.array([0, 0, 0.0]) Transform = fcl.Transform(rotation, translation) 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)
def print_distance_result(o1_name, o2_name, result): print('Distance between {} and {}:'.format(o1_name, o2_name)) print('-' * 30) print('Distance: {}'.format(result.min_distance)) print('Closest Points:') print(result.nearest_points[0]) print(result.nearest_points[1]) print() # Create simple geometries box = fcl.Box(1.0, 2.0, 3.0) sphere = fcl.Sphere(4.0) cone = fcl.Cone(5.0, 6.0) cyl = fcl.Cylinder(2.0, 2.0) verts = np.array([[1.0, 1.0, 1.0], [2.0, 1.0, 1.0], [1.0, 2.0, 1.0], [1.0, 1.0, 2.0]]) tris = np.array([[0, 2, 1], [0, 3, 2], [0, 1, 3], [1, 2, 3]]) # Create mesh geometry mesh = fcl.BVHModel() mesh.beginModel(len(verts), len(tris)) mesh.addSubModel(verts, tris) mesh.endModel() #===================================================================== # Pairwise collision checking #===================================================================== print('=' * 60)
import fcl import numpy as np # Create collision geometry and objects geom1 = fcl.Cylinder(1.0, 1.0) obj1 = fcl.CollisionObject(geom1) geom2 = fcl.Cylinder(1.0, 1.0) obj2 = fcl.CollisionObject(geom2, fcl.Transform(np.array([0.0, 0.0, 0.3]))) geom3 = fcl.Cylinder(1.0, 1.0) obj3 = fcl.CollisionObject(geom3, fcl.Transform(np.array([0.0, 0.0, 3.0]))) geoms = [geom1, geom2, geom3] objs = [obj1, obj2, obj3] names = ['obj1', 'obj2', 'obj3'] # 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())