robotic 0.2.8.dev4__cp311-cp311-manylinux2014_x86_64.whl → 0.3.4.dev5__cp311-cp311-manylinux2014_x86_64.whl

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.setAttributes({'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).asArr()
+                    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.asArr())
+                        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.asArr())

robotic/src/urdf_io.py

@@ -0,0 +1,237 @@
+import os
+import numpy as np
+import robotic as ry
+from lxml import etree
+
+
+class URDFLoader():
+
+    def __init__(self, file, visualsOnly=True, meshPathRemove='', meshExt=None):
+        self.meshPathRemove = meshPathRemove
+        self.meshExt = meshExt
+
+        xmlData = etree.parse(file)
+        self.path, _ = os.path.split(file)
+
+        self.C = ry.Config()
+
+        useCollisionShapes = not visualsOnly
+
+        # materials
+
+        self.materials = {}    
+        mats = xmlData.findall('./material')
+        for mat in mats:
+            name = mat.attrib.get('name', '')
+            tex = mat.find('texture')
+            if tex is not None:
+                tex_file = tex.attrib['filename'].replace(self.meshPathRemove,'',1)
+                self.materials[name] = os.path.join(self.path, tex_file)
+            else:
+                col = mat.find('color')
+                if col is not None:
+                    self.materials[name] = self.as_floats(col.attrib['rgba'])
+
+        # links with shapes
+
+        links = xmlData.findall('./link')
+        for link in links:
+            link_name = link.attrib['name']
+            f_link = self.C.addFrame(link_name)
+
+
+            elem = link.find('inertial/inertia')
+            matrix=[]
+            if elem is not None:
+                tags = ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']
+                matrix = [float(elem.attrib[tag]) for tag in tags]
+
+            elem = link.find('inertial/mass')
+            if elem is not None:
+                mass = float(elem.attrib['value'])
+                f_link.setMass(mass, matrix)
+
+            for visual in link.findall('visual'):
+                self.add_shape(visual, f'{link_name}_0', f_link, isVisual=True)
+
+            # collision shape
+            if useCollisionShapes:
+                for collision in link.findall('collision'):
+                    f_shape = self.add_shape(collision, f'{link_name}_1', f_link, isVisual=False)
+                    f_shape.setColor([1,0,0,.2])
+
+        # joints
+
+        joints = xmlData.findall('./joint')
+        for joint in joints:
+            joint_name = joint.attrib['name']
+            if joint.find('child') is None:
+                continue
+
+            parent_name = joint.find('parent').attrib['link']
+            f_parent = self.C.getFrame(parent_name)
+            if f_parent is None:
+                print('SHIT', joint_name)
+                break
+
+            # add an origin frame as pre frame?
+            elem = joint.find('origin')
+            if elem is not None:
+                f_origin = self.C.addFrame(f'{joint_name}_origin')
+                f_origin.setParent(f_parent)
+                self.setRelativePose(f_origin, elem)
+                f_parent = f_origin
+
+            f_joint = self.C.addFrame(joint_name)
+            f_joint.setParent(f_parent)
+
+            child_name = joint.find('child').attrib['link']
+            f_child = self.C.getFrame(child_name)
+            if f_child is None:
+                print('SHIT', joint_name)
+                break
+            f_child.setParent(f_joint, False)
+
+            elem = joint.find('limit')
+            limits = []
+            if elem is not None:
+                lo = elem.attrib.get('lower')
+                up = elem.attrib.get('upper')
+                eff = elem.attrib.get('effort')
+                vel = elem.attrib.get('velocity')
+                if eff=='0':
+                    eff=None
+                if vel=='0':
+                    vel=None
+                if lo is not None:
+                    limits = [float(lo), float(up)]
+                # if vel is not None:
+                    # print(' ctrl_limits: [%s -1 %s],' % (vel, eff), end='') #the 2nd value is an acceleration limit
+            # else:
+            #     elem = joint.find('safety_controller')
+            #     if elem is not None:
+            #         lo = elem.attrib.get('soft_lower_limit')
+            #         up = elem.attrib.get('soft_upper_limit')
+            #         if lo is not None:
+            #             print(' limits: [%s %s],' % (lo, up), end='')
+
+            elem = joint.find('mimic')
+            f_mimic = None
+            if elem is not None:
+                f_mimic = self.C.getFrame(elem.attrib['joint'])
+                if f_mimic is None:
+                    print('SHIT', elem.attrib['joint'])
+                    break
+
+            att = joint.attrib.get('type')
+            
+            if att in ['revolute', 'continuous']:
+                elem = joint.find('axis')
+                if elem is not None:
+                    axis = elem.attrib['xyz']
+                    if axis=='1 0 0':
+                        f_joint.setJoint(ry.JT.hingeX, limits, 1., f_mimic)
+                    elif axis=='0 1 0':
+                        f_joint.setJoint(ry.JT.hingeY, limits, 1., f_mimic)
+                    elif axis=='0 0 1':
+                        f_joint.setJoint(ry.JT.hingeZ, limits, 1., f_mimic)
+                    elif axis=='0 0 -1':
+                        f_joint.setJoint(ry.JT.hingeZ, limits, -1., f_mimic)
+                    else:
+                        raise Exception('CAN ONLY PROCESS X Y Z hinge joints, not', axis)
+                else:
+                    f_joint.setJoint(ry.JT.hingeX, limits, 1., f_mimic)
+                    
+            if att == 'prismatic':
+                elem = joint.find('axis')
+                if elem is not None:
+                    axis = elem.attrib['xyz']
+                    if axis=='1 0 0':
+                        f_joint.setJoint(ry.JT.transX, limits, 1., f_mimic)
+                    elif axis=='0 1 0':
+                        f_joint.setJoint(ry.JT.transY, limits, 1., f_mimic)
+                    elif axis=='0 -1 0':
+                        f_joint.setJoint(ry.JT.transY, limits, -1., f_mimic)
+                    elif axis=='0 0 1':
+                        f_joint.setJoint(ry.JT.transZ, limits, 1., f_mimic)
+                    elif axis=='0 0 -1':
+                        f_joint.setJoint(ry.JT.transZ, limits, -1., f_mimic)
+                    else:
+                        raise Exception('CAN ONLY PROCESS X Y Z prismatic joints, not', axis)
+                else:
+                    f_joint.setJoint(ry.JT.transX, limits, 1., f_mimic)
+
+            if att == 'fixed':
+                f_joint.setJoint(ry.JT.rigid)
+
+            #elem = joint.find('axis')
+            #if elem is not None:
+            #    print('axis:[%s]' % elem.attrib['xyz'])
+
+    def add_shape(self, shape, shape_name, f_link, isVisual):
+        f_shape = self.C.addFrame(shape_name)
+        f_shape.setParent(f_link)
+
+        elem = shape.find('origin')
+        if elem is not None:
+            self.setRelativePose(f_shape, elem)
+
+        elem = shape.find('geometry/box')
+        if elem is not None:
+            size = self.as_floats(elem.attrib['size'])
+            f_shape.setShape(ry.ST.box, size)
+
+        elem = shape.find('geometry/sphere')
+        if elem is not None:
+            size = self.as_floats(elem.attrib['size'])
+            f_shape.setShape(ry.ST.sphere, size)
+
+        elem = shape.find('geometry/cylinder')
+        if elem is not None:
+            size = [float(elem.attrib[tag]) for tag in ['length', 'radius']]
+            f_shape.setShape(ry.ST.cylinder, size)
+
+        elem = shape.find('geometry/mesh')
+        if elem is not None:
+            filename = elem.attrib['filename'].replace(self.meshPathRemove,'',1)
+            if self.meshExt is not None:
+                filename = filename[:-3] + self.meshExt
+            if 'scale' in elem.attrib:
+                scale = self.as_floats(elem.attrib['scale'])[0]
+            else:
+                scale = 1
+            filename = os.path.join(self.path, filename)
+            f_shape.setMeshFile(filename, scale)
+
+        elem = shape.find('material/color')
+        if elem is not None:
+            f_shape.setColor(self.as_floats(elem.attrib['rgba']))
+
+        # elem = shape.find('material')
+        # if elem is not None and 'name' in elem.attrib:
+        #     mat = self.materials[elem.attrib['name']]
+        #     if len(mat)==4: #rgba
+        #         f_shape.setColor(mat)
+        #     else:
+        #         f_shape.setTextureFile(mat)
+
+        
+        return f_shape
+
+    def as_floats(self, input_string):
+        return [float(num) for num in input_string.replace(',', ' ').split()]
+    
+    def setRelativePose(self, f, attrib):
+        pos = attrib.get('xyz')
+        if pos=='0 0 0':
+            pos=None
+        if pos is not None:
+            f.setRelativePosition(self.as_floats(pos))
+
+        rpy = attrib.get('rpy')
+        if rpy=='0 0 0':
+            rpy=None
+        if rpy is not None:
+            q = ry.Quaternion()
+            q.setRollPitchYaw(self.as_floats(rpy))
+            f.setRelativeQuaternion(q.getArr())

