update parsers and mujoco support

pyrobolearn/simulators/__init__.py

@@ -14,6 +14,9 @@ from .bullet_ros import BulletROS
 # dart simulator
 # from .dart import Dart
 
+# MuJoCo simulator
+# from .mujoco import Mujoco
+
 # # PyBullet simulator
 # import pybullet
 # import pybullet_data

pyrobolearn/simulators/simulator.py

@@ -111,7 +111,8 @@ class Simulator(object):
     GEOM_MESH = 5
     GEOM_PLANE = 6
     GEOM_CAPSULE = 7
-    GEOM_CONE = 8       # NEW
+    GEOM_CONE = 8        # NEW
+    GEOM_ELLIPSOID = 9   # NEW
 
     GUI = 1
     GUI_MAIN_THREAD = 8
@@ -319,6 +320,16 @@ class Simulator(object):
         """Return True if we can use URDFs."""
         return False
 
+    @staticmethod
+    def supports_light():
+        """Return True if we can define and access to the lights in the simulator."""
+        return False
+
+    @staticmethod
+    def can_load_heightmap():
+        """Return True if the simulator can load a heightmap."""
+        return False
+
     ###########
     # Methods #
     ###########

pyrobolearn/utils/__init__.py

@@ -28,6 +28,9 @@ from . import feedback
 # import real-time plotting
 from . import plotting
 
+# import parsers
+# from . import parsers
+
 
 # Built-in functions
 

pyrobolearn/utils/parsers/robots/README.rst

@@ -0,0 +1,5 @@
+Robot parsers
+=============
+
+This folder provides parsers to different robot files (URDF, MJCF, Proto, etc), and allows to convert from one format
+to another.

pyrobolearn/utils/parsers/robots/__init__.py

@@ -0,0 +1,9 @@
+
+# import robot parser
+from .robot_parser import RobotParser
+
+# import urdf parser
+from .urdf_parser import URDFParser
+
+# import mujoco parser
+from .mujoco_parser import MuJoCoParser

