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 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')