<filename>models/do_model_updates.py
import numpy as np
import trimesh
from trimesh.constants import log
from copy import deepcopy
import glob

import os

import open3d
import numpy as np
import cv2
import imutils

from lxml import etree as ET

from pydrake.all import (
    SpatialInertia,
    RotationalInertia
)

def do_visual_mesh_simplification(input_obj_path, target_tris=1000):
    '''
    Given an obj path, simplifies the geometry and texture so
    it renders more easily in meshcat / drake vis by:
    - Creates a (possibly simplified) mesh alongside it, with a 
    "_simple_vis.obj" post-fox.
    - Looks for a texture file at a fixed relative path
    (../material/textures/texture.png), resizes it to max edge length
    1024 to decrease size, and saves it alongside the new saved visual
    mesh with a .png extension.

    No existing files changed -- just new ones added.

    Args:
    - input_obj_path: String path to obj file
    - target_tris: Currently unusued, but would be target for mesh decimation.

    Returns:
    - output obj file path
    '''
    output_obj_path = input_obj_path[:-4] + "_simple_vis.obj"
    output_texture_path = output_obj_path[:-4] + ".png"
    print("Output path: ", output_texture_path)

    mesh = open3d.io.read_triangle_mesh(input_obj_path)
    simplified_mesh = mesh # mesh.simplify_quadric_decimation(target_tris)
    # Can't quite get this to work as Open3D stores UVs per
    # triangle rather than per vertex.
    # I should get the triangle centers and pull the UV from
    # the corresponding triangle rather than doing it by
    # raw vertex.
    # But this whole pipeline is moot if we have a fast enough
    # renderer, so whatever.
    ## Update UVs on simplified mesh by taking UV from
    ## nearest neighbor on original mesh
    #simplified_verts = np.asarray(simplified_mesh.vertices)
    #corresps = []
    #orig_verts_kdtree = open3d.geometry.KDTreeFlann(np.asarray(mesh.vertices).T)
    #for k in range(simplified_verts.shape[0]):
    #    corresps.append(orig_verts_kdtree.search_knn_vector_3d(simplified_verts[k, :], 1)[2])
    ## Now go through and update triangle uvs
    #n_tris = simplified_verts.num_triangles()
    #tri_inds = np.asarray(simplified_verts.triangles)
    #triangle_uvs = np.zeros(3*n_tris, 2)
    #for k in range(n_tris):
    #    for l in range(3):
    #        corresps[tri_inds[k, l]]
    open3d.io.write_triangle_mesh(output_obj_path, simplified_mesh)

    # Hardcode creation of a texture next to it,
    # looking for the texture in ../materials/textures/texture.png
    # (Downsize the texture to a reasonable size, too)
    dirname, _ = os.path.split(input_obj_path)
    texture_path = os.path.join(dirname, "../materials/textures/texture.png")
    if os.path.exists(texture_path):
        texture_image = open3d.io.read_image(texture_path)
        texture_image = cv2.cvtColor(np.asarray(texture_image), cv2.COLOR_BGR2RGB)
        # Resize to height 1024 (which hopefully results in
        # a reasonably-sized image. 
        height, width = texture_image.shape[:2]
        if height > width:
            texture_image = imutils.resize(texture_image, height=1024)
        else:
            texture_image = imutils.resize(texture_image, width=1024)
        cv2.imwrite(output_texture_path, texture_image)


    else:
        print("Found no texture at ", texture_path)

    return output_obj_path