pyrobolearn/utils/parsers/robots/converter.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Define the Converter class which allows to convert from one type of file (urdf, sdf, mjcf, and others) to another
+format.
+"""
+
+from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class Converter(object):
+    r"""Converter"""
+
+    def __init__(self):
+        pass
+
+    def convert(self, from_filename, to_filename):
+        pass

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -0,0 +1,534 @@
+#!/usr/bin/env python
+"""Provide the data structures that are shared among the various parsers and converter.
+"""
+
+import numpy as np
+from collections import OrderedDict
+
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class World(object):
+    r"""World data structure."""
+
+    def __init__(self, trees=None):
+        self.trees = trees
+
+
+class Tree(object):
+    r"""Tree data structure."""
+
+    def __init__(self, name=None, root=None):
+        self.name = name
+        self.root = root
+        self.bodies = OrderedDict()
+        self.joints = OrderedDict()
+        self.materials = {}
+
+
+class Body(object):
+    r"""Body / Link data structure."""
+
+    def __init__(self, body_id, name=None):
+        self.id = body_id
+        self.name = name
+
+        # inner bodies / links
+        self.bodies = []
+        self.joints = OrderedDict()
+
+        self.inertial = None
+        self.visual = None
+        self.collision = None
+
+
+class Joint(object):
+    r"""Joint data structure.
+
+    Joint types: fixed, revolute/hinge, continuous
+    """
+
+    def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None,
+                 position=None, orientation=None, friction=None, damping=None, effort=None, velocity=None):
+        self.id = joint_id
+        self.name = name
+        self.dtype = dtype
+        self.limits = limits
+        self.parent = parent
+        self.child = child
+        self.axis = axis
+        self.position = position
+        self.orientation = orientation
+        self.friction = friction
+        self.damping = damping
+        self.effort = effort
+        self.velocity = velocity
+
+    @property
+    def limits(self):
+        return self._limits
+
+    @limits.setter
+    def limits(self, limits):
+        if limits is not None:
+            if isinstance(limits, str):
+                limits = [lim for lim in limits.split()]
+            limits = tuple(limits)
+            limits = [float(lim) for lim in limits]
+            if len(limits) != 2:
+                raise ValueError("Expecting 2 floats for the joint limits")
+        self._limits = limits
+
+    @property
+    def axis(self):
+        return self._axis
+
+    @axis.setter
+    def axis(self, axis):
+        if axis is not None:
+            if isinstance(axis, str):
+                axis = [float(ax) for ax in axis.split()]
+            axis = tuple(axis)
+            if len(axis) != 3:
+                raise ValueError("Expecting the joint axis to be defined using 3 floats (x,y,z)")
+        self._axis = axis
+
+    @property
+    def position(self):
+        return self._position
+
+    @position.setter
+    def position(self, position):
+        if position is not None:
+            if isinstance(position, str):
+                position = [float(p) for p in position.split()]
+            position = np.asarray(position)
+        self._position = position
+
+    @property
+    def orientation(self):
+        return self._orientation
+
+    @orientation.setter
+    def orientation(self, orientation):
+        if orientation is not None:
+            if isinstance(orientation, str):
+                orientation = [float(o) for o in orientation.split()]
+            if len(orientation) == 4:  # quaternion
+                orientation = get_rpy_from_quaternion(orientation)
+            if len(orientation) == 3:  # rpy
+                pass
+            orientation = np.asarray(orientation)
+        self._orientation = orientation
+
+    @property
+    def friction(self):
+        return self._friction
+
+    @friction.setter
+    def friction(self, friction):
+        if friction is not None:
+            friction = float(friction)
+        self._friction = friction
+
+    @property
+    def damping(self):
+        return self._damping
+
+    @damping.setter
+    def damping(self, damping):
+        if damping is not None:
+            damping = float(damping)
+        self._damping = damping
+
+    @property
+    def effort(self):
+        return self._effort
+
+    @effort.setter
+    def effort(self, effort):
+        if effort is not None:
+            effort = float(effort)
+        self._effort = effort
+
+    @property
+    def velocity(self):
+        return self._velocity
+
+    @velocity.setter
+    def velocity(self, velocity):
+        if velocity is not None:
+            velocity = float(velocity)
+        self._velocity = velocity
+
+
+class Inertia(object):
+    r"""Inertia data structure"""
+
+    def __init__(self, ixx=1., iyy=1., izz=1., ixy=0., ixz=0., iyz=0.):
+        self.ixx = ixx
+        self.iyy = iyy
+        self.izz = izz
+        self.ixy = ixy
+        self.ixz = ixz
+        self.iyz = iyz
+
+    @property
+    def diagonal_inertia(self):
+        return np.array([self.ixx, self.iyy, self.izz])
+
+    @property
+    def full_inertia(self):
+        return np.array([[self.ixx, self.ixy, self.ixz],
+                         [self.ixy, self.iyy, self.iyz],
+                         [self.ixz, self.iyz, self.izz]])
+
+    @property
+    def ixx(self):
+        return self._ixx
+
+    @ixx.setter
+    def ixx(self, ixx):
+        if ixx is not None:
+            ixx = float(ixx)
+        self._ixx = ixx
+
+    @property
+    def iyy(self):
+        return self._iyy
+
+    @iyy.setter
+    def iyy(self, iyy):
+        if iyy is not None:
+            iyy = float(iyy)
+        self._iyy = iyy
+
+    @property
+    def izz(self):
+        return self._izz
+
+    @izz.setter
+    def izz(self, izz):
+        if izz is not None:
+            izz = float(izz)
+        self._izz = izz
+
+    @property
+    def ixy(self):
+        return self._ixy
+
+    @ixy.setter
+    def ixy(self, ixy):
+        if ixy is not None:
+            ixy = float(ixy)
+        self._ixy = ixy
+
+    @property
+    def ixz(self):
+        return self._ixz
+
+    @ixz.setter
+    def ixz(self, ixz):
+        if ixz is not None:
+            ixz = float(ixz)
+        self._ixz = ixz
+
+    @property
+    def iyz(self):
+        return self._iyz
+
+    @iyz.setter
+    def iyz(self, iyz):
+        if iyz is not None:
+            iyz = float(iyz)
+        self._iyz = iyz
+
+
+class Inertial(object):
+    r"""Inertial parameters."""
+
+    def __init__(self, mass=None, inertia=None, position=None, orientation=None):
+        self.mass = mass
+        self.inertia = inertia
+        self.position = position
+        self.orientation = orientation
+
+    @property
+    def mass(self):
+        return self._mass
+
+    @mass.setter
+    def mass(self, mass):
+        if mass is not None:
+            mass = float(mass)
+        self._mass = mass
+
+    @property
+    def inertia(self):
+        return self._inertia
+
+    @inertia.setter
+    def inertia(self, inertia):
+        if inertia is not None:
+            if isinstance(inertia, str):
+                inertia = inertia.split()
+            if isinstance(inertia, (list, tuple, np.ndarray)):
+                if isinstance(inertia, np.ndarray) and inertia.ndim == 2:
+                    if inertia.shape != (3, 3):
+                        raise ValueError("Expecting a 3x3 inertia matrix")
+                    inertia = Inertia(ixx=inertia[0, 0], ixy=inertia[0, 1], ixz=inertia[0, 2],
+                                      iyy=inertia[1, 1], iyz=inertia[1, 2], izz=inertia[2, 2])
+                else:
+                    if len(inertia) == 3:
+                        inertia = Inertia(ixx=inertia[0], iyy=inertia[1], izz=inertia[2])
+                    elif len(inertia) == 6:
+                        inertia = Inertia(ixx=inertia[0], iyy=inertia[1], izz=inertia[2], ixy=inertia[3],
+                                          ixz=inertia[4], iyz=inertia[5])
+                    elif len(inertia) == 9:
+                        inertia = Inertia(ixx=inertia[0], ixy=inertia[1], ixz=inertia[2], iyy=inertia[4],
+                                          iyz=inertia[5], izz=inertia[8])
+            elif isinstance(inertia, dict):
+                inertia = Inertia(**inertia)
+        self._inertia = inertia
+
+    @property
+    def aligned_inertia(self):
+        raise NotImplementedError
+
+    @property
+    def position(self):
+        return self._position
+
+    @position.setter
+    def position(self, position):
+        if position is not None:
+            if isinstance(position, str):
+                position = [float(p) for p in position.split()]
+            position = np.asarray(position)
+        self._position = position
+
+    @property
+    def orientation(self):
+        return self._orientation
+
+    @orientation.setter
+    def orientation(self, orientation):
+        if orientation is not None:
+            if isinstance(orientation, str):
+                orientation = [float(o) for o in orientation.split()]
+            if len(orientation) == 4:  # quaternion
+                orientation = get_rpy_from_quaternion(orientation)
+            if len(orientation) == 3:  # rpy
+                pass
+            orientation = np.asarray(orientation)
+        self._orientation = orientation
+
+    @property
+    def rpy(self):
+        return self._orientation
+
+    @property
+    def quaternion(self):
+        return get_quaternion_from_rpy(self._orientation)
+
+
+class Visual(object):
+    r"""visual parameters for body."""
+
+    def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None,
+                 material=None):
+        self.name = name
+        self.dtype = dtype
+        self.size = size     # depending on the type it can be different size
+        self.color = color
+        self.filename = filename
+        self.position = position
+        self.orientation = orientation
+        self.material = material
+
+    @property
+    def size(self):
+        return self._size
+
+    @size.setter
+    def size(self, size):
+        if size is not None:
+            if isinstance(size, str):
+                size = [float(s) for s in size.split()]
+            elif isinstance(size, (tuple, list, np.ndarray)):
+                size = [float(s) for s in size]
+            elif not isinstance(size, (float, int)):
+                raise TypeError("Expecting the size to be a float, int, list, tuple or np.ndarray")
+        self._size = size
+
+    @property
+    def color(self):
+        return self._color
+
+    @color.setter
+    def color(self, color):
+        if color is not None:
+            if isinstance(color, str):  # e.g. '0.5 0.1 1. 1.'
+                color = (float(c) for c in color.split())
+            if not isinstance(color, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                color = tuple(color)
+        self._color = color
+
+    @property
+    def format(self):
+        """Return the filename format extension for the mesh."""
+        if self.filename is not None:
+            return self.filename.split('.')[-1]
+
+    @property
+    def position(self):
+        return self._position
+
+    @position.setter
+    def position(self, position):
+        if position is not None:
+            if isinstance(position, str):
+                position = [float(p) for p in position.split()]
+            position = np.asarray(position)
+        self._position = position
+
+    @property
+    def orientation(self):
+        return self._orientation
+
+    @orientation.setter
+    def orientation(self, orientation):
+        if orientation is not None:
+            if isinstance(orientation, str):
+                orientation = [float(o) for o in orientation.split()]
+            if len(orientation) == 4:  # quaternion
+                orientation = get_rpy_from_quaternion(orientation)
+            if len(orientation) == 3:  # rpy
+                pass
+            orientation = np.asarray(orientation)
+        self._orientation = orientation
+
+    @property
+    def rpy(self):
+        return self._orientation
+
+    @property
+    def quaternion(self):
+        return get_quaternion_from_rpy(self._orientation)
+
+
+class Collision(object):
+    r"""Collision parameters for body."""
+
+    def __init__(self, name=None, dtype=None, size=None, filename=None, position=None, orientation=None):
+        self.name = name
+        self.dtype = dtype
+        self.size = size
+        self.filename = filename
+        self.position = position
+        self.orientation = orientation
+
+    @property
+    def size(self):
+        return self._size
+
+    @size.setter
+    def size(self, size):
+        if size is not None:
+            if isinstance(size, str):
+                size = [float(s) for s in size.split()]
+            elif isinstance(size, (tuple, list, np.ndarray)):
+                size = [float(s) for s in size]
+            elif not isinstance(size, (float, int)):
+                raise TypeError("Expecting the size to be a float, int, list, tuple or np.ndarray")
+        self._size = size
+
+    @property
+    def format(self):
+        if self.filename is not None:
+            return self.filename.split('.')[-1]
+
+    @property
+    def position(self):
+        return self._position
+
+    @position.setter
+    def position(self, position):
+        if position is not None:
+            if isinstance(position, str):
+                position = [float(p) for p in position.split()]
+            position = np.asarray(position)
+        self._position = position
+
+    @property
+    def orientation(self):
+        return self._orientation
+
+    @orientation.setter
+    def orientation(self, orientation):
+        if orientation is not None:
+            if isinstance(orientation, str):
+                orientation = [float(o) for o in orientation.split()]
+            if len(orientation) == 4:  # quaternion
+                orientation = get_rpy_from_quaternion(orientation)
+            if len(orientation) == 3:  # rpy
+                pass
+            orientation = np.asarray(orientation)
+        self._orientation = orientation
+
+    @property
+    def rpy(self):
+        return self._orientation
+
+    @property
+    def quaternion(self):
+        return get_quaternion_from_rpy(self._orientation)
+
+
+class Material(object):
+    r"""Material info."""
+
+    def __init__(self, name=None, color=None):
+        self.name = name
+        self.color = color
+
+    @property
+    def color(self):
+        return self._color
+
+    @color.setter
+    def color(self, color):
+        if color is not None:
+            if isinstance(color, str):  # e.g. '0.5 0.1 1. 1.'
+                color = (float(c) for c in color.split())
+            if not isinstance(color, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                color = tuple(color)
+        self._color = color
+
+    @property
+    def rgb(self):
+        if self.color is None:
+            return 0.5, 0.5, 0.5
+        return tuple(self.color[:3])
+
+    @property
+    def rgba(self):
+        if self.color is None:
+            return 0.5, 0.5, 0.5, 1.
+        if len(self.color) == 3:
+            return tuple(self.color) + (1.,)
+        return tuple(self.color)

pyrobolearn/utils/parsers/robots/mujoco_parser.py

@@ -0,0 +1,74 @@
+#!/usr/bin/env python
+"""Define the MuJoCo parser.
+"""
+
+# import XML parser
+import xml.etree.ElementTree as ET
+from xml.dom import minidom  # to print in a pretty way the XML file
+
+from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser
+from pyrobolearn.utils.parsers.robots.data_structures import Tree
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class MuJoCoParser(RobotParser):
+    r"""MuJoCo Parser"""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the MuJoCo parser.
+
+        Args:
+            filename (str, None): path to the MuJoCo XML file.
+        """
+        super().__init__(filename)
+
+    def parse(self, filename):
+        """
+        Load and parse the given MuJoCo XML file.
+
+        Args:
+            filename (str): path to the MuJoCo XML file.
+        """
+        # load and parse the XML file
+        tree = ET.parse(filename)
+
+        # get the root
+        root = tree.getroot()
+
+        # check that the root is <mujoco>
+        if root.tag != 'mujoco':
+            raise RuntimeError("Expecting the first XML tag to be 'mujoco' but found instead: {}".format(root.tag))
+
+        # build the tree
+
+    def get_tree(self):
+        """
+        Return the Tree containing all the elements.
+
+        Returns:
+            Tree: tree data structure.
+        """
+        pass
+
+    def generate(self, tree=None):
+        """
+        Generate the XML tree from the `Tree` data structure.
+
+        Args:
+            tree (Tree): Tree data structure.
+
+        Returns:
+            ET.Element: root element in the XML file.
+        """
+        pass
+

