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 update_polygons(self, q): joints = self.fkine(q)[0] joints = torch.cat([torch.zeros(1, 2, dtype=joints.dtype), joints], dim=0) centers = (joints[:-1] + joints[1:]) / 2 q = torch.reshape(q, (-1, self.dof)) angles = torch.cumsum(q, dim=1)[0] if self.collision_objs is None: self.collision_objs = [] for trans, angle, l in zip(centers, angles, self.link_length): obj = fcl.Box(l, self.link_width, 1000) self.collision_objs.append( fcl.CollisionObject( obj, fcl.Transform( Rotation.from_rotvec([0, 0, angle ]).as_quat()[[3, 0, 1, 2]], [trans[0], trans[1], 0]))) else: for obj, trans, angle, l in zip(self.collision_objs, centers, angles, self.link_length): obj.setTransform( fcl.Transform( Rotation.from_rotvec([0, 0, angle]).as_quat()[[3, 0, 1, 2]], [trans[0], trans[1], 0])) # self.collsion_objs.append(fcl.CollisionObject(obj, fcl.Transform( # tgm.angle_axis_to_quaternion(torch.FloatTensor([0, 0, angle])), # [point[0], point[1], 0]))) return self.collision_objs
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, 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 __init__(self, collision_dict): Collision_Object.__init__(self, collision_dict) if not len(self.params) == 3: raise TypeError(bc.FAIL + 'ERROR: parameters for collision box must be list of 3 floats.' + bc.ENDC) self.x = self.params[0] self.y = self.params[1] self.z = self.params[2] self.g = fcl.Box(self.x, self.y, self.z) self.t = fcl.Transform() self.obj = fcl.CollisionObject(self.g, self.t) self.make_rviz_marker()
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 obs2fcl(obs, dimW): obs_fcl = [] for i in range(0, obs.shape[0] // (2 * dimW)): # plot obstacle patches width = obs[i * 2 * dimW + dimW] - obs[i * 2 * dimW] height = obs[i * 2 * dimW + dimW + 1] - obs[i * 2 * dimW + 1] # width = height = 1 ob = fcl.Box(width, height, 1) x = obs[i * 2 * dimW] + width/2 y = obs[i * 2 * dimW + 1] + height/2 # x = y = 0 T = np.array([x, y, 0]) t = fcl.Transform(T) co = fcl.CollisionObject(ob, t) obs_fcl.append(co) # print("x: ", x, " y: ", y, " width: ", width, " height: ", height) obs_manager = fcl.DynamicAABBTreeCollisionManager() obs_manager.registerObjects(obs_fcl) obs_manager.setup() return obs_manager
def stick_configuration(rota_degree, trans_x, trans_y, length, width, top_left, top_right, bottom_left, bottom_right): 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([trans_x, trans_y, 0.0]) Transform = fcl.Transform(rotation, translation) #before transform stick = fcl.CollisionObject(fcl.Box(length, width, 0.0), Transform) #x,y,z length center at the origin #after transform transform_matrix = np.c_[rotation, translation] transform_matrix = np.row_stack((transform_matrix, np.array([0, 0, 0, 1]))) top_left_homo = np.r_[top_left, np.array([ 1, ])] top_right_homo = np.r_[top_right, np.array([ 1, ])] bottom_left_homo = np.r_[bottom_left, np.array([ 1, ])] bottom_right_homo = np.r_[bottom_right, np.array([ 1, ])] top_left_trans = transform_matrix @ top_left_homo top_left_trans = top_left_trans[:2] top_right_trans = transform_matrix @ top_right_homo top_right_trans = top_right_trans[:2] bottom_left_trans = transform_matrix @ bottom_left_homo bottom_left_trans = bottom_left_trans[:2] bottom_right_trans = transform_matrix @ bottom_right_homo bottom_right_trans = bottom_right_trans[:2] return transform_matrix, [ top_left_trans, top_right_trans, bottom_right_trans, bottom_left_trans ], stick
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 create_fcl_objs(self, offset): self.g1 = fcl.Box(self.width, self.height, 3) self.t1 = fcl.Transform(np.array(self.center)) self.o1 = fcl.CollisionObject(self.g1, self.t1) self.g2 = fcl.Sphere(offset)
def rec2fcl(self, rec): box = fcl.Box(rec[2], rec[3], 1.0) tf = fcl.Transform([rec[0], rec[1], 0]) return fcl.CollisionObject(box, tf)
import numpy as np import fcl from scipy.spatial.transform import Rotation 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()
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))
import fcl import numpy as np 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([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)
print('Time of collision: {}'.format(result.time_of_contact)) print() 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() #=====================================================================
import numpy as np import fcl g1 = fcl.Box(2, 2, 2) trans = np.array([0, 0.0, 0.0]) t1 = fcl.Transform(trans) print("g1:", id(g1)) o1 = fcl.CollisionObject(g1, t1) b = fcl.Box(2, 2, 2) trans = np.array([3, 0.0, 0.0]) t = fcl.Transform(trans) print("b:", id(b)) o = fcl.CollisionObject(b, t) g3 = fcl.Box(2, 2, 2) trans = np.array([7, 0.0, 0.0]) t3 = fcl.Transform(trans) print("g3:", id(g3)) o5 = fcl.CollisionObject(g3, t3) # request = fcl.CollisionRequest() # result = fcl.CollisionResult() objs1 = [o1, o5, o] req = fcl.CollisionRequest(num_max_contacts=100, enable_contact=True) rdata = fcl.CollisionData(request=req) manager1 = fcl.DynamicAABBTreeCollisionManager() manager1.registerObjects(objs1) manager1.setup()