def do_collision_mesh_simplification(input_obj_path, show=False):
    '''
    Given an obj, performs a convex decomposition of it with
    trimesh _ vhacd, saving all the parts in a subfolder.
    
    Args:
    - input_obj_path: String path to obj file to decompose
    - show: Whether to open (and block on) a window to preview
    the decomposition.

    Returns: (out_paths, inertia)
    - out_paths: List of generated obj files
    - inertia: total inertia of the obj, assuming density of 2000 kg/m^3
    '''

    # Create a subdir for the convex decomp parts, as
    # there might be many.
    dirname, objname = os.path.split(input_obj_path)
    piece_dirname = objname[:-4] + "_parts"
    out_dir = os.path.join(dirname, piece_dirname)
    os.makedirs(out_dir, exist_ok=True)

    mesh = trimesh.load(input_obj_path, skip_materials=True)
    if show:
        mesh.show()
    try:
        convex_pieces = []
        # Do convex decomp multiple times, hoping for overlap.
        args_per_iter = [
            {"maxhulls": 12,
             "maxNumVerticesPerCH": 12,
             "minVolumePerCH": 0.001,
             "resolution": 100000,
             "pca": 1},
            {"maxhulls": 12,
             "maxNumVerticesPerCH": 12,
             "minVolumePerCH": 0.001,
             "resolution": 100000,
             "pca": 1},
        ]
        for k, args in enumerate(args_per_iter):
            # Apply random rotation if not the first iteration
            # (many meshes decompose better upright in axis-aligned arrangement)
            tf = trimesh.transformations.random_rotation_matrix(np.random.rand(3))
            mesh.apply_transform(tf)
            convex_pieces_new = trimesh.decomposition.convex_decomposition(
                mesh, **args) # TODO: args
            # Unrotate and add pieces
            inv_tf = tf.T # No translation so this is OK
            mesh.apply_transform(inv_tf)
            if not isinstance(convex_pieces_new, list):
                convex_pieces_new = [convex_pieces_new]
            for convex_piece in convex_pieces_new:
                convex_piece.apply_transform(inv_tf)
            convex_pieces += convex_pieces_new
    except Exception as e:
        print("Problem in decomp: ", e)
    
    # Give them random colors for display
    for part in convex_pieces:
        this_color = trimesh.visual.random_color()
        part.visual.face_colors[:] = this_color
    scene = trimesh.scene.scene.Scene()
    for part in convex_pieces:
        scene.add_geometry(part)
    if show:
        scene.show()

    mesh.density = 2000
    I = mesh.moment_inertia
    inertia = SpatialInertia.MakeFromCentralInertia(
        mass=mesh.mass,
        p_PScm_E=mesh.center_mass,
        I_SScm_E=RotationalInertia(
            Ixx=I[0, 0],
            Ixy=I[0, 1],
            Ixz=I[0, 2],
            Iyy=I[1, 1],
            Iyz=I[1, 2],
            Izz=I[2, 2]).ShiftFromCenterOfMass(mass=mesh.mass,
                                               p_BcmQ_E=-mesh.center_mass)
    )

    # Save them each out
    out_paths = []
    for k, part in enumerate(convex_pieces):
        piece_name = '%s_convex_piece_%03d.obj' % (objname[:-4], k)
        full_path = os.path.join(out_dir, piece_name)
        trimesh.exchange.export.export_mesh(part, full_path)
        out_paths.append(full_path)

    return out_paths, inertia