robotic/src/yaml_helper.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python3
+
+import sys
+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)
+
+if __name__ == "__main__":
+    filename = sys.argv[1]
+    with open(filename, 'r', encoding='utf-8') as fil:
+        data = yaml.safe_load(fil)
+    print(yaml.dump(noflow_dict(data), None, default_flow_style=True, sort_keys=False, width=500))
+        

robotic/version.py

@@ -1 +1 @@
-__version__ = '0.2.8.dev4'
+__version__ = '0.3.4.dev5'

{robotic-0.2.8.dev4.data → robotic-0.3.4.dev5.data}/scripts/ry-h5info

@@ -2,25 +2,20 @@
 
 import argparse
 import h5py
+from robotic.src.h5_helper import *
 
 parser = argparse.ArgumentParser(description='h5-file info')
 
 parser.add_argument('FILE', type=str,
                     help='h5-file name')
 
-def print_attrs(name, obj):
-    if isinstance(obj, h5py.Dataset):
-        print('   ', name, obj.name, obj.shape, obj.dtype)
-    else:
-        print('---', name)
-
 def main():
     args = parser.parse_args()
 
     print('=== file', args.FILE)
     try:
-        with h5py.File(args.FILE, 'r') as fil:
-            fil.visititems(print_attrs)
+        h5 = H5Reader(args.FILE)
+        h5.print_info()
     except KeyboardInterrupt:
         sys.exit(1)
 