pyrobolearn/utils/parsers/robots/proto_parser.py

@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+"""Define the Proto parser.
+
+Proto files are notably used in Webots.
+"""
+
+from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser
+from pyrobolearn.utils.parsers.robots.data_structures import Tree
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ProtoParser(RobotParser):
+    r"""Proto Parser"""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the Proto parser.
+
+        Args:
+            filename (str, None): path to the MuJoCo XML file.
+        """
+        super().__init__(filename)
+
+    def parse(self, filename):
+        """
+        Load and parse the given URDF file.
+
+        Args:
+            filename (str): path to the MuJoCo XML file.
+        """
+        pass
+
+    def get_tree(self):
+        """
+        Return the Tree containing all the elements.
+
+        Returns:
+            Tree: tree data structure.
+        """
+        pass
+
+    def generate(self, tree=None):
+        """
+        Generate the XML tree from the `Tree` data structure.
+
+        Args:
+            tree (Tree): Tree data structure.
+
+        Returns:
+            ET.Element: root element in the XML file.
+        """
+        pass

pyrobolearn/utils/parsers/robots/robot_parser.py

@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+"""Define the abstract Robot parser.
+"""
+
+# import XML parser
+import xml.etree.ElementTree as ET
+from xml.dom import minidom  # to print in a pretty way the XML file
+
+from pyrobolearn.utils.parsers.robots.data_structures import Tree
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class RobotParser(object):
+    r"""Robot Parser"""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the robot parser.
+
+        Args:
+            filename (str, None): path to the MuJoCo XML file.
+        """
+        self.root = None
+        self.tree = None
+        self.filename = filename
+        if filename is not None:
+            self.parse(filename)
+
+    def parse(self, filename):
+        """
+        Load and parse a given MuJoCo XML filename.
+
+        Args:
+            filename (str): path to the MuJoCo XML file.
+        """
+        pass
+
+    def get_tree(self):
+        """
+        Return the Tree containing all the elements.
+
+        Returns:
+            Tree: tree data structure.
+        """
+        return self.tree
+
+    def generate(self, tree=None):
+        """
+        Generate the XML tree from the `Tree` data structure.
+
+        Args:
+            tree (Tree): Tree data structure.
+
+        Returns:
+            ET.Element: root element in the XML file.
+        """
+        pass
+
+    def get_string(self, root=None):
+        """
+        Return the XML string from the root element.
+
+        Args:
+            root (ET.Element): root element in the XML file.
+
+        Returns:
+            str: string representing the XML file.
+        """
+        if root is None:
+            root = self.root
+        if not isinstance(root, ET.Element):
+            raise ValueError("Expecting the root to be an instance of `ET.Element`, but got instead: "
+                             "{}".format(type(root)))
+        return minidom.parseString(ET.tostring(root)).toprettyxml(indent="   ")
+
+    def write(self, filename, root=None):
+        """
+        Write the XML tree in the specified XML file.
+
+        Args:
+            filename (str): path to the file to write the XML in.
+            root (ET.Element): root element in the XML file.
+        """
+        xml_str = self.get_string(root)
+        with open(filename, "w") as f:
+            f.write(xml_str)  # .encode('utf-8'))