def update_sdf_with_convex_decomp(input_file):
    '''
    Given an SDF from IgnitionRobotics, produces a new SDF that:
    - For each visual item, replaces it with a simplified
    visual item with a simplified texture saved alongside,
    which are auto-generated next to the original
    model.
    - For each collision item, replaces it with a convex
    decomp of the collision geometry, which is auto-generated
    next to the original model.
    - Inserts inertia for each link, calculated from the
    collision geometry with an assumed constant density.
    and saves this SDF alongside the original with a
    "_simplified.sdf" suffix.
    '''

    this_file_path, _ = os.path.split(input_file)

    # Open up the SDF as an XML
    parser = ET.XMLParser(remove_blank_text=True)
    data = ET.parse(input_file, parser=parser)
    # Add drake namespace
    ET.register_namespace('drake', 'drake.mit.edu')
    sdf_root = data.getroot()

    link_items = sdf_root.findall('.//link')

    for link_item in link_items:
        visual_items = link_item.findall('visual')
        for visual_item in visual_items:
            # Simplify each mesh
            for visual_mesh in visual_item.findall('.//mesh'):
                for uri in visual_mesh.findall('.//uri'):
                    model_path = os.path.join(this_file_path, uri.text)
                    new_model_path = do_visual_mesh_simplification(model_path)
                    new_model_path = os.path.relpath(new_model_path, this_file_path)
                    # Replace mesh reference with our simplified version
                    uri.text = new_model_path

        collision_items = link_item.findall('collision')
        component_inertias = []
        total_collision_k = 0
        for collision_item in collision_items:
            new_model_paths = []
            for geometry in collision_item.findall('.//geometry'):
                for uri in geometry.findall('.//uri'):
                    model_path = os.path.join(this_file_path, uri.text)
                    new_paths, new_inertia = do_collision_mesh_simplification(model_path)
                    new_model_paths += new_paths
                    component_inertias.append(new_inertia)
            new_model_paths = [
                os.path.relpath(new_model_path, this_file_path)
                for new_model_path in new_model_paths]
            # Create a new XML subtree for the collection of meshes
            # we just created. I *think* each convex piece needs
            # to be in its own collision tag, otherwise Drake
            # seems to be ignoring them...
            link_item.remove(collision_item)

            for path in new_model_paths:
                collision_item = ET.SubElement(link_item, 'collision', name="collision_%04d" % total_collision_k)
                total_collision_k += 1
                geometry_item = ET.SubElement(collision_item, 'geometry')
                mesh_item = ET.SubElement(geometry_item, "mesh")
                uri_item = ET.SubElement(mesh_item, "uri")
                uri_item.text = path
                ET.SubElement(mesh_item, '{drake.mit.edu}declare_convex')

        inertial_items = link_item.findall('inertial')
        if len(inertial_items) == 0:
            # Estimate inertia from the collision mesh pieces
            if len(component_inertias) != 1:
                raise NotImplementedError("Don't know how to combine SpatialInertias.")
            else:
                inertia = component_inertias[0]
            inertial = ET.SubElement(link_item, 'inertial')
            pose_item = ET.SubElement(inertial, 'pose')
            pose_item.text = "{:.2E} {:.2E} {:.2E} 0. 0. 0.".format(
                                *inertia.get_com().tolist())
            mass_item = ET.SubElement(inertial, 'mass')
            mass_item.text = '{:.2E}'.format(inertia.get_mass())
            I = [['{:.4E}'.format(y) for y in x]  # NOQA
                 for x in inertia.CalcRotationalInertia().CopyToFullMatrix3()[:]]
            inertia_item = ET.SubElement(inertial, 'inertia')
            for key, value in zip(
                    ["ixx", "ixy", "ixz", "iyy", "iyz", "izz"],
                    [I[0][0], I[0][1], I[0][2], I[1][1], I[1][2], I[2][2]]):
                item = ET.SubElement(inertia_item, key)
                item.text = value


    output_path = input_file[:-4] + "_simplified.sdf"
    data.write(output_path, pretty_print=True)

if __name__ == "__main__":
    # attach to logger so trimesh messages will be printed to console
    #trimesh.util.attach_to_log()

    # Set to_update to grab the model SDFs from extracted
    # downloaded IgnitionRobotics SDFs.
    # I assume here that you've downloaded a set of model archives
    # from the site, and put them each in their own folder in a
    # data directory somewhere. Each subfolder should have a
    # model.sdf file next to "meshes" and "materials" folders.

    # e.g. the following directories exist:
    # <my data dir>/<model name>/model.sdf
    # <my data dir>/<other>/model.sdf

    data_folder = "./plates_cups_and_bowls/"
    # Update a specific model by name.
    #to_update = glob.glob(data_folder + "/*/Chefmate_8_Frypan/model.sdf")
    # Update all models.
    #to_update = glob.glob(data_folder + "/**/model.sdf", recursive=True)
    #to_update = glob.glob(data_folder + "/**/Plastic_Cup/model.sdf")
    to_update = glob.glob(data_folder + "/utensils/***/model.sdf")
    
    #to_update = glob.glob(data_folder + "/**/Cole_Hardware_Mug_Classic_Blue/model.sdf")
    print(to_update)
    for file in to_update:
        print("Processing %s" % file)
        update_sdf_with_convex_decomp(os.path.abspath(file))
