class Collision_Object_Container: def __init__(self, yaml_path): self.collision_objects = [] f = open(yaml_path) y = yaml.load(f) keys = y.keys() for k in keys: if not y[k] == None: if k == 'robot_link_radius' or k == 'sample_states' or k == 'training_states' or k == 'problem_states': continue for i in range(len(y[k])): if k == 'boxes': self.collision_objects.append(Collision_Box(y[k][i])) elif k == 'spheres': self.collision_objects.append(Collision_Sphere( y[k][i])) elif k == 'ellipsoids': self.collision_objects.append( Collision_Ellipsoid(y[k][i])) elif k == 'capsules': self.collision_objects.append( Collision_Capsule(y[k][i])) elif k == 'cones': self.collision_objects.append(Collision_Cone(y[k][i])) elif k == 'cylinders': self.collision_objects.append( Collision_Cylinder(y[k][i])) elif k == 'meshes': self.collision_objects.append(Collision_Mesh(y[k][i])) self.collision_objects[-1].type = 'environment_object' self.robot_link_radius = 0.05 if 'robot_link_radius' in keys: self.robot_link_radius = float(y['robot_link_radius']) if 'sample_states' in keys: self.sample_states = y['sample_states'] else: raise Exception( 'Must specify at least one sample state in collision yaml file!' ) self.set_rviz_ids() def set_rviz_ids(self): for i, c in enumerate(self.collision_objects): c.marker.id = i def get_min_distance(self, (a, b)): obja = self.collision_objects[a].obj objb = self.collision_objects[b].obj self.request = fcl.DistanceRequest() self.result = fcl.DistanceResult() ret = fcl.distance(obja, objb, self.request, self.result) return self.result.min_distance
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_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 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 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 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 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
print_collision_result('Box', 'Cylinder', res) n_contacts = fcl.collide( fcl.CollisionObject(mesh, fcl.Transform(np.array([0.0, 0.0, -1.0]))), fcl.CollisionObject(cyl, fcl.Transform()), req, res) print_collision_result('Box', 'Mesh', res) #===================================================================== # Pairwise distance checking #===================================================================== print('=' * 60) print('Testing Pairwise Distance Checking') print('=' * 60) print() req = fcl.DistanceRequest(enable_nearest_points=True) res = fcl.DistanceResult() dist = fcl.distance(fcl.CollisionObject(box, fcl.Transform()), fcl.CollisionObject(cone, fcl.Transform()), req, res) print_distance_result('Box', 'Cone', res) dist = fcl.distance( fcl.CollisionObject(box, fcl.Transform()), fcl.CollisionObject(cyl, fcl.Transform(np.array([6.0, 0.0, 0.0]))), req, res) print_distance_result('Box', 'Cylinder', res) dist = fcl.distance( fcl.CollisionObject(box, fcl.Transform()), fcl.CollisionObject(box, fcl.Transform(np.array([1.01, 0.0, 0.0]))), req,