{robotic-0.2.8.dev4.data → robotic-0.3.4.dev5.data}/scripts/ry-test

@@ -10,15 +10,16 @@ C.addFile(ry.raiPath('panda/panda.g'), 'b_').setPosition([0,0,.5])
 C.addFrame('box1') .setShape(ry.ST.ssBox,[.3, .3, .3, .05]) .setPosition([1.3, 0, 2.]) .setMass(3)
 C.addFrame('box2') .setShape(ry.ST.ssBox,[.3, .3, .3, .05]) .setPosition([.5, .0, 1.8]) .setMass(3)
 
-#C.view(False)
+C.view(False)
+time.sleep(.5)
 
 q0 = C.getJointState()
 qT = q0
 qT[0] += 1.
 
-sim = ry.Simulation(C, ry.SimulationEngine.physx, verbose=1) #try verbose=2
+sim = ry.Simulation(C, ry.SimulationEngine.physx, verbose=2) #try verbose=3 or 1
 
-[X, q, V, qDot] = sim.getState()
+state = sim.getState()
 
 tau=.01
 
@@ -33,7 +34,7 @@ for t in range(int(4./tau)):
     #    C.view(False, f'Note: the sim operates *directly* on the given config\nt:{t:4d} = {tau*t:5.2f}sec')
 
     if (t%100)==0:
-        sim.setState(X, q, V, qDot)
-        q0 = q
+        sim.setState(*state)
+        #q0 = q
         sim.resetSplineRef()
         sim.setSplineRef(qT, [1.])

robotic-0.3.4.dev5.data/scripts/ry-urdfConvert.py

@@ -0,0 +1,74 @@
+#!python
+
+# from config_mujoco import *
+# from config_urdf import *
+# from mesh_helper import *
+from robotic.src.yaml_helper import *
+import robotic as ry
+import argparse
+import glob
+import os
+
+parser = argparse.ArgumentParser(
+    description='Utility to clean meshes in meshes/')
+
+parser.add_argument('file', type=str, help='urdf file', nargs='?', default='none')
+
+parser.add_argument('-view', help='view mesh', action="store_true", default=True)
+parser.add_argument('-flipDaeYZ', help='view mesh', action="store_true", default=False)
+parser.add_argument('-pruneRigidJoints', help='view mesh', action="store_true")
+parser.add_argument('-recomputeInertias', help='view mesh', action="store_true")
+parser.add_argument('-processMeshes', help='view mesh', action="store_true", default=True)
+parser.add_argument('-meshlab', help='apply meshlab filters', action="store_true", default=False)
+
+def main():
+    args = parser.parse_args()
+
+    if args.file=='none':
+        # args.file = '/home/mtoussai/git/rai-robotModels/pr2/pr2.urdf'
+        args.file = '/home/mtoussai/git/rai-robotModels/panda/panda_arm_hand.urdf'
+        # args.file = '/home/mtoussai/git/rai-robotModels/g1/g1_description/g1_29dof.urdf'
+        # args.file = '/home/mtoussai/git/rai-robotModels/ranger/ranger_mini.urdf'
+
+    print('=== URDF CONVERT ===', args.file)
+
+    path, file = os.path.split(args.file)
+    filebase, _ = os.path.splitext(file)
+
+    if args.flipDaeYZ:
+        ry.params_add({'assimp/daeFlipYZ': False})
+
+    C = ry.URDFLoader(args.file, visualsOnly=True, meshPathRemove='package://').C
+
+    C.processStructure(args.pruneRigidJoints, True, False, False)
+    C.processInertias(args.recomputeInertias)
+    C.processStructure(args.pruneRigidJoints, True, False, False)
+
+    os.system('rm -Rf meshes/')
+    C.writeMeshes('meshes/', copyTextures=True)
+
+    print('#frames: ', C.getFrameDimension())
+    with open(f'{filebase}_conv.g', 'w') as fil:
+        #yaml.dump(C.asDict(), file, default_flow_style=False)
+        fil.write(C.write())
+
+    yaml_write_dict(C.asDict(), f'{filebase}_conv.yml')
+
+    C.view(True)
+    # C.animate()
+
+    if args.processMeshes:
+        for file in sorted(glob.glob('meshes/*.h5')):
+
+            M = ry.MeshHelper(file)
+            if M.mesh is None:
+                continue
+
+            M.repair(mergeTolerance=1e-4)
+            print('  watertight:', M.mesh.is_watertight)
+            print('  oriented:', M.mesh.is_winding_consistent)
+            M.report()
+            M.export_h5()
+
+if __name__ == "__main__":
+    main()