<filename>models/do_model_updates.py
import numpy as np
import trimesh
from trimesh.constants import log
from copy import deepcopy
import glob

import os

import open3d
import numpy as np
import cv2
import imutils

from lxml import etree as ET

from pydrake.all import (
    SpatialInertia,
    RotationalInertia
)

def do_visual_mesh_simplification(input_obj_path, target_tris=1000):
    '''
    Given an obj path, simplifies the geometry and texture so
    it renders more easily in meshcat / drake vis by:
    - Creates a (possibly simplified) mesh alongside it, with a 
    "_simple_vis.obj" post-fox.
    - Looks for a texture file at a fixed relative path
    (../material/textures/texture.png), resizes it to max edge length
    1024 to decrease size, and saves it alongside the new saved visual
    mesh with a .png extension.

    No existing files changed -- just new ones added.

    Args:
    - input_obj_path: String path to obj file
    - target_tris: Currently unusued, but would be target for mesh decimation.

    Returns:
    - output obj file path
    '''
    output_obj_path = input_obj_path[:-4] + "_simple_vis.obj"
    output_texture_path = output_obj_path[:-4] + ".png"
    print("Output path: ", output_texture_path)

    mesh = open3d.io.read_triangle_mesh(input_obj_path)
    simplified_mesh = mesh # mesh.simplify_quadric_decimation(target_tris)
    # Can't quite get this to work as Open3D stores UVs per
    # triangle rather than per vertex.
    # I should get the triangle centers and pull the UV from
    # the corresponding triangle rather than doing it by
    # raw vertex.
    # But this whole pipeline is moot if we have a fast enough
    # renderer, so whatever.
    ## Update UVs on simplified mesh by taking UV from
    ## nearest neighbor on original mesh
    #simplified_verts = np.asarray(simplified_mesh.vertices)
    #corresps = []
    #orig_verts_kdtree = open3d.geometry.KDTreeFlann(np.asarray(mesh.vertices).T)
    #for k in range(simplified_verts.shape[0]):
    #    corresps.append(orig_verts_kdtree.search_knn_vector_3d(simplified_verts[k, :], 1)[2])
    ## Now go through and update triangle uvs
    #n_tris = simplified_verts.num_triangles()
    #tri_inds = np.asarray(simplified_verts.triangles)
    #triangle_uvs = np.zeros(3*n_tris, 2)
    #for k in range(n_tris):
    #    for l in range(3):
    #        corresps[tri_inds[k, l]]
    open3d.io.write_triangle_mesh(output_obj_path, simplified_mesh)

    # Hardcode creation of a texture next to it,
    # looking for the texture in ../materials/textures/texture.png
    # (Downsize the texture to a reasonable size, too)
    dirname, _ = os.path.split(input_obj_path)
    texture_path = os.path.join(dirname, "../materials/textures/texture.png")
    if os.path.exists(texture_path):
        texture_image = open3d.io.read_image(texture_path)
        texture_image = cv2.cvtColor(np.asarray(texture_image), cv2.COLOR_BGR2RGB)
        # Resize to height 1024 (which hopefully results in
        # a reasonably-sized image. 
        height, width = texture_image.shape[:2]
        if height > width:
            texture_image = imutils.resize(texture_image, height=1024)
        else:
            texture_image = imutils.resize(texture_image, width=1024)
        cv2.imwrite(output_texture_path, texture_image)


    else:
        print("Found no texture at ", texture_path)

    return output_obj_path

