def __init__(self, obstacles) -> None: self.robot = URDF.load('data/xarm6/xarm6.urdf') self.spaces = RealVectorSpace(6) self.robot_cm = CollisionManager() self.env_cm = CollisionManager() self.start_config = [0, 0, 0, 0, 0, 0] self.traj = [] self.count_i = 0 self.curr_q = [0, 0, 0, 0, 0, 0] cfg = self.get_config([0, 0, 0, 0, 0, 0]) self.init_poses = [0 for i in range(6)] #print(cfg) fk = self.robot.collision_trimesh_fk(cfg=cfg) #fk = self.robot.visual_trimesh_fk(cfg=cfg) # adding robot to the scene for i, tm in enumerate(fk): pose = fk[tm] name = "link_" + str(i + 1) self.init_poses.append(pose) self.robot_cm.add_object(name, tm, pose) for i, ob in enumerate(obstacles): self.env_cm.add_object("obstacle_" + str(i), ob) for i, ob in enumerate(obstacles[:-1]): self.env_cm.add_object("prediction_" + str(i), ob)
def __init__(self, obstacles) -> None: self.robot = URDF.load('data/ur5/ur5.urdf') self.spaces = RealVectorSpace(6) self.robot_cm = CollisionManager() self.env_cm = CollisionManager() cfg = self.get_config([0, 0, 0, 0, 0, 0]) self.init_poses = [0 for i in range(6)] #print(cfg) fk = self.robot.collision_trimesh_fk(cfg=cfg) #fk = self.robot.visual_trimesh_fk(cfg=cfg) # adding robot to the scene for i, tm in enumerate(fk): pose = fk[tm] name = "link_" + str(i + 1) self.init_poses.append(pose) self.robot_cm.add_object(name, tm, pose) table = cylinder(radius=0.7, height=0.02) table.apply_translation([0, 0, -0.015]) obstacles.append(table) for i, ob in enumerate(obstacles): self.env_cm.add_object("obstacle_" + str(i), ob)
def __init__(self, obstacles) -> None: self.robot = URDF.load('data/two_planar/2dof_planar.urdf') self.spaces = RealVectorSpace(2) self.robot_cm = CollisionManager() self.env_cm = CollisionManager() self.start_config = [0, 0] self.traj = [] self.count_i = 0 self.curr_q = [0, 0] cfg = self.get_config([0, 0]) fk = self.robot.link_fk(cfg=cfg) self.init_poses = [] for i, tm in enumerate(fk): pose = fk[tm] init_pose, link_mesh = self.get_link_mesh(tm) self.init_poses.append(init_pose) self.robot_cm.add_object( tm.name, link_mesh, np.matmul(pose, init_pose)) for i, ob in enumerate(obstacles): self.env_cm.add_object("obstacle_" + str(i), ob) self.env_cm.add_object("prediction_" + str(i), ob)
def check_rooted(verts, faces): # Load up the mesh mesh = tm.Trimesh(vertices=verts, faces=faces) # Switch from y-up to z-up mesh.vertices = mesh.vertices[:, [0, 2, 1]] mesh.fix_normals() # Find the height of the ground plane z_ground = mesh.bounds[0][2] # Extract the individual cuboid parts comps = mesh.split().tolist() # Also create a thin box for the ground plane ground = box( extents = [10, 10, 0.01], transform = [ [1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, z_ground - 0.01/2], [0, 0, 0, 1] ] ) comps.insert(0, ground) # Detect (approximate) intersections between parts # collision_dist = 0.005 * mesh.scale # collision_dist = 0.01 * mesh.scale collision_dist = 0.02 * mesh.scale adjacencies = {comp_index : [] for comp_index in range(len(comps))} manager = CollisionManager() for i in range(len(comps)-1): manager.add_object(str(i), comps[i]) for j in range(i+1, len(comps)): dist = manager.min_distance_single(comps[j]) if (dist < collision_dist): adjacencies[i].append(j) adjacencies[j].append(i) manager.remove_object(str(i)) # Run a DFS starting from the ground, check if everything is reachable visited = [False for comp in comps] stack = [0] # Index of 'ground' while len(stack) > 0: nindex = stack.pop() visited[nindex] = True for cindex in adjacencies[nindex]: if not visited[cindex]: stack.append(cindex) return all(visited)
def determine_grasps(obj_grasp_configs, kit_t_objects: Dict[str, Transform], min_clearance, debug=False, debug_view_res=(800, 500)): if debug: scene = trimesh.Scene() for name, mesh, *_ in obj_grasp_configs: scene.add_geometry(mesh, transform=kit_t_objects[name].matrix, geom_name=name) scene.show(resolution=debug_view_res) lense = trimesh.load_mesh( Path(__file__).parent.parent / 'stl' / 'lense.stl') lense.apply_scale(1e-3) col_tool = CollisionManager() col_tool.add_object('lense', lense) col_objects = CollisionManager() for name, mesh, *_ in obj_grasp_configs: col_objects.add_object(name, mesh, transform=kit_t_objects[name].matrix) worst_possible_clearance = np.inf grasps = [] candidates = [*obj_grasp_configs] clearances = [] i = 0 while candidates: candidate = candidates[i] name, mesh, tcp_t_obj, grasp_start_width, grasp_end_width, sym_deg, sym_offset = candidate kit_t_obj = kit_t_objects[name] # move away the grasp obj to ignore obj-tcp collision col_objects.set_transform(name, Transform(p=(1000, 0, 0)).matrix) finger_grasp_volume = build_finger_grasp_volume(grasp_start_width) col_tool.add_object('finger_grasp_volume', finger_grasp_volume) kit_t_tcp = kit_t_obj @ tcp_t_obj.inv ts = rotational_grasp_symmetry(kit_t_tcp, sym_deg, sym_offset) dists, dists_name = [], [] for t in ts: col_tool.set_transform('lense', t.matrix) col_tool.set_transform('finger_grasp_volume', t.matrix) dist, dist_names = col_objects.min_distance_other( col_tool, return_names=True) if dist < worst_possible_clearance: worst_possible_clearance = dist dists.append(dist) dists_name.append(dist_names[0]) di = np.argmax(dists).item() kit_t_tcp, dist, dist_name = ts[di], dists[di], dists_name[di] tcp_t_obj = kit_t_tcp.inv @ kit_t_obj col_tool.remove_object('finger_grasp_volume') if dist > min_clearance: grasps.append( (name, tcp_t_obj, grasp_start_width, grasp_end_width)) candidates.remove(candidate) clearances.append(dist) i = 0 if debug: print(f'{name}: {dist:.3f}') scene.add_geometry(lense, transform=kit_t_tcp.matrix, geom_name='lense') scene.add_geometry(finger_grasp_volume, transform=kit_t_tcp.matrix, geom_name='finger_grasp_volume') scene.show(resolution=debug_view_res) scene.delete_geometry([name, 'lense', 'finger_grasp_volume']) else: if debug: print(f'!! {name}: {dist_name} {dist:.3f} - changing order') # move the grasp object back in place col_objects.set_transform(name, kit_t_obj.matrix) i += 1 if i == len(candidates): raise RuntimeError('cant find solution with this clearance') return grasps
def obj2urdf(input_file, output_dir, density=1): if not os.path.exists(output_dir): os.makedirs(output_dir) # Load up the mesh mesh = tm.load(input_file) # Switch from y-up to z-up mesh.vertices = mesh.vertices[:, [0, 2, 1]] mesh.fix_normals() # Extract the individual cuboid parts comps = mesh.split().tolist() # Detect (approximate) intersections between parts, to use for building joints # collision_dist = 0.005 * mesh.scale # collision_dist = 0.01 * mesh.scale collision_dist = 0.02 * mesh.scale adjacencies = {comp_index : [] for comp_index in range(len(comps))} manager = CollisionManager() for i in range(len(comps)-1): manager.add_object(str(i), comps[i]) for j in range(i+1, len(comps)): dist = manager.min_distance_single(comps[j]) if (dist < collision_dist): adjacencies[i].append(j) adjacencies[j].append(i) manager.remove_object(str(i)) # Compute connected components conn_comps = [] visited = [False for _ in comps] while not all(visited): conn_comp = set([]) start_idx = visited.index(False) stack = [start_idx] while len(stack) > 0: idx = stack.pop() visited[idx] = True conn_comp.add(idx) for nidx in adjacencies[idx]: if not visited[nidx]: stack.append(nidx) conn_comps.append(list(conn_comp)) # We export one URDF object file per connected component for i,conn_comp in enumerate(conn_comps): # Re-center this connected component ccmesh = meshconcat([comps[j] for j in conn_comp]) c = ccmesh.centroid transmat = [ [1, 0, 0, -c[0]], [0, 1, 0, -c[1]], [0, 0, 1, -c[2]], [0, 0, 0, 1] ] for j in conn_comp: comps[j].apply_transform(transmat) ccmesh.apply_transform(transmat) # Also, record where to start this mesh in the simulation # That's the x,y coords of the centroid, and -bbox bottom for the z (so it sits on the ground) # And the bbox diagonal (we use this for error thresholding) metadata = { 'start_pos': [c[0], c[1], -ccmesh.bounds[0][2]], 'scale': ccmesh.volume } with open(f'{output_dir}/assembly_{i}.json', 'w') as f: f.write(json.dumps(metadata)) # Build a directed tree by DFS root_idx = conn_comp[0] root = {'id': root_idx, 'children': []} fringe = [root] visited = set([root['id']]) while len(fringe) > 0: node = fringe.pop() for neighbor in adjacencies[node['id']]: if not (neighbor in visited): child_node = {'id': neighbor, 'children': []} node['children'].append(child_node) visited.add(child_node['id']) fringe.append(child_node) # Build up the URDF data structure urdf_root = xml.Element('robot') urdf_root.set('name', 'part_graph_shape') # Links for j in conn_comp: comp = comps[j] link = xml.SubElement(urdf_root, 'link') link.set('name', f'part_{j}') visual = xml.SubElement(link, 'visual') geometry = xml.SubElement(visual, 'geometry') mesh = xml.SubElement(geometry, 'mesh') mesh.set('filename', f'{output_dir}/part_{j}.stl') material = xml.SubElement(visual, 'material') material.set('name', 'gray') color = xml.SubElement(material, 'color') color.set('rgba', '0.5 0.5 0.5 1') collision = xml.SubElement(link, 'collision') geometry = xml.SubElement(collision, 'geometry') mesh = xml.SubElement(geometry, 'mesh') mesh.set('filename', f'{output_dir}/part_{j}.stl') inertial = xml.SubElement(link, 'inertial') mass = xml.SubElement(inertial, 'mass') mass.set('value', str(comp.volume * density)) inertia = xml.SubElement(inertial, 'inertia') inertia.set('ixx', '1.0') inertia.set('ixy', '0.0') inertia.set('ixz', '0.0') inertia.set('iyy', '1.0') inertia.set('iyz', '0.0') inertia.set('izz', '1.0') # Joints fringe = [root] while len(fringe) > 0: node = fringe.pop() for child_node in node['children']: joint = xml.SubElement(urdf_root, 'joint') joint.set('name', f'{node["id"]}_to_{child_node["id"]}') joint.set('type', 'fixed') parent = xml.SubElement(joint, 'parent') parent.set('link', f'part_{node["id"]}') child = xml.SubElement(joint, 'child') child.set('link', f'part_{child_node["id"]}') origin = xml.SubElement(joint, 'origin') origin.set('xyz', '0 0 0') fringe.append(child_node) # Save URDF file to disk # Have to make sure to split it into multiple lines, otherwise Bullet's URDF parser will # throw an error trying to load really large files as a single line... xmlstring = xml.tostring(urdf_root, encoding='unicode') xmlstring = '>\n'.join(xmlstring.split('>')) with open(f'{output_dir}/assembly_{i}.urdf', 'w') as f: f.write(xmlstring) # Write the parts to disk as STL files for the URDF to refer to for i,comp in enumerate(comps): comp.export(f'{output_dir}/part_{i}.stl')
class UR5(RealVectorSpace): def __init__(self, obstacles) -> None: self.robot = URDF.load('data/ur5/ur5.urdf') self.spaces = RealVectorSpace(6) self.robot_cm = CollisionManager() self.env_cm = CollisionManager() cfg = self.get_config([0, 0, 0, 0, 0, 0]) self.init_poses = [0 for i in range(6)] #print(cfg) fk = self.robot.collision_trimesh_fk(cfg=cfg) #fk = self.robot.visual_trimesh_fk(cfg=cfg) # adding robot to the scene for i, tm in enumerate(fk): pose = fk[tm] name = "link_" + str(i + 1) self.init_poses.append(pose) self.robot_cm.add_object(name, tm, pose) table = cylinder(radius=0.7, height=0.02) table.apply_translation([0, 0, -0.015]) obstacles.append(table) for i, ob in enumerate(obstacles): self.env_cm.add_object("obstacle_" + str(i), ob) def spaces(self): return self.spaces def robot(self): return self.robot def is_in_collision(self, q): cfg = self.get_config(q) fk = self.robot.collision_trimesh_fk(cfg=cfg) # adding robot to the scene for i, tm in enumerate(fk): pose = fk[tm] self.robot_cm.set_transform("link_" + str(i + 1), pose) # print("obs:", self.env_cm._objs["obstacle_0"]["obj"].getTransform()) return self.robot_cm.in_collision_other(self.env_cm) def is_valid(self, q, qe=None, num_checks=None): if qe is None or num_checks is None: res = self.is_in_collision(q) return not (res) else: for i in range(num_checks): q_i = self.get_qnew(q, qe, i / num_checks) res = self.is_in_collision(q_i) if res: return False return True def get_config(self, q): if len(self.robot.actuated_joints) != len(q): raise Exception("Wrong dimensions of q") cfg = {} for i in range(len(q)): cfg[self.robot.actuated_joints[i].name] = q[i] return cfg def get_link_mesh(self, tm): print(tm.visuals) init_pose = tm.visuals[0].origin if tm.visuals[0].geometry.cylinder is not None: length = tm.visuals[0].geometry.cylinder.length radius = tm.visuals[0].geometry.cylinder.radius mesh = cylinder(radius, length) mesh.visual.face_color = tm.visuals[0].material.color return init_pose, mesh else: ext = tm.visuals[0].geometry.box.size mesh = box(ext) mesh.visual.face_colors = tm.visuals[0].material.color return init_pose, mesh def show(self, q=None, obstacles=None, use_collision=False): cfg = self.get_config(q) #print(cfg) if use_collision: fk = self.robot.collision_trimesh_fk(cfg=cfg) else: fk = self.robot.visual_trimesh_fk(cfg=cfg) scene = pyrender.Scene() # adding robot to the scene for tm in fk: pose = fk[tm] mesh = pyrender.Mesh.from_trimesh(tm, smooth=False) scene.add(mesh, pose=pose) # adding base box to the scene table = cylinder(radius=0.7, height=0.02) #([0.5, 0.5, 0.02]) table.apply_translation([0, 0, -0.015]) table.visual.vertex_colors = [205, 243, 8, 255] scene.add(pyrender.Mesh.from_trimesh(table)) # adding obstacles to the scene for i, ob in enumerate(obstacles): if i < len(obstacles) - 1: ob.visual.vertex_colors = [255, 0, 0, 255] else: ob.visual.vertex_colors = [205, 243, 8, 255] scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False)) pyrender.Viewer(scene, use_raymond_lighting=True) def animate(self, q_traj=None, obstacles=None): import time fps = 10.0 cfgs = [self.get_config(q) for q in q_traj] # Create the scene fk = self.robot.visual_trimesh_fk(cfg=cfgs[0]) node_map = {} init_pose_map = {} scene = pyrender.Scene() for tm in fk: pose = fk[tm] mesh = pyrender.Mesh.from_trimesh(tm, smooth=False) node = scene.add(mesh, pose=pose) node_map[tm] = node # adding base box to the scene #table = cylinder(radius=0.7, height=0.02) #([0.5, 0.5, 0.02]) #table.apply_translation([0, 0, -0.015]) #table.visual.vertex_colors = [205, 243, 8, 255] #scene.add(pyrender.Mesh.from_trimesh(table)) cam = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.414) init_cam_pose = np.eye(4) init_cam_pose[2, 3] = 2.5 scene.add(cam, pose=init_cam_pose) # adding obstacles to the scene for i, ob in enumerate(obstacles): if i < len(obstacles) - 1: ob.visual.vertex_colors = [255, 0, 0, 255] else: ob.visual.vertex_colors = [205, 243, 8, 255] scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False)) # Pop the visualizer asynchronously v = pyrender.Viewer(scene, run_in_thread=True, use_raymond_lighting=True) time.sleep(1.0) # Now, run our loop i = 0 while v.is_active: cfg = cfgs[i] fk = self.robot.visual_trimesh_fk(cfg=cfg) if i < len(cfgs) - 1: i += 1 else: i = 0 time.sleep(1.0) v.render_lock.acquire() for mesh in fk: pose = fk[mesh] node_map[mesh].matrix = pose v.render_lock.release() time.sleep(1.0 / fps)
l_selection_cells = ['all'] #'all' will process all cells input_folder, output_folder = read_parms() (output_folder / "STL_converted").mkdir(parents=True, exist_ok=True) #Part1: find out contact pairs via Trimesh collision manager/ #trimesh cannot work with vtp directly, so convert to stl first # print(trimesh.exchange.load.available_formats()) d_cellname_poly = {} for ix, vtp_path_i in enumerate(sorted(input_folder.glob('*.vtp'))): poly = read_vtp_file(vtp_path_i) d_cellname_poly[vtp_path_i.stem] = poly write_stl_file(poly, (output_folder / "STL_converted" / (vtp_path_i.stem + ".stl"))) cm = CollisionManager() d_cellname_trimeshStl = {} for ix, stl_i in enumerate( sorted((output_folder / "STL_converted").glob('cell*.stl'))): print('Adding stl -> {0}'.format(stl_i.name)) f = open(str(stl_i), 'r') trimesh_stl = trimesh.load(f, file_type="stl") d_cellname_trimeshStl[stl_i.stem] = trimesh_stl cm.add_object(stl_i.stem, trimesh_stl) is_collision, s_t_colliding_pairs = cm.in_collision_internal(return_names=True, return_data=False) print("cm.in_collision_internal = {0} \nwith colliding pairs {1} ".format( is_collision, s_t_colliding_pairs))
class TwoDOF(RealVectorSpace): def __init__(self, obstacles) -> None: self.robot = URDF.load('data/two_planar/2dof_planar.urdf') self.spaces = RealVectorSpace(2) self.robot_cm = CollisionManager() self.env_cm = CollisionManager() self.start_config = [0, 0] self.traj = [] self.count_i = 0 self.curr_q = [0, 0] cfg = self.get_config([0, 0]) fk = self.robot.link_fk(cfg=cfg) self.init_poses = [] for i, tm in enumerate(fk): pose = fk[tm] init_pose, link_mesh = self.get_link_mesh(tm) self.init_poses.append(init_pose) self.robot_cm.add_object( tm.name, link_mesh, np.matmul(pose, init_pose)) for i, ob in enumerate(obstacles): self.env_cm.add_object("obstacle_" + str(i), ob) self.env_cm.add_object("prediction_" + str(i), ob) def set_trajectory(self, path): self.traj = path self.curr_q = path[0] self.count_i = 0 # def get_next_q(self): # if len(self.traj) == 0: # return None # self.traj.pop() # self.curr_q = self.traj[-1] # # print(self.traj) # return self.curr_q def get_next_q(self): self.count_i += 1 if self.count_i >= len(self.traj): return None self.curr_q = self.traj[self.count_i] return self.curr_q def update_env(self, obstacles): with threading.Lock(): for i, ob in enumerate(obstacles): self.env_cm.remove_object("obstacle_" + str(i)) self.env_cm.add_object("obstacle_" + str(i), ob) def update_predictions(self, predictions): with threading.Lock(): for i, ob in enumerate(predictions): self.env_cm.remove_object("prediction_" + str(i)) self.env_cm.add_object("prediction_" + str(i), ob) def spaces(self): return self.spaces def robot(self): return self.robot def is_in_collision(self, q): cfg = self.get_config(q) fk = self.robot.link_fk(cfg=cfg) # adding robot to the scene for i, tm in enumerate(fk): pose = fk[tm] init_pose = self.init_poses[i] self.robot_cm.set_transform(tm.name, np.matmul(pose, init_pose)) # print("obs:", self.env_cm._objs["obstacle_0"]["obj"].getTransform()) return self.robot_cm.in_collision_other(self.env_cm) def distance(self, q): cfg = self.get_config(q) fk = self.robot.link_fk(cfg=cfg) # adding robot to the scene for i, tm in enumerate(fk): pose = fk[tm] init_pose = self.init_poses[i] self.robot_cm.set_transform(tm.name, np.matmul(pose, init_pose)) # print("obs:", self.env_cm._objs["obstacle_0"]["obj"].getTransform()) return self.robot_cm.min_distance_other(self.env_cm) def is_valid(self, q, qe=None, num_checks=None): if qe is None or num_checks is None: res = self.is_in_collision(q) return not(res) else: for i in range(num_checks): q_i = self.get_qnew(q, qe, i/num_checks) res = self.is_in_collision(q_i) if res: return False return True def get_config(self, q): if len(self.robot.actuated_joints) != len(q): raise Exception("Wrong dimensions of q") cfg = {} for i in range(len(q)): cfg[self.robot.actuated_joints[i].name] = q[i] return cfg def get_link_mesh(self, tm): init_pose = tm.visuals[0].origin if tm.visuals[0].geometry.cylinder is not None: length = tm.visuals[0].geometry.cylinder.length radius = tm.visuals[0].geometry.cylinder.radius mesh = cylinder(radius, length) mesh.visual.face_color = tm.visuals[0].material.color return init_pose, mesh else: ext = tm.visuals[0].geometry.box.size mesh = box(ext) mesh.visual.face_colors = tm.visuals[0].material.color return init_pose, mesh def show(self, q=None, obstacles=None): cfg = self.get_config(q) # print(cfg) fk = self.robot.link_fk(cfg=cfg) scene = pyrender.Scene() # adding robot to the scene for tm in fk: pose = fk[tm] init_pose, link_mesh = self.get_link_mesh(tm) mesh = pyrender.Mesh.from_trimesh(link_mesh, smooth=False) scene.add(mesh, pose=np.matmul(pose, init_pose)) # adding base box to the scene # table = box([0.5, 0.5, 0.01]) # table.apply_translation([0, 0, -0.1]) cam = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.414) # nc = pyrender.Node(camera=cam, matrix=np.eye(4)) # scene.add_node(nc) init_cam_pose = np.eye(4) init_cam_pose[2, 3] = 2.5 scene.add(cam, pose=init_cam_pose) # scene.add(pyrender.Mesh.from_trimesh(table)) # adding obstacles to the scene for ob in obstacles: scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False)) pyrender.Viewer(scene, use_raymond_lighting=True) def animate(self, q_traj=None, obstacles=None): import time fps = 10.0 cfgs = [self.get_config(q) for q in q_traj] # Create the scene fk = self.robot.link_fk(cfg=cfgs[0]) node_map = {} init_pose_map = {} scene = pyrender.Scene() for tm in fk: pose = fk[tm] init_pose, link_mesh = self.get_link_mesh(tm) mesh = pyrender.Mesh.from_trimesh(link_mesh, smooth=False) node = scene.add(mesh, pose=np.matmul(pose, init_pose)) node_map[tm] = node init_pose_map[tm] = init_pose cam = pyrender.PerspectiveCamera(yfov=np.pi / 3.0, aspectRatio=1.414) init_cam_pose = np.eye(4) init_cam_pose[2, 3] = 2.5 scene.add(cam, pose=init_cam_pose) for ob in obstacles: scene.add(pyrender.Mesh.from_trimesh(ob, smooth=False)) # Pop the visualizer asynchronously v = pyrender.Viewer(scene, run_in_thread=True, use_raymond_lighting=True) time.sleep(1.0) # Now, run our loop i = 0 while v.is_active: cfg = cfgs[i] fk = self.robot.link_fk(cfg=cfg) if i < len(cfgs) - 1: i += 1 else: i = 0 time.sleep(1.0) v.render_lock.acquire() for mesh in fk: pose = fk[mesh] node_map[mesh].matrix = np.matmul(pose, init_pose_map[mesh]) v.render_lock.release() time.sleep(1.0 / fps)
tmh.apply_scale(scaling) stl_ascii = trimesh.exchange.stl.export_stl_ascii(tmh) # tmh.export writes a binary stl by default , problems with reloading this (so pass via file object (str)) f = open(str(p_stl_i), 'w') f.write(stl_ascii) elif input_file_type=='ply': #convert to stl and scale for ix, ply_path_i in enumerate(sorted(input_folder_RES.glob('*.ply'))): tmh = trimesh.load(ply_path_i, file_type="ply") tmh.apply_scale(scaling) stl_ascii = trimesh.exchange.stl.export_stl_ascii(tmh) # tmh.export writes a binary stl by default , problems with reloading this (so pass via file object (str)) p_stl = folder_RES_STL / (ply_path_i.stem + ".stl") f = open(str(p_stl), 'w') f.write(stl_ascii) print("Part2a: make 2 collision managers, one for GT , one for RES. and build up datastructures of cells") cm_GT = CollisionManager() cm_RES = CollisionManager() init_ds_cells(folder_RES_STL, ic_GT=False) init_ds_cells(input_folder_GT, ic_GT=True) print("# Part2b: already compose excel with the RES geometric data") write_excel_RES_data() # Part3 : find max contacting volume for each GT cell (always start from GT cells = similar to SEG scoring ). # 1 : Use the collision detection to detect which cells overlap. # 2 : check the boolean difference volume. # 3 : pick the maximum for scoring