pyrobolearn/utils/parsers/robots/sdf_parser.py

@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+"""Define the SDF parser.
+
+SDF files are notably used in Gazebo, and Bullet.
+"""
+
+# import XML parser
+import xml.etree.ElementTree as ET
+
+from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser
+from pyrobolearn.utils.parsers.robots.data_structures import Tree
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class SDFParser(RobotParser):
+    r"""SDF Parser"""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the SDF parser.
+
+        Args:
+            filename (str, None): path to the MuJoCo XML file.
+        """
+        super().__init__(filename)
+
+    def parse(self, filename):
+        """
+        Load and parse the given URDF file.
+
+        Args:
+            filename (str): path to the MuJoCo XML file.
+        """
+        # load and parse the XML file
+        tree = ET.parse(filename)
+
+        # get the root
+        root = tree.getroot()
+
+        # check that the root is <robot>
+        if root.tag != 'sdf':
+            raise RuntimeError("Expecting the first XML tag to be 'sdf' but found instead: {}".format(root.tag))
+
+        # build the tree
+
+    def get_tree(self):
+        """
+        Return the Tree containing all the elements.
+
+        Returns:
+            Tree: tree data structure.
+        """
+        pass
+
+    def get_world(self):
+        """
+        Return the world (which is basically a list of Tree).
+        """
+        pass
+
+    def generate(self, tree=None):
+        """
+        Generate the XML tree from the `Tree` data structure.
+
+        Args:
+            tree (Tree): Tree data structure.
+
+        Returns:
+            ET.Element: root element in the XML file.
+        """
+        pass
+

pyrobolearn/utils/parsers/robots/urdf_parser.py

@@ -0,0 +1,241 @@
+#!/usr/bin/env python
+"""Define the URDF parser.
+
+URDF files are notably used in ROS, Gazebo, Bullet, Dart, and MuJoCo.
+"""
+
+# import XML parser
+import xml.etree.ElementTree as ET
+
+from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser
+from pyrobolearn.utils.parsers.robots.data_structures import *
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class URDFParser(RobotParser):
+    r"""URDF Parser"""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the URDF parser.
+
+        Args:
+            filename (str, None): path to the MuJoCo XML file.
+        """
+        super().__init__(filename)
+
+    def parse(self, filename):
+        """
+        Load and parse the given URDF file.
+
+        Args:
+            filename (str): path to the MuJoCo XML file.
+        """
+        # load and parse the XML file
+        tree_xml = ET.parse(filename)
+
+        # get the root
+        root = tree_xml.getroot()
+
+        # check that the root is <robot>
+        if root.tag != 'robot':
+            raise RuntimeError("Expecting the first XML tag to be 'robot' but found instead: {}".format(root.tag))
+
+        # build the tree
+        tree = Tree(name=root.attrib.get('name'))
+
+        # check materials
+        for i, material in enumerate(root.findall('material')):
+            attrib = material.attrib
+            mat = Material(name=attrib.get('name', 'material_' + str(i)), color=attrib.get('color'))
+            tree.materials[mat.name] = mat
+
+        # check bodies / links
+        for i, body in enumerate(root.findall('link')):
+            attrib = body.attrib
+            b = Body(body_id=i, name=attrib.get('name', 'body_' + str(i)))
+
+            # check <inertial> tag
+            inertial = body.find('inertial')
+            if inertial is not None:
+                i = Inertial()
+
+                # origin
+                origin = inertial.find('origin')
+                if origin is not None:
+                    i.position = origin.attrib.get('xyz')
+                    i.orientation = origin.attrib.get('rpy')
+
+                # mass
+                mass = inertial.find('mass')
+                if mass is not None:
+                    i.mass = mass.attrib.get('value')
+
+                # inertia
+                inertia = inertial.find('inertia')
+                if inertia is not None:
+                    i.inertia = {name: inertia.attrib.get(name) for name in ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']}
+
+                # set inertial to body
+                b.inertial = i
+
+            # check <visual> tag
+            visual = body.find('visual')
+            if visual is not None:
+                v = Visual()
+
+                # name
+                v.name = visual.attrib.get('name')
+
+                # origin
+                origin = visual.find('origin')
+                if origin is not None:
+                    v.position = origin.attrib.get('xyz')
+                    v.orientation = origin.attrib.get('rpy')
+
+                # geometry
+                geometry = visual.find('geometry')
+                if geometry is not None:
+                    for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+                        geom = geometry.find(geometry_type)
+                        if geom is not None:
+                            dtype = geometry_type
+                            v.dtype = dtype
+                            if dtype == 'box':
+                                v.size = geom.attrib['size']
+                            elif dtype == 'sphere':
+                                v.size = geom.attrib['radius']
+                            elif dtype == 'cylinder':
+                                v.size = (geom.attrib['radius'], geom.attrib['length'])
+                            elif dtype == 'mesh':
+                                v.filename = geom.attrib['filename']
+                                v.size = geom.attrib.get('scale')
+
+                # material
+                material = visual.find('material')
+                if material is not None:
+                    name = material.attrib.get('name')
+                    color = material.find('color')
+                    if color is not None:
+                        v.color = color.attrib['rgba']
+                    else:
+                        mat = tree.materials.get(name)
+                        if mat is not None:
+                            v.material = mat
+
+                # set visual to body
+                b.visual = v
+
+            # check <collision> tag
+            collision = body.find('collision')
+            if collision is not None:
+                c = Collision()
+
+                # name
+                c.name = collision.attrib.get('name')
+
+                # origin
+                origin = collision.find('origin')
+                if origin is not None:
+                    c.position = origin.attrib.get('xyz')
+                    c.orientation = origin.attrib.get('rpy')
+
+                # geometry
+                geometry = collision.find('geometry')
+                if geometry is not None:
+                    for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+                        geom = geometry.find(geometry_type)
+                        if geom is not None:
+                            dtype = geometry_type
+                            c.dtype = dtype
+                            if dtype == 'box':
+                                c.size = geom.attrib['size']
+                            elif dtype == 'sphere':
+                                c.size = geom.attrib['radius']
+                            elif dtype == 'cylinder':
+                                c.size = (geom.attrib['radius'], geom.attrib['length'])
+                            elif dtype == 'mesh':
+                                c.filename = geom.attrib['filename']
+                                c.size = geom.attrib.get('scale')
+
+                # set collision to body
+                b.collision = c
+
+            # add body to tree
+            tree.bodies[b.name] = b
+
+        # check joints
+        for i, joint in enumerate(root.findall('joint')):
+            attrib = joint.attrib
+            j = Joint(joint_id=i, name=attrib.get('name', 'joint_' + str(i)), dtype=attrib['type'])
+
+            # add parent and child body/link
+            parent = joint.find('parent')
+            if parent is None:
+                raise RuntimeError("Expecting the joint '" + j.name + "' to have a parent link/body")
+            j.parent = parent.attrib['link']
+
+            child = joint.find('child')
+            if child is None:
+                raise RuntimeError("Expecting the joint '" + j.name + "' to have a child link/body")
+            j.child = child.attrib['link']
+
+            # origin
+            origin = joint.find('origin')
+            if origin is not None:
+                j.position = origin.attrib.get('xyz')
+                j.orientation = origin.attrib.get('rpy')
+
+            # axis
+            axis = joint.find('axis')
+            if axis is not None:
+                j.axis = axis.attrib.get('xyz')
+
+            # dynamics
+            dynamics = joint.find('dynamics')
+            if dynamics is not None:
+                j.damping = dynamics.attrib.get('damping')
+                j.friction = dynamics.attrib.get('friction')
+
+            # limits
+            limits = joint.find('limits')
+            if limits is not None:
+                j.effort = limits.attrib.get('effort')
+                j.velocity = limits.attrib.get('velocity')
+                lower_limit = limits.attrib.get('lower')
+                upper_limit = limits.attrib.get('upper')
+                if lower_limit is not None and upper_limit is not None:  # TODO: check if we can have one limit
+                    j.limits = [lower_limit, upper_limit]
+
+            # add joint in trees
+            tree.joints[j.name] = j
+
+            # add joint in parent body
+            tree.bodies[j.parent] = j
+
+        # set the tree
+        self.tree = tree
+
+    def generate(self, tree=None):
+        """
+        Generate the XML tree from the `Tree` data structure.
+
+        Args:
+            tree (Tree): Tree data structure.
+
+        Returns:
+            ET.Element: root element in the XML file.
+        """
+        if tree is None:
+            tree = self.tree
+
+        pass