def do_collision_mesh_simplification(input_obj_path, show=False):
    '''
    Given an obj, performs a convex decomposition of it with
    trimesh _ vhacd, saving all the parts in a subfolder.
    
    Args:
    - input_obj_path: String path to obj file to decompose
    - show: Whether to open (and block on) a window to preview
    the decomposition.

    Returns: (out_paths, inertia)
    - out_paths: List of generated obj files
    - inertia: total inertia of the obj, assuming density of 2000 kg/m^3
    '''

    # Create a subdir for the convex decomp parts, as
    # there might be many.
    dirname, objname = os.path.split(input_obj_path)
    piece_dirname = objname[:-4] + "_parts"
    out_dir = os.path.join(dirname, piece_dirname)
    os.makedirs(out_dir, exist_ok=True)

    mesh = trimesh.load(input_obj_path, skip_materials=True)
    if show:
        mesh.show()
    try:
        convex_pieces = []
        # Do convex decomp multiple times, hoping for overlap.
        args_per_iter = [
            {"maxhulls": 12,
             "maxNumVerticesPerCH": 12,
             "minVolumePerCH": 0.001,
             "resolution": 100000,
             "pca": 1},
            {"maxhulls": 12,
             "maxNumVerticesPerCH": 12,
             "minVolumePerCH": 0.001,
             "resolution": 100000,
             "pca": 1},
        ]
        for k, args in enumerate(args_per_iter):
            # Apply random rotation if not the first iteration
            # (many meshes decompose better upright in axis-aligned arrangement)
            tf = trimesh.transformations.random_rotation_matrix(np.random.rand(3))
            mesh.apply_transform(tf)
            convex_pieces_new = trimesh.decomposition.convex_decomposition(
                mesh, **args) # TODO: args
            # Unrotate and add pieces
            inv_tf = tf.T # No translation so this is OK
            mesh.apply_transform(inv_tf)
            if not isinstance(convex_pieces_new, list):
                convex_pieces_new = [convex_pieces_new]
            for convex_piece in convex_pieces_new:
                convex_piece.apply_transform(inv_tf)
            convex_pieces += convex_pieces_new
    except Exception as e:
        print("Problem in decomp: ", e)
    
    # Give them random colors for display
    for part in convex_pieces:
        this_color = trimesh.visual.random_color()
        part.visual.face_colors[:] = this_color
    scene = trimesh.scene.scene.Scene()
    for part in convex_pieces:
        scene.add_geometry(part)
    if show:
        scene.show()

    mesh.density = 2000
    I = mesh.moment_inertia
    inertia = SpatialInertia.MakeFromCentralInertia(
        mass=mesh.mass,
        p_PScm_E=mesh.center_mass,
        I_SScm_E=RotationalInertia(
            Ixx=I[0, 0],
            Ixy=I[0, 1],
            Ixz=I[0, 2],
            Iyy=I[1, 1],
            Iyz=I[1, 2],
            Izz=I[2, 2]).ShiftFromCenterOfMass(mass=mesh.mass,
                                               p_BcmQ_E=-mesh.center_mass)
    )

    # Save them each out
    out_paths = []
    for k, part in enumerate(convex_pieces):
        piece_name = '%s_convex_piece_%03d.obj' % (objname[:-4], k)
        full_path = os.path.join(out_dir, piece_name)
        trimesh.exchange.export.export_mesh(part, full_path)
        out_paths.append(full_path)

    return out_paths, inertia


