robotic 0.2.9.dev1__cp312-cp312-manylinux2014_x86_64.whl → 0.3.0__cp312-cp312-manylinux2014_x86_64.whl

robotic/src/mesh_helper.py

@@ -0,0 +1,395 @@
+import os
+import numpy as np
+#import matplotlib.pyplot as plt
+import trimesh
+import base64
+import yaml
+import h5py
+import matplotlib.pyplot as plt
+from PIL import Image
+
+def write_arr(X, fil, type='float32'):
+    data = (type, list(X.shape), )
+    fil.write(f'[ "{type}", {list(X.shape)}, "')
+    fil.write(base64.b64encode(X.astype(type)).decode('utf-8'))
+    fil.write('" ]')
+
+def conv_tuple_arr(data_tuple):
+    X = np.frombuffer(base64.decodebytes(bytearray(data_tuple[2].encode('utf-8'))), dtype=data_tuple[0])
+    X = X.reshape(data_tuple[1])
+    return X
+
+class MeshHelper():
+    def __init__(self, file):
+        self.failed = False
+        self.inertiaIsDiagonal = False
+        if file[-3:]==".h5":
+            self.load_h5(file)
+        else:
+            self.load(file)
+            self.file = file
+
+    def load(self, file):
+        print('=== file: ', file)
+        self.filebase = os.path.splitext(file)[0]
+        self.mesh = trimesh.load(file, force='mesh')
+        try:
+            scene_or_mesh = trimesh.load(file, force='mesh')
+        except Exception as e:
+            print(e)
+            print('  load failed:', file)
+            return None
+
+        if isinstance(scene_or_mesh, trimesh.Scene):
+            if len(scene_or_mesh.geometry) == 0:
+                self.mesh = None
+                self.failed = True
+            else:
+                # we lose texture information here
+                self.mesh = trimesh.util.concatenate(
+                    tuple(trimesh.Trimesh(vertices=g.vertices, faces=g.faces)
+                        for g in scene_or_mesh.geometry.values()))
+        else:
+            assert(isinstance(scene_or_mesh, trimesh.Trimesh))
+            self.mesh = scene_or_mesh
+
+    def view(self):
+        self.mesh.show(resolution=(300,300))
+
+    def report(self):
+        print('  filebase:', self.filebase)
+        print('  #vertices:', self.mesh.vertices.shape)
+        print('  #faces:', self.mesh.faces.shape)
+        if hasattr(self.mesh.visual, 'uv'):
+            print('#uv:', self.mesh.visual.uv.shape)
+
+    def autoScale(self):
+        scale = 1
+        #if self.mesh.scale > 10000:
+        #    scale = 1e-5
+        #elif self.mesh.scale > 1000:
+        #    scale = 1e-4
+        if self.mesh.scale > 200:
+            scale = 1e-3
+        elif self.mesh.scale > 20:
+            scale = 1e-2
+        elif self.mesh.scale > 2:
+            scale = 1e-1
+        #translate = -mesh.centroid
+        translate = -.5*(self.mesh.bounds[0]+self.mesh.bounds[1])
+        matrix = np.eye(4)
+        matrix[0:3, 3] = translate
+        matrix[0:3, 0:4] *= scale
+        self.mesh.apply_transform(matrix)
+
+    def transformInertia(self, verbose=False):
+        print("prior COM" , self.mesh.center_mass)
+        print("prior inertia\n" , self.mesh.moment_inertia)
+
+        U, D, V = np.linalg.svd(self.mesh.moment_inertia)
+
+        # Ensure proper rotation  
+        if np.linalg.det(V) < 0:
+            V[:, -1] *= -1
+
+        matrix = np.eye(4)
+        matrix[0:3, 3] = V @ (-self.mesh.center_mass)  # Move center of mass to origin
+        matrix[0:3, 0:3] = V  # Rotate the mesh to align with principal axes
+
+        self.mesh.apply_transform(matrix)
+
+        if verbose:
+            print("COM after transformation:" , self.mesh.center_mass)
+            print("Inertia after transfromation:\n" , self.mesh.moment_inertia)
+        self.inertiaIsDiagonal = True
+
+        return np.linalg.inv(matrix)
+
+    def repair(self, mergeTolerance=1e-6):
+        try:
+            trimesh.constants.tol.merge = mergeTolerance
+            self.mesh.process(validate=True)
+            trimesh.repair.fill_holes(self.mesh)
+            trimesh.repair.fix_inversion(self.mesh, multibody=True)
+        except Exception as e:
+            print('  --- repair failed ---', e)
+            print('  --- this might be a trimesh bug: change order within mesh.process method')
+            self.failed = True
+            exit(0) # this might be a trimesh bug: change order within mesh.process method
+
+        # if self.mesh.visual != None:
+            # self.mesh.visual.uv = np.array([[0,0]])
+
+    def texture2vertexColors(self):
+        if hasattr(self.mesh.visual, 'uv'):
+            colors = self.mesh.visual.material.to_color(self.mesh.visual.uv)
+            vis = trimesh.visual.ColorVisuals(mesh=self.mesh, vertex_colors=colors)
+            self.mesh.visual = vis
+        # else:
+            # raise ValueError("Mesh does not have UV coordinates!")
+
+    def export_ply(self, filename=None):
+        if filename is None:
+            filename = self.filebase+'-.ply'
+        print('  exporting', filename)
+        self.mesh.export(filename)
+
+    def export_scene(self, convex=False):
+        with open(self.filebase+'.g', 'w', encoding='utf-8') as fil:
+            if self.inertiaIsDiagonal:
+                if convex:
+                    fil.write(f'obj: {{ X:[0., 0., 1.], mesh_decomp: <{self.filebase}.h5>, mass: {self.mesh.center_mass.tolist()}, inertia: {np.diagonal(self.mesh.moment_inertia).tolist()} }}\n')
+            else:
+                fil.write(f'obj: {{ X:[0., 0., 12.], mass: {self.mesh.center_mass.tolist()}, inertia: {self.mesh.moment_inertia.reshape([9]).tolist()} }}\n')
+            fil.write(f'obj_mesh (obj): {{ mesh: <{self.filebase}.h5> }}\n')
+            #fil.write(f'obj_points (obj): {{ mesh_points: <{self.filebase}.h5>, color: [1 1 0], size: [2.] }}\n')
+            return self.filebase+'.g'
+
+    def createPoints(self):
+        self.pts, faces = trimesh.sample.sample_surface(self.mesh, 20000)
+        self.normals = self.mesh.face_normals[faces]
+        #bary = trimesh.triangles.points_to_barycentric(self.mesh.triangles[faces], pts)
+        #normals = trimesh.unitize((self.mesh.vertex_normals[self.mesh.faces[faces]] *
+        #                          trimesh.unitize(bary).reshape((-1, 3, 1))).sum(axis=1))
+    
+
+    def createDecomposition(self):
+
+        convex_hulls = self.mesh.convex_decomposition()
+        
+        self.decomp_parts = [0]
+        self.decomp_vertices = np.asarray(convex_hulls[0].vertices)
+        self.decomp_faces = np.asarray(convex_hulls[0].faces)
+        vert_len = len(convex_hulls[0].vertices) 
+        for i, part in enumerate(convex_hulls[1:]):                
+
+            self.decomp_vertices = np.concatenate([self.decomp_vertices, part.vertices]) 
+            self.decomp_faces = np.concatenate([self.decomp_faces, part.faces + vert_len])  
+            self.decomp_parts.append(len(part.vertices)+self.decomp_parts[i])
+
+            vert_len+=len(part.vertices)
+        
+        self.decomp_parts = np.asarray(self.decomp_parts)
+        self.decomp_colors = [0,255,255]  # fixed blue for now
+
+    def createDecomposition_lowlevel(self):
+        ### create decomposition
+        ret = os.system('ry-meshTool.sh ' + self.filebase+'.mesh' + ' -decomp -hide -quiet'
+                        ' && mv z.arr ' + self.filebase+'.decomp' )
+        if ret>0:
+            print('  --- decomposition failed --- return:', ret)
+            self.failed=True
+            return
+
+        ### load decomposition
+        with open(self.filebase+'.decomp', 'r') as fil:
+            decomp = yaml.safe_load(fil)
+        self.decomp_vertices = conv_tuple_arr(decomp['V'])
+        self.decomp_faces = conv_tuple_arr(decomp['T'])
+        self.decomp_colors = conv_tuple_arr(decomp['C'])
+        self.decomp_parts = conv_tuple_arr(decomp['cvxParts'])
+
+    def export_h5(self, filename=None, inertia=False):
+        if filename is None:
+            filename = self.filebase+'.h5'
+        print('  exporting', filename)
+        with h5py.File(filename, 'w') as fil:
+            fil.create_dataset('mesh/vertices', data=self.mesh.vertices, dtype='float32')
+            assert self.mesh.faces.shape[1]==3, 'can only export triangle meshes'
+            if(self.mesh.vertices.shape[0]<65535):
+                fil.create_dataset('mesh/faces', data=self.mesh.faces, dtype='uint16')
+            else:
+                fil.create_dataset('mesh/faces', data=self.mesh.faces, dtype='uint32')
+            
+            if hasattr(self.mesh.visual, 'vertex_colors'):
+                colors = np.asarray(self.mesh.visual.vertex_colors)[:,0:3]
+                fil.create_dataset('mesh/colors', data=colors, dtype='uint8')
+
+            if hasattr(self.mesh.visual, 'uv'):
+                texCoords = self.mesh.visual.uv.copy()
+                texCoords[:, 1] = 1 - texCoords[:, 1]
+                fil.create_dataset('mesh/textureCoords', data=texCoords, dtype='float32')
+
+                if self.textureFile is not None:
+                    file = np.frombuffer(self.textureFile.encode(), dtype=np.int8)
+                    file = np.append(file, [0])
+                    fil.create_dataset('mesh/textureFile', data=file, dtype='int8')
+                else:
+                    if hasattr(self.mesh.visual.material, 'baseColorTexture'):
+                        img = self.mesh.visual.material.baseColorTexture
+                    else:
+                        img = self.mesh.visual.material.image
+                    if img is not None:
+                        texImg = 255.*np.asanyarray(img.convert("RGB"))
+                        fil.create_dataset('mesh/textureImg', data=texImg, dtype='uint8')
+
+            if hasattr(self, 'pts') and self.pts.shape[1]==3:
+                fil.create_dataset('points/vertices', data=self.pts, dtype='float32')
+                fil.create_dataset('points/normals', data=self.normals, dtype='float32')
+
+            if hasattr(self, 'decomp_vertices') and self.decomp_vertices.shape[1]==3:
+                fil.create_dataset('decomp/vertices', data=self.decomp_vertices, dtype='float32')
+                assert self.decomp_faces.shape[0]<65535
+                fil.create_dataset('decomp/faces', data=self.decomp_faces, dtype='uint16')
+                fil.create_dataset('decomp/colors', data=self.decomp_colors, dtype='uint8')
+                assert self.decomp_parts.shape[ 0]<65535
+                fil.create_dataset('decomp/parts', data=self.decomp_parts, dtype='uint16')
+
+            if inertia:
+                fil.create_dataset('inertia/mass', data=[self.mesh.mass], dtype='float32')
+                fil.create_dataset('inertia/com', data=self.mesh.center_mass, dtype='float32')
+                if self.inertiaIsDiagonal:
+                    fil.create_dataset('inertia/tensor', data=np.diagonal(self.mesh.moment_inertia), dtype='float32')
+                else:
+                    fil.create_dataset('inertia/tensor', data=self.mesh.moment_inertia, dtype='float32')
+        
+    def load_h5(self, file):
+        print('=== file: ', file)
+        self.filebase = os.path.splitext(file)[0]
+        path, _ = os.path.split(file)
+        with h5py.File(file, 'r') as fil:
+            V = fil['mesh/vertices'][()]
+            T = fil['mesh/faces'][()]
+            if 'mesh/colors' in fil:
+                C = fil['mesh/colors'][()]
+            else:
+                C = None
+
+            texture = None
+            texImage = None
+            if 'mesh/textureFile' in fil:
+                self.textureFile = fil['mesh/textureFile'][()]
+                self.textureFile = ''.join([chr(i) for i in self.textureFile])[:-1] #cut the last embedding zero
+                print(self.textureFile)
+                texImage = Image.open(os.path.join(path, self.textureFile))
+                material = trimesh.visual.texture.SimpleMaterial(image=texImage)
+            if 'mesh/textureImg' in fil:
+                texImage = fil['mesh/textureImg'][()]
+                material = trimesh.visual.texture.SimpleMaterial(image=texImage)
+            if 'mesh/textureCoords' in fil and texImage is not None:
+                uv = fil['mesh/textureCoords'][()]
+                uv[:, 1] = 1 - uv[:, 1]
+                texture = trimesh.visual.TextureVisuals(uv=uv, image=texImage)
+
+            if texture is None:
+                self.mesh = trimesh.Trimesh(vertices=V, faces=T, vertex_colors=C, process=True)
+            else:
+                self.mesh = trimesh.Trimesh(vertices=V, faces=T, visual=texture, material=material, process=True)
+
+                # colors = self.mesh.visual.material.to_color(self.mesh.visual.uv)
+                # colors = mat.to_color(uv)
+                # vis = trimesh.visual.ColorVisuals(mesh=self.mesh, vertex_colors=colors)
+                # self.mesh.visual = vis
+
+
+    def apply_texture(self, texture_path):
+        try:
+            texture_image = Image.open(texture_path)
+            texture = trimesh.visual.TextureVisuals(image=texture_image, uv=self.mesh.visual.uv)
+            self.mesh.visual = texture
+            
+            # texture = np.array(texture_image).astype(float) / 255.0
+            # if texture.shape[-1] == 3:
+            #     alpha = np.ones((texture.shape[0], texture.shape[1], 1))
+            #     texture = np.concatenate([texture, alpha], axis=-1)
+            
+            # if not hasattr(self.mesh.visual, 'uv'):
+            #     raise ValueError("Mesh does not have UV coordinates!")
+            
+            # uv_coords = self.mesh.visual.uv
+            # uv_coords_copy = uv_coords.copy()
+            # uv_coords_copy[:, 1] = 1 - uv_coords_copy[:, 1]
+            
+            # pixel_coords = (uv_coords_copy * [texture.shape[1] - 1, texture.shape[0] - 1]).astype(int)
+            # vertex_colors = texture[pixel_coords[:, 1], pixel_coords[:, 0]]
+            
+            # self.mesh.visual = trimesh.visual.ColorVisuals(mesh=self.mesh, vertex_colors=vertex_colors)
+        except Exception as e:
+            print(texture_path, "is not a valid texture path or could not be applied to the mesh.", e)
+
+
+    # def apply_scaling(self, tri_obj, scale):
+    #     if scale != 1.0:
+    #         print(scale)
+    #         scaling_mat = scale * np.eye(4)
+    #         scaling_mat[-1, -1] = 1.0
+    #         tri_obj.apply_transform(scaling_mat)
+
+
+    # def export_mesh(self, tri_obj, meshfile, ply_out, transformInertia, cvxDecomp):
+    #     tri_obj.export(ply_out)
+    #     print(f"Converted {meshfile}")
+    #     M = MeshHelper(ply_out)
+    #     if transformInertia:
+    #         self.pose = M.transformInertia()
+    #     if cvxDecomp:
+    #         M.createDecomposition()
+    #     M.export_h5(meshfile[:-3]+'h5', inertia=False)
+
+
+    # def obj2ply(self, ply_out: str, scale: float=1.0, texture_path: str="none", transformInertia = False, cvxDecomp = False) -> bool:
+    #     tri_obj = self.mesh
+    #     if hasattr(tri_obj.visual, 'to_color'):
+    #         if texture_path == "none":
+    #             tri_obj.visual = tri_obj.visual.to_color()
+    #         else:
+    #             self.apply_texture(tri_obj, texture_path)
+            
+    #     elif hasattr(tri_obj.visual, 'vertex_colors'):
+    #         print("Mesh already has vertex colors.")
+        
+    #     else:
+    #         print(f"Failed on {tri_obj}")
+    #         return False
+        
+    #     self.apply_scaling(tri_obj, scale)
+    #     self.export_mesh(tri_obj, self.file, ply_out, transformInertia, cvxDecomp)
+        
+    #     return True
+    
+def timeout(signum, frame):
+    raise Exception("timeout handler")
+
+def get_sdf(mesh, N=30):
+    #scale_mesh(mesh, .1)
+    # get bounds
+    bounds = mesh.bounds.copy()
+    size = bounds[1] - bounds[0]
+    # enlarge by 10%
+    bounds[0] = bounds[0] - .1 * size
+    bounds[1] = bounds[1] + .1 * size
+    size = bounds[1] - bounds[0]
+    # decide on a voxel size
+    voxelSize = 1. / (N * np.power(np.prod(size), -1. / 3))
+    gridDim = (size / voxelSize).astype(int) + 2
+    # change bounds[1] to be on the grid
+    bounds[1] = bounds[0] + voxelSize * (gridDim - 1)
+    print('  sdf voxel:', voxelSize, 'dim:', gridDim, 'mem:', np.prod(gridDim))
+    # create grid
+    x_range = np.linspace(bounds[0, 0], bounds[1, 0], num=gridDim[0])
+    y_range = np.linspace(bounds[0, 1], bounds[1, 1], num=gridDim[1])
+    z_range = np.linspace(bounds[0, 2], bounds[1, 2], num=gridDim[2])
+    grid = np.stack(np.meshgrid(x_range, y_range, z_range, indexing='ij'), axis=-1)
+    # get sdf
+    #sdf = -mesh.nearest.signed_distance(grid).reshape(gridDim)
+    sdf = np.empty(gridDim)
+    for z in range(0, sdf.shape[2]):
+        print('\r  slice', z, end=' ', flush=True)
+        sdf[:,:,z] = -mesh.nearest.signed_distance(grid[:,:,z,:].reshape(-1, 3)).reshape(gridDim[:2])
+    print('- done')
+    #scale_mesh(mesh, 10.)
+    #bounds *= 10.
+    return [sdf, bounds]
+
+def display_sdf(sdf):
+    ax = plt.subplot()
+    cax = plt.axes([0.85, 0.1, 0.075, 0.8])
+    for z in range(0, sdf.shape[2]):
+        print(z)
+        print(sdf[:, :, z])
+        im = ax.imshow(sdf[:, :, z])
+        plt.colorbar(im, cax=cax)
+        plt.pause(.2)
+
+
+        

