def min_distance_other(self, other_manager, return_names=False, return_data=False): """ Get the minimum distance between any pair of objects, one in each manager. Parameters ---------- other_manager: CollisionManager, another collision manager object return_names: bool, If true, a 2-tuple is returned containing the names of the closest objects. return_data: bool, If true, a DistanceData object is returned as well Returns ------- distance: float, The min distance between a pair of objects, one from each manager. names: 2-tup of str, A 2-tuple containing two names (first from this manager, second from the other_manager) indicating the two closest objects. data: DistanceData, Extra data about the distance query """ ddata = fcl.DistanceData() if return_data: ddata = fcl.DistanceData( fcl.DistanceRequest(enable_nearest_points=True), fcl.DistanceResult()) self._manager.distance(other_manager._manager, ddata, fcl.defaultDistanceCallback) distance = ddata.result.min_distance names, data = None, None if return_names or return_data: reverse = False names = (self._extract_name(ddata.result.o1), other_manager._extract_name(ddata.result.o2)) if names[0] is None: reverse = True names = (self._extract_name(ddata.result.o2), other_manager._extract_name(ddata.result.o1)) dnames = tuple(names) if reverse: dnames = reversed(dnames) data = DistanceData(dnames, ddata.result) if return_names and return_data: return distance, names, data elif return_names: return distance, names elif return_data: return distance, data else: return distance
def min_distance_other(self, other_manager, return_names=False): ''' Get the minimum distance between any pair of objects, one in each manager. Parameters ---------- other_manager: CollisionManager, another collision manager object return_names: bool, If true, a 2-tuple is returned containing the names of the closest objects. Returns ------- distance: float, The min distance between a pair of objects, one from each manager. names: 2-tup of str, A 2-tuple containing two names (first from this manager, second from the other_manager) indicating the two closest objects. ''' ddata = fcl.DistanceData() self._manager.distance(other_manager._manager, ddata, fcl.defaultDistanceCallback) distance = ddata.result.min_distance if return_names: name1, name2 = (self._extract_name(ddata.result.o1), other_manager._extract_name(ddata.result.o2)) if name1 is None: name1, name2 = (self._extract_name(ddata.result.o2), other_manager._extract_name(ddata.result.o1)) return distance, (name1, name2) else: return distance
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 min_distance_internal(self, return_names=False): ''' Get the minimum distance between any pair of objects in the manager. Parameters ---------- return_names : bool If true, a 2-tuple is returned containing the names of the closest objects. Returns ------- distance: float, Min distance between any two managed objects names: (2,) str, The names of the closest objects ''' ddata = fcl.DistanceData() self._manager.distance(ddata, fcl.defaultDistanceCallback) distance = ddata.result.min_distance if return_names: names = tuple( sorted((self._extract_name(ddata.result.o1), self._extract_name(ddata.result.o2)))) return distance, names else: return distance
def min_distance_internal(self, return_names=False, return_data=False): """ Get the minimum distance between any pair of objects in the manager. Parameters ------------- return_names : bool If true, a 2-tuple is returned containing the names of the closest objects. return_data : bool If true, a DistanceData object is returned as well Returns ----------- distance : float Min distance between any two managed objects names : (2,) str The names of the closest objects data : DistanceData Extra data about the distance query """ ddata = fcl.DistanceData() if return_data: ddata = fcl.DistanceData( fcl.DistanceRequest(enable_nearest_points=True), fcl.DistanceResult() ) self._manager.distance(ddata, fcl.defaultDistanceCallback) distance = ddata.result.min_distance names, data = None, None if return_names or return_data: names = (self._extract_name(ddata.result.o1), self._extract_name(ddata.result.o2)) data = DistanceData(names, ddata.result) names = tuple(sorted(names)) if return_names and return_data: return distance, names, data elif return_names: return distance, names elif return_data: return distance, data else: return distance
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 _distance_check(tube_manager, environment): """Checks distance between given collision manager and object list. Parameters ---------- tube_manager : DynamicAABBTreeCollisionManager environment : list of CollisionObjects Returns ------- float minimum distance between the two collections of collision objects """ env_manager = fcl.DynamicAABBTreeCollisionManager() env_manager.registerObjects(environment) env_manager.setup() data = fcl.DistanceData() tube_manager.distance(env_manager, data, fcl.defaultDistanceCallback) return data.result.min_distance
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 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 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 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
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()) == 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
def test_managed_distances(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.DistanceData(self.drequest, fcl.DistanceResult()) manager1.distance(o1, cdata, fcl.defaultDistanceCallback) self.assertTrue(cdata.result.min_distance < 0) cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager1.distance(o2, cdata, fcl.defaultDistanceCallback) assert abs(cdata.result.min_distance - 3.6) < 1e-4 cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager2.distance(o1, cdata, fcl.defaultDistanceCallback) self.assertAlmostEqual(cdata.result.min_distance, 4.0) cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager2.distance(o2, cdata, fcl.defaultDistanceCallback) self.assertTrue(cdata.result.min_distance < 0) # Many-to-many, internal cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager1.distance(cdata, fcl.defaultDistanceCallback) self.assertTrue(cdata.result.min_distance < 0) cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager2.distance(cdata, fcl.defaultDistanceCallback) self.assertAlmostEqual(cdata.result.min_distance, 9.0) # Many-to-many, grouped cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager1.distance(manager2, cdata, fcl.defaultDistanceCallback) self.assertAlmostEqual(cdata.result.min_distance, 4.0) cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager2.distance(manager3, cdata, fcl.defaultDistanceCallback) self.assertTrue(cdata.result.min_distance < 0) cdata = fcl.DistanceData(self.drequest, fcl.DistanceResult()) manager1.distance(manager3, cdata, fcl.defaultDistanceCallback) self.assertTrue(cdata.result.min_distance < 0)