def update_sdf_with_convex_decomp(input_file):
    '''
    Given an SDF from IgnitionRobotics, produces a new SDF that:
    - For each visual item, replaces it with a simplified
    visual item with a simplified texture saved alongside,
    which are auto-generated next to the original
    model.
    - For each collision item, replaces it with a convex
    decomp of the collision geometry, which is auto-generated
    next to the original model.
    - Inserts inertia for each link, calculated from the
    collision geometry with an assumed constant density.
    and saves this SDF alongside the original with a
    "_simplified.sdf" suffix.
    '''

    this_file_path, _ = os.path.split(input_file)

    # Open up the SDF as an XML
    parser = ET.XMLParser(remove_blank_text=True)
    data = ET.parse(input_file, parser=parser)
    # Add drake namespace
    ET.register_namespace('drake', 'drake.mit.edu')
    sdf_root = data.getroot()

    link_items = sdf_root.findall('.//link')

    for link_item in link_items:
        visual_items = link_item.findall('visual')
        for visual_item in visual_items:
            # Simplify each mesh
            for visual_mesh in visual_item.findall('.//mesh'):
                for uri in visual_mesh.findall('.//uri'):
                    model_path = os.path.join(this_file_path, uri.text)
                    new_model_path = do_visual_mesh_simplification(model_path)
                    new_model_path = os.path.relpath(new_model_path, this_file_path)
                    # Replace mesh reference with our simplified version
                    uri.text = new_model_path

        collision_items = link_item.findall('collision')
        component_inertias = []
        total_collision_k = 0
        for collision_item in collision_items:
            new_model_paths = []
            for geometry in collision_item.findall('.//geometry'):
                for uri in geometry.findall('.//uri'):
                    model_path = os.path.join(this_file_path, uri.text)
                    new_paths, new_inertia = do_collision_mesh_simplification(model_path)
                    new_model_paths += new_paths
                    component_inertias.append(new_inertia)
            new_model_paths = [
                os.path.relpath(new_model_path, this_file_path)
                for new_model_path in new_model_paths]
            # Create a new XML subtree for the collection of meshes
            # we just created. I *think* each convex piece needs
            # to be in its own collision tag, otherwise Drake
            # seems to be ignoring them...
            link_item.remove(collision_item)

            for path in new_model_paths:
                collision_item = ET.SubElement(link_item, 'collision', name="collision_%04d" % total_collision_k)
                total_collision_k += 1
                geometry_item = ET.SubElement(collision_item, 'geometry')
                mesh_item = ET.SubElement(geometry_item, "mesh")
                uri_item = ET.SubElement(mesh_item, "uri")
                uri_item.text = path
                ET.SubElement(mesh_item, '{drake.mit.edu}declare_convex')

        inertial_items = link_item.findall('inertial')
        if len(inertial_items) == 0:
            # Estimate inertia from the collision mesh pieces
            if len(component_inertias) != 1:
                raise NotImplementedError("Don't know how to combine SpatialInertias.")
            else:
                inertia = component_inertias[0]
            inertial = ET.SubElement(link_item, 'inertial')
            pose_item = ET.SubElement(inertial, 'pose')
            pose_item.text = "{:.2E} {:.2E} {:.2E} 0. 0. 0.".format(
                                *inertia.get_com().tolist())
            mass_item = ET.SubElement(inertial, 'mass')
            mass_item.text = '{:.2E}'.format(inertia.get_mass())
            I = [['{:.4E}'.format(y) for y in x]  # NOQA
                 for x in inertia.CalcRotationalInertia().CopyToFullMatrix3()[:]]
            inertia_item = ET.SubElement(inertial, 'inertia')
            for key, value in zip(
                    ["ixx", "ixy", "ixz", "iyy", "iyz", "izz"],
                    [I[0][0], I[0][1], I[0][2], I[1][1], I[1][2], I[2][2]]):
                item = ET.SubElement(inertia_item, key)
                item.text = value


    output_path = input_file[:-4] + "_simplified.sdf"
    data.write(output_path, pretty_print=True)

if __name__ == "__main__":
    # attach to logger so trimesh messages will be printed to console
    #trimesh.util.attach_to_log()

    # Set to_update to grab the model SDFs from extracted
    # downloaded IgnitionRobotics SDFs.
    # I assume here that you've downloaded a set of model archives
    # from the site, and put them each in their own folder in a
    # data directory somewhere. Each subfolder should have a
    # model.sdf file next to "meshes" and "materials" folders.

    # e.g. the following directories exist:
    # <my data dir>/<model name>/model.sdf
    # <my data dir>/<other>/model.sdf

    data_folder = "./plates_cups_and_bowls/"
    # Update a specific model by name.
    #to_update = glob.glob(data_folder + "/*/Chefmate_8_Frypan/model.sdf")
    # Update all models.
    #to_update = glob.glob(data_folder + "/**/model.sdf", recursive=True)
    #to_update = glob.glob(data_folder + "/**/Plastic_Cup/model.sdf")
    to_update = glob.glob(data_folder + "/utensils/***/model.sdf")
    
    #to_update = glob.glob(data_folder + "/**/Cole_Hardware_Mug_Classic_Blue/model.sdf")
    print(to_update)
    for file in to_update:
        print("Processing %s" % file)
        update_sdf_with_convex_decomp(os.path.abspath(file))