robotic/{rai-robotModels/ranger/meshes/cleanMeshes.mlx → src/meshlabFilters.mlx}

@@ -1,8 +1,5 @@
 <!DOCTYPE FilterScript>
 <FilterScript>
- <filter name="Merge Close Vertices">
-  <Param tooltip="All the vertices that closer than this threshold are merged together. Use very small values, default values is 1/10000 of bounding box diagonal. " type="RichAbsPerc" description="Merging distance" min="0" value="0.001" name="Threshold" max="0.00938277"/>
- </filter>
  <filter name="Repair non Manifold Edges by removing faces"/>
  <filter name="Simplification: Quadric Edge Collapse Decimation">
   <Param tooltip="The desired final number of faces." type="RichInt" description="Target number of faces" value="20000" name="TargetFaceNum"/>

robotic/src/mujoco_io.py

@@ -0,0 +1,242 @@
+
+import os
+import numpy as np
+import robotic as ry
+import xml.etree.ElementTree as ET
+from copy import copy
+
+class MujocoLoader():
+
+    def __init__(self, file, visualsOnly=True, defaultConType='0', basePos=[0,0,0], baseQuat=[0,0,0,1]):
+        ry.params_add({'cd_into_mesh_files': False})
+
+        self.visualsOnly = visualsOnly
+        self.defaultConType = defaultConType
+        self.debug_counter = 0
+        self.muj2rai_joint_map = {
+            '1 0 0': ry.JT.hingeX,
+            '0 1 0': ry.JT.hingeY,
+            '0 0 1': ry.JT.hingeZ,
+            '-1 0 0': ry.JT.hingeX,
+            '0 -1 0': ry.JT.hingeY,
+            '0 0 -1': ry.JT.hingeZ,
+        }
+
+        tree = ET.parse(file)
+        path, _ = os.path.split(file)
+        root = tree.getroot()
+
+        self.materials = {}    
+        self.textures = {}
+        self.meshes = {}
+        self.load_assets(root, path)
+        self.bodyCount = -1
+
+        self.C = ry.Config()
+        self.base = self.C.addFrame('base')
+        self.base.addAttributes({'multibody':True})
+        self.base.setPosition(basePos)
+        self.base.setQuaternion(baseQuat)
+        self.add_node(root, self.base, path, 0)
+
+    def as_floats(self, input_string):
+        return [float(num) for num in input_string.replace(',', ' ').split()]
+    
+    def load_assets(self, root, path):
+        texs = root.findall('.//texture')
+        for tex in texs:
+            name = tex.attrib.get('name', '')
+            file = tex.attrib.get('file', '')
+            self.textures[name] = os.path.join(path, file)
+
+        maters = root.findall('.//material')
+        for mater in maters:
+            name = mater.attrib.get('name', '')
+            color = mater.attrib.get('rgba', '')
+            texture_name = mater.attrib.get('texture', '')
+            if texture_name:
+                self.materials[name] = self.textures[texture_name]
+            elif color:
+                self.materials[name] = color
+            else:
+                self.materials[name] = ''
+
+        for mesh in root.findall('.//mesh'):
+            name = mesh.attrib.get('name', '')
+            file = mesh.attrib.get('file', '')
+            if file.startswith('visual') or file.startswith('collision'): #HACK: the true path is hidden in some compiler attribute
+                file = 'meshes/'+file
+            mesh.attrib['file'] = file
+            self.meshes[name] = mesh.attrib
+    
+    def add_node(self, node, f_parent, path, level):
+        if 'file' in node.attrib:
+            file = node.attrib['file']
+            node.attrib['file'] = os.path.join(path, file)
+
+        print('|'+level*'  ', node.tag, node.attrib)
+
+        if node.tag == 'include':
+            file = node.attrib['file'] 
+            tree = ET.parse(file)
+            root = tree.getroot()
+            path, _ = os.path.split(file)
+            self.load_assets(root, path)
+            self.add_node(root, f_parent, path, level+1)
+
+        f_body = None
+
+        if node.tag == 'body':
+            f_body = self.add_body(node, f_parent)
+
+        for child in node:
+            if f_body is None:
+                self.add_node(child, f_parent, path, level+1)
+            else:
+                self.add_node(child, f_body, path, level+1)
+
+    def add_body(self, body, f_parent):
+        self.bodyCount += 1
+        body_name = body.attrib.get('name', f'body_{self.bodyCount}')
+
+        # if self.C.getFrame(body_name, False):
+        body_name = f'{body_name}_{self.bodyCount}'
+        f_body = self.C.addFrame(body_name)
+        f_body.setParent(f_parent)
+        self.setRelativePose(f_body, body.attrib)
+
+        for i, joint in enumerate(body.findall('./joint')):
+            axis = joint.attrib.get('axis', None)
+            limits = joint.attrib.get('range', None)
+            joint_name = joint.attrib.get('name', f'{body_name}_joint{i*"_"}')
+            f_origin = self.C.addFrame(f'{joint_name}_origin')
+            f_origin.setParent(f_body)
+            self.setRelativePose(f_origin, joint.attrib)
+            f_origin.unLink()
+            f_origin.setParent(f_parent, True)
+
+            if axis:
+                if axis in self.muj2rai_joint_map:
+                    axis = self.muj2rai_joint_map[axis]
+                else:
+                    vec1 = np.array([0., 0., 1.])
+                    vec2 = np.array(self.as_floats(axis))
+                    quat = ry.Quaternion().setDiff(vec1, vec2).getArr()
+                    f_origin.setRelativeQuaternion(quat)
+                    axis = ry.JT.hingeZ
+            else:
+                axis = ry.JT.hingeZ
+
+            if joint.attrib.get('type', 'hinge')=='slide':
+                trans_map = {
+                    ry.JT.hingeX: ry.JT.transX,
+                    ry.JT.hingeY: ry.JT.transY,
+                    ry.JT.hingeZ: ry.JT.transZ}
+                axis = trans_map[axis]
+
+            if not limits:
+                limits = '-1 1'
+
+            # if self.C.getFrame(joint_name, False):
+            #     joint_name = f'{joint_name}_{self.bodyCount}'
+            f_joint = self.C.addFrame(joint_name)
+            f_joint.setParent(f_origin)
+            f_joint.setJoint(axis, self.as_floats(limits))
+            
+            # relink body:
+            f_parent = f_joint
+            f_body.unLink()
+            f_body.setParent(f_parent, True)
+
+        for i, geom in enumerate(body.findall('./geom')):
+            isColl = geom.attrib.get('contype', self.defaultConType)!='0' or 'coll' in geom.attrib.get('class','')
+            if self.visualsOnly and isColl:
+                continue
+
+            f_shape = self.C.addFrame(f'{body_name}_shape{i}')
+            f_shape.setParent(f_body)
+
+            if 'mesh' in geom.attrib:
+                mesh = geom.attrib.get('mesh', '')
+                material_name = geom.attrib.get('material', '')
+                texture_path = self.materials.get(material_name, None)
+                meshfile = self.meshes[mesh]['file']
+                scale = self.as_floats(self.meshes[mesh].get('scale', '1 1 1'))
+
+                f_shape.setMeshFile(meshfile, scale[0])
+                
+                if texture_path:
+                    if len(texture_path.split()) == 4:  # Is a color rgba
+                        f_shape.setColor(self.as_floats(texture_path))
+
+            elif 'type' in geom.attrib:
+                size = self.as_floats(geom.attrib['size'])
+                if geom.attrib['type']=='capsule':
+                    if 'fromto' in geom.attrib:
+                        fromto = self.as_floats(geom.attrib['fromto'])
+                        a, b = np.array(fromto[:3]), np.array(fromto[3:])
+                        l = np.linalg.norm(b-a)
+                        q = ry.Quaternion().setDiff([0,0,1],(b-a)/l)
+                        f_shape.setRelativePosition(0.5*(a+b))
+                        f_shape.setRelativeQuaternion(q.getArr())
+                        f_shape.setShape(ry.ST.capsule, [l, size[0]])
+                    elif len(size)==2:
+                        f_shape.setShape(ry.ST.capsule, [2.*size[1], size[0]])
+
+                elif geom.attrib['type']=='cylinder':
+                    if len(size)==2:
+                        f_shape.setShape(ry.ST.cylinder, [2.*size[1], size[0]])
+
+                elif geom.attrib['type']=='box':
+                    assert len(size)==3
+                    size = [2.*f for f in size]
+                    f_shape.setShape(ry.ST.box, size)
+                    if geom.attrib.get('material', None):
+                        texture_path = self.materials[geom.attrib.get('material', None)]
+                        if len(texture_path.split()) == 4:  # Is a color rgba
+                            f_shape.setColor(self.as_floats(texture_path))
+                        else:
+                            print('applying to box:', texture_path)
+                            #TODO incorperate <texrepeat> tag correctly
+                            uv_coords = np.array([
+                            [0, 0],  # vertex 0
+                            [size[0], 0],  # vertex 1
+                            [size[0], size[1]],  # vertex 2
+                            [0, size[1]],  # vertex 3
+                            [0, 0],  # vertex 0
+                            [size[0], 0],  # vertex 1
+                            [size[0], size[1]],  # vertex 2
+                            [0, size[1]],  # vertex 3
+                            ])
+                            
+                            f_shape.setTextureFile(texture_path, uv_coords)
+
+                elif geom.attrib['type']=='sphere':
+                    if len(size)==1:
+                        f_shape.setShape(ry.ST.sphere, size)
+                
+            self.setRelativePose(f_shape, geom.attrib)
+
+            if geom.attrib.get('rgba', None):
+                if geom.attrib.get('material', None) is None:
+                    f_shape.setColor(self.as_floats(geom.attrib['rgba']))
+
+            elif isColl:
+                f_shape.setColor([1,0,0,.2])
+                
+        return f_body
+
+    def setRelativePose(self, f, attrib):
+        pos = attrib.get('pos', None)
+        if pos:
+            f.setRelativePosition(self.as_floats(pos))
+        
+        quat = attrib.get('quat', None)
+        if quat:
+            f.setRelativeQuaternion(self.as_floats(quat))
+        
+        rpy = attrib.get('euler', None)
+        if rpy:
+            q = ry.Quaternion()
+            q.setRollPitchYaw(self.as_floats(rpy))
+            f.setRelativeQuaternion(q.getArr())

