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 is_in_collision(self, tf, other, tf_other): """ Collision checking with another shape for the given transforms. """ fcl_tf_1 = fcl.Transform(tf[:3, :3], tf[:3, 3]) fcl_tf_2 = fcl.Transform(tf_other[:3, :3], tf_other[:3, 3]) o1 = fcl.CollisionObject(self.fcl_shape, fcl_tf_1) o2 = fcl.CollisionObject(other.fcl_shape, fcl_tf_2) return fcl.collide(o1, o2, self.request, self.result)
def test_pairwise_continuous_collisions(self): request = fcl.ContinuousCollisionRequest() result = fcl.ContinuousCollisionResult() box = fcl.CollisionObject(self.geometry['box']) cone = fcl.CollisionObject(self.geometry['cone'], fcl.Transform(np.array([0.0, 0.0, -2.0]))) ret = fcl.continuousCollide(box, fcl.Transform(), cone, fcl.Transform(), request, 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 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 in_collision_single(self, mesh, transform=None): ''' 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 Returns ------- is_collision: bool, True if a collision occurs and False otherwise ''' 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() self._manager.collide(o, cdata, fcl.defaultCollisionCallback) return cdata.result.is_collision
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 collision_check(self, x, y): t2 = fcl.Transform(np.array([x, y, 0])) o2 = fcl.CollisionObject(self.g2, t2) request = fcl.ContinuousCollisionRequest() result = fcl.ContinuousCollisionResult() ret = fcl.continuousCollide(self.o1, self.t1, o2, t2, request, result) return ret == 0.0
def __init__(self): self.check_collision = True self.ang_comp = False self.ang_comp2 = False self.lin_comp = False self.init = False self.init_arm = False self.angG = 0 self.ang = 0 self.send_g = True self.status = 0 self.goal_id = 0 self.ret = np.zeros(7) self.obj = [ fcl.CollisionObject(fcl.Sphere(0.5), fcl.Transform()) for i in range(7) ] self.quaternion = np.zeros((8, 4)) self.translation = np.zeros((8, 3)) for i in range(8): self.quaternion[i] = np.array([0, 0, 0, 0]) self.translation[i] = np.array([0, 0, 0]) self.quaternionG = np.zeros((11, 4)) self.translationG = np.zeros((11, 3)) for i in range(10): self.quaternionG[i] = np.array([0, 0, 0, 0]) self.translationG[i] = np.array([0, 0, 0]) self.set_goal()
def add_object(self, name, mesh, transform=None): ''' Add an object to the collision manager. If an object with the given name is already in the manager, replace it. Parameters ---------- name: str, an identifier for the object mesh: Trimesh object, the geometry of the collision object transform: (4,4) float, homogenous transform matrix for the object ''' 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) # Add collision object to set if name in self._objs: self._manager.unregisterObject(self._objs[name]) self._objs[name] = {'obj': o, 'geom': b} self._manager.registerObject(o) self._manager.update()
def is_path_in_collision(self, tf, tf_target, other, tf_other): # assert np.sum(np.abs(tf - tf_target)) > 1e-12 fcl_tf_1 = fcl.Transform(tf[:3, :3], tf[:3, 3]) fcl_tf_2 = fcl.Transform(tf_other[:3, :3], tf_other[:3, 3]) fcl_tf_1_target = fcl.Transform(tf_target[:3, :3], tf_target[:3, 3]) o1 = fcl.CollisionObject(self.fcl_shape, fcl_tf_1) o2 = fcl.CollisionObject(other.fcl_shape, fcl_tf_2) self.c_req.ccd_motion_type = fcl.CCDMotionType.CCDM_LINEAR fcl.continuousCollide(o1, fcl_tf_1_target, o2, fcl_tf_2, self.c_req, self.c_res) return self.c_res.is_collide
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 __init__(self, collision_dict): Collision_Object.__init__(self, collision_dict) if not type(self.params) == float: raise TypeError(bc.FAIL + 'ERROR: parameters for collision sphere must be a float value (for radius).' + bc.ENDC) self.r = self.params self.g = fcl.Sphere(self.r) self.t = fcl.Transform() self.obj = fcl.CollisionObject(self.g, self.t) self.make_rviz_marker()
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, collision_dict): Collision_Object.__init__(self, collision_dict) if not len(self.params) == 3: raise TypeError(bc.FAIL + 'ERROR: parameters for collision ellipsoid must be list of 3 floats.' + bc.ENDC) self.x, self.y, self.z = self.params[0], self.params[1], self.params[2] self.g = fcl.Ellipsoid(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 update_transform(self, translation, rotation): if len(translation) == 1: translation = translation[0] self.t = fcl.Transform(rotation, translation) self.obj.setTransform(self.t) self.marker.pose.position.x = translation[0] self.marker.pose.position.y = translation[1] self.marker.pose.position.z = translation[2] self.marker.pose.orientation.w = rotation[0] self.marker.pose.orientation.x = rotation[1] self.marker.pose.orientation.y = rotation[2] self.marker.pose.orientation.z = rotation[3]
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 __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 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 __init__(self, collision_dict): Collision_Object.__init__(self, collision_dict) if not len(self.params) == 2: raise TypeError(bc.FAIL + 'ERROR: parameters for collision mesh must be a dictionary consisting of a list of verts and tris.' + bc.ENDC) if not len(self.params['verts']) == len(self.params['tris']): raise TypeError(bc.FAIL + 'ERROR: number of tris must equal the number of verts in collision mesh.' + bc.ENDC) self.verts = np.array(self.params['verts']) self.tris = np.array(self.params['tris']) self.g = fcl.BVHModel() self.g.beginModel(len(self.verts), len(self.tris)) self.g.addSubModel(self.verts, self.tris) self.g.endModel() self.t = fcl.Transform() self.obj = fcl.CollisionObject(self.g, self.t) self.make_rviz_marker()
def add_object(self, name, mesh, transform=None): """ Add an object to the collision manager. If an object with the given name is already in the manager, replace it. Parameters ---------- name : str An identifier for the object mesh : Trimesh object The geometry of the collision object transform : (4,4) float Homogeneous transform matrix for the object """ # if no transform passed, assume identity transform if transform is None: transform = np.eye(4) transform = np.asanyarray(transform, dtype=np.float32) if transform.shape != (4, 4): raise ValueError('transform must be (4,4)!') # create or recall from cache BVH bvh = self._get_BVH(mesh) # create the FCL transform from (4,4) matrix t = fcl.Transform(transform[:3, :3], transform[:3, 3]) o = fcl.CollisionObject(bvh, t) # Add collision object to set if name in self._objs: self._manager.unregisterObject(self._objs[name]) self._objs[name] = {'obj': o, 'geom': bvh} # store the name of the geometry self._names[id(bvh)] = name self._manager.registerObject(o) self._manager.update() return o
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 min_distance_single(self, mesh, transform=None, return_name=False): ''' Get the minimum distance between a single object and any object 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, return name of the closest object Returns ------- distance: float, Min distance between mesh and any object in the manager name: str, The name of the object in the manager that was closest ''' 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 ddata = fcl.DistanceData() self._manager.distance(o, ddata, fcl.defaultDistanceCallback) distance = ddata.result.min_distance # If we want to return the objects that were collision, collect them. if return_name: cg = ddata.result.o1 if cg == b: cg = ddata.result.o2 name = self._extract_name(cg) return distance, name else: return distance
def check_collision(self, curve, rad): """Determine if the curve given collides with obstacles or goal. The curve is translated to discretized cylinders with the given radius and checked for collisions with the obstacles and goal. Minimum distance to obstacles and tip distance to goal are returned, with a negative value denoting a collision. Parameters ---------- curve : list of list of 4x4 numpy arrays The SE3 g values for each curve rad : list of float radii of the tubes Returns ------- float minimum distance between curve and obstacles float minimum distance between curve and goal """ tube = self._build_tube(curve, rad) tube_manager = fcl.DynamicAABBTreeCollisionManager() tube_manager.registerObjects(tube) tube_manager.setup() obstacle_min = self._distance_check(tube_manager, self.obstacles) s = fcl.Sphere(rad[-1]) final_point = curve[-1][-1][0:3, 3] t = fcl.Transform(final_point) # coordinates of last point of tube tip = fcl.CollisionObject(s, t) request = fcl.DistanceRequest() result = fcl.DistanceResult() goal_dist = fcl.distance(tip, self.goal, request, result) return obstacle_min, goal_dist
def min_distance_single(self, mesh, transform=None, return_name=False, return_data=False): """ Get the minimum distance between a single object and any object 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, return name of the closest object return_data: bool, If true, a DistanceData object is returned as well Returns ------- distance: float, Min distance between mesh and any object in the manager name: str, The name of the object in the manager that was closest data: DistanceData, Extra data about the distance query """ 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 ddata = fcl.DistanceData() if return_data: ddata = fcl.DistanceData( fcl.DistanceRequest(enable_nearest_points=True), fcl.DistanceResult()) self._manager.distance(o, ddata, fcl.defaultDistanceCallback) distance = ddata.result.min_distance # If we want to return the objects that were collision, collect them. name, data = None, None if return_name or return_data: cg = ddata.result.o1 if cg == b: cg = ddata.result.o2 name = self._extract_name(cg) names = (name, '__external') if cg == ddata.result.o2: names = reversed(names) data = DistanceData(names, ddata.result) if return_name and return_data: return distance, name, data elif return_name: return distance, name elif return_data: return distance, data else: return distance
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
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