Given an obj path, simplifies the geometry and texture so     it renders more easily in meshcat / drake vis by:     - Creates a (possibly simplified) mesh alongside it, with a      "_simple_vis.obj" post-fox.     - Looks for a texture file at a fixed relative path     (../material/textures/texture.png), resizes it to max edge length     1024 to decrease size, and saves it alongside the new saved visual     mesh with a .png extension.      No existing files changed -- just new ones added.      Args:     - input_obj_path: String path to obj file     - target_tris: Currently unusued, but would be target for mesh decimation.      Returns:     - output obj file path # mesh.simplify_quadric_decimation(target_tris) # Can't quite get this to work as Open3D stores UVs per # triangle rather than per vertex. # I should get the triangle centers and pull the UV from # the corresponding triangle rather than doing it by # raw vertex. # But this whole pipeline is moot if we have a fast enough # renderer, so whatever. ## Update UVs on simplified mesh by taking UV from ## nearest neighbor on original mesh #simplified_verts = np.asarray(simplified_mesh.vertices) #corresps = [] #orig_verts_kdtree = open3d.geometry.KDTreeFlann(np.asarray(mesh.vertices).T) #for k in range(simplified_verts.shape[0]): #    corresps.append(orig_verts_kdtree.search_knn_vector_3d(simplified_verts[k, :], 1)[2]) ## Now go through and update triangle uvs #n_tris = simplified_verts.num_triangles() #tri_inds = np.asarray(simplified_verts.triangles) #triangle_uvs = np.zeros(3*n_tris, 2) #for k in range(n_tris): #    for l in range(3): #        corresps[tri_inds[k, l]] # Hardcode creation of a texture next to it, # looking for the texture in ../materials/textures/texture.png # (Downsize the texture to a reasonable size, too) # Resize to height 1024 (which hopefully results in # a reasonably-sized image. Given an obj, performs a convex decomposition of it with     trimesh _ vhacd, saving all the parts in a subfolder.          Args:     - input_obj_path: String path to obj file to decompose     - show: Whether to open (and block on) a window to preview     the decomposition.      Returns: (out_paths, inertia)     - out_paths: List of generated obj files     - inertia: total inertia of the obj, assuming density of 2000 kg/m^3 # Create a subdir for the convex decomp parts, as # there might be many. # Do convex decomp multiple times, hoping for overlap. # Apply random rotation if not the first iteration # (many meshes decompose better upright in axis-aligned arrangement) # TODO: args # Unrotate and add pieces # No translation so this is OK # Give them random colors for display # Save them each out Given an SDF from IgnitionRobotics, produces a new SDF that:     - For each visual item, replaces it with a simplified     visual item with a simplified texture saved alongside,     which are auto-generated next to the original     model.     - For each collision item, replaces it with a convex     decomp of the collision geometry, which is auto-generated     next to the original model.     - Inserts inertia for each link, calculated from the     collision geometry with an assumed constant density.     and saves this SDF alongside the original with a     "_simplified.sdf" suffix. # Open up the SDF as an XML # Add drake namespace # Simplify each mesh # Replace mesh reference with our simplified version # Create a new XML subtree for the collection of meshes # we just created. I *think* each convex piece needs # to be in its own collision tag, otherwise Drake # seems to be ignoring them... # Estimate inertia from the collision mesh pieces # NOQA # attach to logger so trimesh messages will be printed to console #trimesh.util.attach_to_log() # Set to_update to grab the model SDFs from extracted # downloaded IgnitionRobotics SDFs. # I assume here that you've downloaded a set of model archives # from the site, and put them each in their own folder in a # data directory somewhere. Each subfolder should have a # model.sdf file next to "meshes" and "materials" folders. # e.g. the following directories exist: # <my data dir>/<model name>/model.sdf # <my data dir>/<other>/model.sdf # Update a specific model by name. #to_update = glob.glob(data_folder + "/*/Chefmate_8_Frypan/model.sdf") # Update all models. #to_update = glob.glob(data_folder + "/**/model.sdf", recursive=True) #to_update = glob.glob(data_folder + "/**/Plastic_Cup/model.sdf") #to_update = glob.glob(data_folder + "/**/Cole_Hardware_Mug_Classic_Blue/model.sdf")