robotic/src/yaml_helper.py

@@ -0,0 +1,19 @@
+import yaml
+#from ruamel import yaml
+
+# yaml dump tweaks
+class noflow_dict(dict):
+    pass
+def noflow_dict_rep(dumper, data):
+    return dumper.represent_mapping("tag:yaml.org,2002:map", data, flow_style=False)
+yaml.add_representer(noflow_dict, noflow_dict_rep)
+
+class quoted_string(str):
+    pass
+def quoted_string_rep(dumper, data):
+    return dumper.represent_scalar("tag:yaml.org,2002:str", data, style='"')
+yaml.add_representer(quoted_string, quoted_string_rep)
+
+def yaml_write_dict(data, filename):
+    with open(filename, 'w') as fil:
+        yaml.dump(noflow_dict(data), fil, default_flow_style=True, sort_keys=False, width=500)

robotic/test.py

@@ -0,0 +1,15 @@
+import point_cloud_utils as pcu
+
+# v is a [n, 3] shaped NumPy array of vertices
+# f is a [m, 3] shaped integer NumPy array of indices into v
+# n is a [n, 3] shaped NumPy array of vertex normals
+v, f, n = pcu.load_mesh_vfn("bunny.ply")
+
+# Generate barycentric coordinates of random samples
+num_samples = 1000
+fid, bc = pcu.sample_mesh_random(v, f, num_samples)
+
+# Interpolate the vertex positions and normals using the returned barycentric coordinates
+# to get sample positions and normals
+rand_positions = pcu.interpolate_barycentric_coords(f, fid, bc, v)
+rand_normals = pcu.interpolate_barycentric_coords(f, fid, bc, n)

robotic/version.py

@@ -1 +1 @@
-__version__ = '0.2.9.dev1'
+__version__ = '0.3.0'