robotic 0.2.9__cp39-cp39-manylinux2014_x86_64.whl → 0.2.9.dev2__cp39-cp39-manylinux2014_x86_64.whl

robotic/src/config_mujoco.py

@@ -0,0 +1,237 @@
+
+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):
+        self.visualsOnly = visualsOnly
+        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.setQuaternion([0,0,0,1])
+        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 = ('contype' in geom.attrib and geom.attrib.get('contype', '')!='0') or 'col' 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
+                    f_shape.setShape(ry.ST.box, [2.*f for f in 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
+                            [1, 0],  # vertex 1
+                            [1, 1],  # vertex 2
+                            [0, 1],  # vertex 3
+                            [0, 0],  # vertex 4
+                            [1, 0],  # vertex 5
+                            [1, 1],  # vertex 6
+                            [0, 1]   # vertex 7
+                            ])
+                            
+                            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/config_urdf.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())