update parsers/generators

pyrobolearn/simulators/mujoco.py

@@ -62,7 +62,8 @@ except ImportError as e:
 
 # import pyrobolearn related functionalities
 from pyrobolearn.simulators.simulator import Simulator
-from pyrobolearn.utils.parsers.robots import mujoco_parser, urdf_parser, sdf_parser, converter
+# from pyrobolearn.utils.parsers.robots import mujoco_parser, urdf_parser, sdf_parser
+from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
 
 
 # check Python version
@@ -237,8 +238,7 @@ class Mujoco(Simulator):
         self._root = ET.Element("mujoco")
 
         # create worldbody
-        ET.SubElement(self._root, 'worldbody')
-        self._worldbody = self._root.find('worldbody')
+        self._worldbody = ET.SubElement(self._root, 'worldbody')
 
         # add a light
         ET.SubElement(self._worldbody, "light", attrib={"diffuse": ".5 .5 .5", "pos": "0 0 3", "dir": "0 0 -1"})
@@ -572,8 +572,15 @@ class Mujoco(Simulator):
         Returns:
             int (non-negative): unique id associated to the load model.
         """
-        # create xml file based on URDF file
-        pass
+        # parse URDF file
+        urdf_parser = URDFParser(filename=filename)
+        mujoco_generator = MuJoCoParser()
+
+        # generate XML element
+        element = mujoco_generator.generate(urdf_parser.tree)
+
+        # append element to worldbody
+        self._worldbody.append(element)
 
     def load_sdf(self, filename, scaling=1., *args, **kwargs):
         """Load a SDF file in the simulator.
@@ -585,16 +592,24 @@ class Mujoco(Simulator):
         Returns:
             list(int): list of object unique id for each object loaded
         """
-        # parse sdf
-        tree = ET.parse(filename)
-        root = tree.getroot()
+        # parse sdf file
+        sdf_parser = SDFParser(filename=filename)
+        mujoco_generator = MuJoCoParser()
 
-        # add bodies
-        pass
+        # generate XML elements
+        elements = [mujoco_generator.generate(tree) for tree in sdf_parser.world.trees]
+
+        # append each element to worldbody
+        for element in elements:
+            self._worldbody.append(element)
 
     def load_mjcf(self, filename, scaling=1., *args, **kwargs):
         """Load a Mujoco file in the simulator.
 
+        Warnings: this only loads the bodies, joints, and assets. It does not load other elements such as the physical
+        engine properties (number of iterations, solver, etc), physical properties (gravity, friction, viscosity, etc),
+        and others.
+
         Args:
             filename (str): a relative or absolute path to the MJCF file on the file system of the physics server.
             scaling (float): scale factor for the object
@@ -602,11 +617,19 @@ class Mujoco(Simulator):
         Returns:
             list(int): list of object unique id for each object loaded
         """
-        # update the world
+        # load MJCF  # TODO: check if empty world
+        # self.model = mujoco.load_model_from_path(filename)
+        # self.sim = mujoco.MjSim(self.model)
 
-        # load MJCF
-        self.model = mujoco.load_model_from_path(filename)
-        self.sim = mujoco.MjSim(self.model)
+        # parse MJCF file
+        parser = MuJoCoParser(filename=filename)
+
+        # generate XML elements
+        elements = [parser.generate(tree) for tree in parser.world.trees]
+
+        # append each element to worldbody
+        for element in elements:
+            self._worldbody.append(element)
 
     def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None,
                   with_collision=True, flags=None, *args, **kwargs):
@@ -628,6 +651,10 @@ class Mujoco(Simulator):
         Returns:
             int: unique id of the mesh in the world
         """
+        # convert file '.obj' to '.stl' as MuJoCo only supports STL formats.
+
+        # try to look for textures and colors in the '.mtl' file
+
         # create collision shape if specified
 
         # create visual shape
@@ -635,6 +662,9 @@ class Mujoco(Simulator):
         # create body
         pass
 
+    def load_soft_body(self, shape=None, filename=None):  # TODO
+        pass
+
     ##########
     # Bodies #
     ##########

pyrobolearn/simulators/mujoco_empty_world.xml

@@ -1,11 +0,0 @@
-
-<mujoco>
-   <worldbody>
-      <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
-<!--      <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>-->
-<!--      <body pos="0 0 1">-->
-<!--         <joint type="free"/>-->
-<!--         <geom type="box" size=".1 .2 .3" rgba="0 .9 0 1"/>-->
-<!--      </body>-->
-   </worldbody>
-</mujoco>

pyrobolearn/utils/parsers/robots/__init__.py

@@ -1,9 +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
+
+# import sdf parser
+from .sdf_parser import SDFParser

pyrobolearn/utils/parsers/robots/converter.py

@@ -3,7 +3,7 @@
 format.
 """
 
-from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser
+from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -16,10 +16,68 @@ __status__ = "Development"
 
 
 class Converter(object):
-    r"""Converter"""
+    r"""Converter
+
+    This converts one world / robot file to another one.
+    """
 
     def __init__(self):
         pass
 
     def convert(self, from_filename, to_filename):
-        pass
+        """
+        Convert one robot/world file to another one. If it is a world file to a robot file, it will create a robot file
+        for each model that were in the world.
+
+        Args:
+            from_filename (str): file to parse (specified with the extension).
+            to_filename (str, list of str): file to generate (specified with the extension). You can also only
+                specified the extension if you wish. If that is the case, the name will be taken from the file that
+                is being parsed.
+        """
+        # check the types
+        if not isinstance(from_filename, str):
+            raise TypeError("Expecting the 'from_filename' to be a str, but got instead: "
+                            "{}".format(type(from_filename)))
+        if not isinstance(to_filename, str):
+            raise TypeError("Expecting the 'to_filename' to be a str, but got instead: "
+                            "{}".format(type(to_filename)))
+
+        # extension for the 'from_filename'
+        from_extension = from_filename.split('.')[-1]
+        if from_extension == 'urdf':  # URDF
+            parser = URDFParser(filename=from_filename)
+        elif from_extension == 'sdf' or from_extension == 'world':  # SDF
+            parser = SDFParser(filename=from_filename)
+        elif from_extension == 'mjcf' or from_extension == 'xml':  # MuJoCo
+            parser = MuJoCoParser(filename=from_filename)
+        elif from_extension == 'proto':
+            # parser = ProtoParser(filename=from_filename)
+            raise NotImplementedError("The proto parser has not been implemented yet")
+        else:
+            raise ValueError("Got the extension '{}' from 'from_filename', however this format is not "
+                             "known".format(type(from_extension)))
+
+        # extension for the 'to_filename'
+        to_extension = to_filename.split('.')
+        if len(to_extension) == 1:
+            to_extension = to_filename
+        else:
+            to_extension = to_extension[-1]
+
+        # generator
+        if to_extension == 'urdf':  # URDF
+            generator = URDFParser()
+        elif to_extension == 'sdf' or to_extension == 'world':  # SDF
+            generator = SDFParser()
+        elif to_extension == 'mjcf' or to_extension == 'xml':  # MuJoCo
+            generator = MuJoCoParser()
+        elif to_extension == 'proto':
+            # generator = ProtoParser()
+            raise NotImplementedError("The proto parser has not been implemented yet")
+        else:
+            raise ValueError("Got the extension '{}' from 'to_filename', however this format is not "
+                             "known".format(type(from_extension)))
+
+        # generate the files
+        # TODO

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -1,11 +1,11 @@
 #!/usr/bin/env python
-"""Provide the data structures that are shared among the various parsers and converter.
+"""Provide the data structures that are shared among the various parsers and converters.
 """
 
 import numpy as np
 from collections import OrderedDict
 
-from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy, get_matrix_from_rpy
 
 
 __author__ = "Brian Delhaisse"
@@ -18,11 +18,132 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class World(object):
-    r"""World data structure."""
+class Simulator(object):
+    r"""Simulator data structure."""
 
-    def __init__(self, trees=None):
-        self.trees = trees
+    def __init__(self, world=None, physics_engine=None):
+        self.world = world
+        self.engine = physics_engine
+        self.physics = physics_properties
+
+    @property
+    def world(self):
+        return self._world
+
+    @world.setter
+    def world(self, world):
+        if world is not None and not isinstance(world, World):
+            raise TypeError("Expecting the world to be an instance of `World`, but got instead: "
+                            "{}".format(type(world)))
+        self._world = world
+
+    @property
+    def engine(self):
+        return self._engine
+
+    @engine.setter
+    def engine(self, engine):
+        if engine is not None and not isinstance(engine, PhysicsEngine):
+            raise TypeError("Expecting the engine to be an instance of `PhysicsEngine`, but got instead: "
+                            "{}".format(type(engine)))
+        self._engine = engine
+
+
+class PhysicsEngine(object):
+    r"""Physics Engine properties.
+
+    This include number of iterations, solver used, tolerance, timesteps, etc.
+    """
+
+    def __init__(self, timestep=None):
+        self.timestep = timestep
+        self.num_iterations = None
+        self.solver = None
+        self.tolerance = None
+
+
+class World(object):
+    r"""World data structure.
+
+    World frame (robotics convention with the right-hand rule):
+    - the x axis points forward
+    - the y axis points to the left
+    - the z axis points upward
+    """
+
+    def __init__(self, name=None):
+        self.name = name
+        self.trees = OrderedDict()
+        self.physics = None
+
+    @property
+    def physics(self):
+        return self._physics
+
+    @physics.setter
+    def physics(self, physics):
+        if physics is not None and not isinstance(physics, Physics):
+            raise TypeError("Expecting the physics to be an instance of `Physics`, but got instead: "
+                            "{}".format(type(physics)))
+        self._physics = physics
+
+
+class Light(object):
+    r"""Light data structure.
+
+    Type of light: point, directional, and spot
+    """
+
+    def __init__(self, name=None, dtype=None, cast_shadows=None, diffuse=None, specular=None, attenuation=None,
+                 direction=None, spot=None, position=None, orientation=None):
+        """
+        Initialize the Light data structure.
+
+        Args:
+            name (str): unique name for the light.
+            dtype (str): type of light, select between {'point', 'directional', 'spot'}
+            cast_shadows (bool): if True, it will cast shadows.
+            diffuse (tuple of 4 float, np.array[4]): diffuse light (RGBA) color.
+            specular (tuple of 4 float, np.array[4]): specular light (RGBA) color.
+            attenuation: light attenuation
+            direction (np.array[3]): direction of the light if dtype='directional' or dtype='spot'.
+            spot: spot light parameters
+            position (tuple/list of 3 float, np.array[3]): position of the light in the world.
+            orientation (tuple/list of 3 float, np.array[3]): orientation of the light in the world.
+        """
+        self.name = name
+        self.dtype =dtype
+        self.shadows = cast_shadows
+        self.diffuse = diffuse
+        self.specular = specular
+        self.attenuation = attenuation
+        self.direction = direction
+        self.spot = spot
+        self.position = position
+        self.orientation = orientation
+
+
+class Physics(object):
+    r"""Physical properties of the world.
+
+    This includes gravity, friction, viscosity, etc.
+    """
+
+    def __init__(self, gravity=(0., 0., -9.81)):
+        self.gravity = gravity
+
+
+class Frame(object):
+    r"""Reference Frame"""
+
+    def __init__(self, position=None, orientation=None, dtype=None, right_handed=True, forward_axis=(1., 0., 0.),
+                 up_axis=(0., 0., 1.)):
+        self.position = position
+        self.orientation = orientation
+        self.dtype = dtype  # world frame, body frame, joint frame, inertial frame, etc.
+        self.right_handed = right_handed
+        self.forward_axis = forward_axis
+        self.up_axis = up_axis
 
 
 class Tree(object):
@@ -34,6 +155,71 @@ class Tree(object):
         self.bodies = OrderedDict()
         self.joints = OrderedDict()
         self.materials = {}
+        self.position = None
+        self.orientation = None
+
+    @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)
+
+    @property
+    def rot(self):
+        return get_matrix_from_rpy(self.rpy)
+
+    @property
+    def pose(self):
+        return self.position, self.orientation
+
+    @pose.setter
+    def pose(self, pose):
+        if pose is not None:
+            if isinstance(pose, str):
+                pose = pose.split()
+                self.position = pose[:3]
+                self.orientation = pose[3:]
+            elif isinstance(pose, (tuple, list, np.ndarray)):
+                if len(pose) == 2:
+                    self.position = pose[0]
+                    self.orientation = pose[1]
+                elif len(pose) == 6:
+                    self.position = pose[:3]
+                    self.orientation = pose[3:]
+                else:
+                    raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+            else:
+                raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
 
 
 class Body(object):
@@ -55,7 +241,20 @@ class Body(object):
 class Joint(object):
     r"""Joint data structure.
 
-    Joint types: fixed, revolute/hinge, continuous
+    Joint types: fixed, floating/free, prismatic, revolute/hinge, continuous, gearbox, revolute2, ball, screw,
+    universal, and planar.
+
+    - fixed: no motions is allowed; both links are rigidly attached to each other.
+    - floating/free: allows motion for all 6 degrees of motion.
+    - prismatic: allows motion along 1 translational DoF.
+    - revolute/hinge: allows rotational motion around one axis (1 DoF).
+    - continuous: a revolute/hinge joint that doesn't have lower or upper limits.
+    - gearbox: geared revolute joint.
+    - revolute2: two revolute joints connected in series
+    - ball: a ball and socket joint which allows rotational motions around the 3 axis (3 DoFs).
+    - screw: a single DoF joint wich coupled sliding and rotational motion
+    - universal: like a ball joint, but constrains one DoF
+    - planar: allows motion in a plane perpendicular to the axis.
     """
 
     def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None,
@@ -131,6 +330,41 @@ class Joint(object):
             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)
+
+    @property
+    def rot(self):
+        return get_matrix_from_rpy(self.rpy)
+
+    @property
+    def pose(self):
+        return self.position, self.orientation
+
+    @pose.setter
+    def pose(self, pose):
+        if pose is not None:
+            if isinstance(pose, str):
+                pose = pose.split()
+                self.position = pose[:3]
+                self.orientation = pose[3:]
+            elif isinstance(pose, (tuple, list, np.ndarray)):
+                if len(pose) == 2:
+                    self.position = pose[0]
+                    self.orientation = pose[1]
+                elif len(pose) == 6:
+                    self.position = pose[:3]
+                    self.orientation = pose[3:]
+                else:
+                    raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+            else:
+                raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
     @property
     def friction(self):
         return self._friction
@@ -183,23 +417,49 @@ class Inertia(object):
         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 diagonal_inertia(self):
+        """Aligned inertia.
+
+        Returns:
+            np.array[3]: principal moments of the inertia.
+
+        References:
+            - [1] https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_matrix_in_different_reference_frames
+        """
+        inertia = self.full_inertia
+        evals, evecs = np.linalg.eigh(inertia)
+        return evals
+
+    @property
+    def principal_inertia(self):
+        """Return the principal moments of the inertia (np.array[3]), and the direction of the principal axes of the
+        body (np.array[3,3])."""
+        inertia = self.full_inertia
+        evals, evecs = np.linalg.eigh(inertia)
+        return evals, evecs
+
+    @property
+    def principal_axes(self):
+        """Return the directions of the principal axes of the body as a 3x3 matrix where each column represents an
+        axis."""
+        inertia = self.full_inertia
+        evals, evecs = np.linalg.eigh(inertia)
+        return evecs
+
     @property
     def ixx(self):
         return self._ixx
 
     @ixx.setter
     def ixx(self, ixx):
-        if ixx is not None:
+        if ixx is None:
             ixx = float(ixx)
         self._ixx = ixx
 
@@ -257,7 +517,16 @@ class Inertia(object):
 class Inertial(object):
     r"""Inertial parameters."""
 
-    def __init__(self, mass=None, inertia=None, position=None, orientation=None):
+    def __init__(self, mass=None, inertia=None, position=(0., 0., 0.), orientation=(0., 0., 0.)):
+        """
+
+        Args:
+            mass (float): mass value (in kg)
+            inertia (str, list / tuple of 3/6/9 float, np.ndarray[3/6/9], np.ndarray[3,3]): inertia matrix represented
+                in the body frame.
+            position (np.array[3], str): position of the center of mass.
+            orientation (np.array[3], str): rotation expressed as roll-pitch-yaw angles.
+        """
         self.mass = mass
         self.inertia = inertia
         self.position = position
@@ -302,8 +571,35 @@ class Inertial(object):
         self._inertia = inertia
 
     @property
-    def aligned_inertia(self):
-        raise NotImplementedError
+    def full_inertia(self):
+        rot = self.rot
+        return rot.dot(self._inertia.full_inertia).dot(rot.T)
+
+    @property
+    def principal_inertia(self):
+        """Return the principal moments of the inertia (np.array[3]), and the direction of the principal axes of the
+        body (np.array[3,3])."""
+        evals, evecs = self.inertia.principal_inertia
+        return evals, self.rot.dot(evecs)
+
+    @property
+    def diagonal_inertia(self):
+        """Aligned inertia.
+
+        Returns:
+            np.array[3]: principal moments of the inertia.
+
+        References:
+            - [1] https://en.wikipedia.org/wiki/Moment_of_inertia#Inertia_matrix_in_different_reference_frames
+        """
+        return self.inertia.diagonal_inertia
+
+    @property
+    def principal_axes(self):
+        """Return the directions of the principal axes of the body as a 3x3 matrix where each column represents an
+        axis."""
+        evecs = self.inertia.principal_axes
+        return self.rot.dot(evecs)
 
     @property
     def position(self):
@@ -341,20 +637,41 @@ class Inertial(object):
     def quaternion(self):
         return get_quaternion_from_rpy(self._orientation)
 
+    @property
+    def rot(self):
+        return get_matrix_from_rpy(self.rpy)
 
-class Visual(object):
-    r"""visual parameters for body."""
+    @property
+    def pose(self):
+        return self.position, self.orientation
 
-    def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=None,
-                 material=None):
-        self.name = name
+    @pose.setter
+    def pose(self, pose):
+        if pose is not None:
+            if isinstance(pose, str):
+                pose = pose.split()
+                self.position = pose[:3]
+                self.orientation = pose[3:]
+            elif isinstance(pose, (tuple, list, np.ndarray)):
+                if len(pose) == 2:
+                    self.position = pose[0]
+                    self.orientation = pose[1]
+                elif len(pose) == 6:
+                    self.position = pose[:3]
+                    self.orientation = pose[3:]
+                else:
+                    raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+            else:
+                raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
+
+class Geometry(object):  # Shape
+    """Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, heightmap, etc."""
+
+    def __init__(self, dtype=None, size=None, filename=None):
         self.dtype = dtype
-        self.size = size     # depending on the type it can be different size
-        self.color = color
+        self.size = size            # depending on the type it can be different size
         self.filename = filename
-        self.position = position
-        self.orientation = orientation
-        self.material = material
 
     @property
     def size(self):
@@ -365,12 +682,63 @@ class Visual(object):
         if size is not None:
             if isinstance(size, str):
                 size = [float(s) for s in size.split()]
+                if len(size) == 1:
+                    size = size[0]
             elif isinstance(size, (tuple, list, np.ndarray)):
                 size = [float(s) for s in size]
+                if len(size) == 1:
+                    size = size[0]
             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):
+        """Return the filename format extension for the mesh."""
+        if self.filename is not None:
+            return self.filename.split('.')[-1]
+
+
+# alias
+Shape = Geometry
+
+
+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.geometry = Geometry(dtype=dtype, size=size, filename=filename)
+        self.color = color
+        self.position = position
+        self.orientation = orientation
+        self.material = material
+
+    @property
+    def dtype(self):
+        return self.geometry.dtype
+
+    @dtype.setter
+    def dtype(self, dtype):
+        self.geometry.dtype = dtype
+
+    @property
+    def size(self):
+        return self.geometry.size
+
+    @size.setter
+    def size(self, size):
+        self.geometry.size = size
+
+    @property
+    def filename(self):
+        return self.geometry.filename
+
+    @filename.setter
+    def filename(self, filename):
+        self.geometry.filename = filename
+
     @property
     def color(self):
         return self._color
@@ -428,32 +796,66 @@ class Visual(object):
     def quaternion(self):
         return get_quaternion_from_rpy(self._orientation)
 
+    @property
+    def rot(self):
+        return get_matrix_from_rpy(self.rpy)
+
+    @property
+    def pose(self):
+        return self.position, self.orientation
+
+    @pose.setter
+    def pose(self, pose):
+        if pose is not None:
+            if isinstance(pose, str):
+                pose = pose.split()
+                self.position = pose[:3]
+                self.orientation = pose[3:]
+            elif isinstance(pose, (tuple, list, np.ndarray)):
+                if len(pose) == 2:
+                    self.position = pose[0]
+                    self.orientation = pose[1]
+                elif len(pose) == 6:
+                    self.position = pose[:3]
+                    self.orientation = pose[3:]
+                else:
+                    raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+            else:
+                raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
 
 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.geometry = Geometry(dtype=dtype, size=size, filename=filename)
         self.position = position
         self.orientation = orientation
 
+    @property
+    def dtype(self):
+        return self.geometry.dtype
+
+    @dtype.setter
+    def dtype(self, dtype):
+        self.geometry.dtype = dtype
+
     @property
     def size(self):
-        return self._size
+        return self.geometry.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
+        self.geometry.size = size
+
+    @property
+    def filename(self):
+        return self.geometry.filename
+
+    @filename.setter
+    def filename(self, filename):
+        self.geometry.filename = filename
 
     @property
     def format(self):
@@ -496,13 +898,41 @@ class Collision(object):
     def quaternion(self):
         return get_quaternion_from_rpy(self._orientation)
 
+    @property
+    def rot(self):
+        return get_matrix_from_rpy(self.rpy)
+
+    @property
+    def pose(self):
+        return self.position, self.orientation
+
+    @pose.setter
+    def pose(self, pose):
+        if pose is not None:
+            if isinstance(pose, str):
+                pose = pose.split()
+                self.position = pose[:3]
+                self.orientation = pose[3:]
+            elif isinstance(pose, (tuple, list, np.ndarray)):
+                if len(pose) == 2:
+                    self.position = pose[0]
+                    self.orientation = pose[1]
+                elif len(pose) == 6:
+                    self.position = pose[:3]
+                    self.orientation = pose[3:]
+                else:
+                    raise ValueError("Expecting the pose to be tuple, list or np.ndarray of length 2 or 6")
+            else:
+                raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
+
 
 class Material(object):
     r"""Material info."""
 
-    def __init__(self, name=None, color=None):
+    def __init__(self, name=None, color=None, texture=None):
         self.name = name
         self.color = color
+        self.texture = texture
 
     @property
     def color(self):
@@ -532,3 +962,11 @@ class Material(object):
         if len(self.color) == 3:
             return tuple(self.color) + (1.,)
         return tuple(self.color)
+
+
+class Sensor(object):
+    pass
+
+
+class Heightmap(object):
+    pass

pyrobolearn/utils/parsers/robots/mujoco_parser.py

@@ -4,10 +4,9 @@
 
 # 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
+from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
+from pyrobolearn.utils.parsers.robots.data_structures import Tree, World
 
 
 __author__ = "Brian Delhaisse"
@@ -20,8 +19,8 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class MuJoCoParser(RobotParser):
-    r"""MuJoCo Parser"""
+class MuJoCoParser(WorldParser):
+    r"""MuJoCo Parser and Generator"""
 
     def __init__(self, filename=None):
         """
@@ -40,23 +39,69 @@ class MuJoCoParser(RobotParser):
             filename (str): path to the MuJoCo XML file.
         """
         # load and parse the XML file
-        tree = ET.parse(filename)
+        tree_xml = ET.parse(filename)
 
         # get the root
-        root = tree.getroot()
+        root = tree_xml.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
+        # build the world
+        world = World()
 
-    def get_tree(self):
+        # check default (this is the default configuration when they are not specified)
+        default_tag = root.find('default')
+        if default_tag is not None:
+            pass
+
+        # check physics
+
+        # check assets
+        asset_tag = root.find('asset')
+        if asset_tag is not None:
+            pass
+
+        # check world body
+        worldbody_tag = root.find('worldbody')
+        if worldbody_tag is not None:
+            pass
+
+        # check contact
+
+        # check equality constraint
+
+        # check actuator
+
+        # check sensor
+
+        # set the world
+        self.world = world
+
+    def _check_body(self, body_tag, idx):
         """
-        Return the Tree containing all the elements.
+        Return Body instance from a <body>.
+
+        Args:
+            body_tag (ET.Element): body XML element.
+            idx (int): link index.
 
         Returns:
-            Tree: tree data structure.
+            Body: body data structure.
+        """
+        pass
+
+    def _check_joint(self, joint_tag, idx):
+        """
+        Return Joint instance from a <joint> tag.
+
+        Args:
+            joint_tag (ET.Element): joint XML element.
+            idx (int): joint index.
+
+        Returns:
+            Joint: joint data structure.
         """
         pass
 

pyrobolearn/utils/parsers/robots/proto_parser.py

@@ -19,23 +19,23 @@ __status__ = "Development"
 
 
 class ProtoParser(RobotParser):
-    r"""Proto Parser"""
+    r"""Proto Parser and Generator."""
 
     def __init__(self, filename=None):
         """
         Initialize the Proto parser.
 
         Args:
-            filename (str, None): path to the MuJoCo XML file.
+            filename (str, None): path to the proto file.
         """
         super().__init__(filename)
 
     def parse(self, filename):
         """
-        Load and parse the given URDF file.
+        Load and parse the given proto file.
 
         Args:
-            filename (str): path to the MuJoCo XML file.
+            filename (str): path to the proto file.
         """
         pass
 

pyrobolearn/utils/parsers/robots/robot_parser.py

@@ -19,14 +19,14 @@ __status__ = "Development"
 
 
 class RobotParser(object):
-    r"""Robot Parser"""
+    r"""Robot Parser and Generator."""
 
     def __init__(self, filename=None):
         """
         Initialize the robot parser.
 
         Args:
-            filename (str, None): path to the MuJoCo XML file.
+            filename (str, None): path to the file to parse.
         """
         self.root = None
         self.tree = None
@@ -34,12 +34,34 @@ class RobotParser(object):
         if filename is not None:
             self.parse(filename)
 
+    @property
+    def root(self):
+        return self._root
+
+    @root.setter
+    def root(self, root):
+        if root is not None and not isinstance(root, ET.Element):
+            raise TypeError("Expecting the root to be an instance of `ET.Element`, but got instead: "
+                            "{}".format(type(root)))
+        self._root = root
+
+    @property
+    def tree(self):
+        return self._tree
+
+    @tree.setter
+    def tree(self, tree):
+        if tree is not None and not isinstance(tree, Tree):
+            raise TypeError("Expecting the given tree to be an instance of `Tree`, but got instead: "
+                            "{}".format(type(tree)))
+        self._tree = tree
+
     def parse(self, filename):
         """
-        Load and parse a given MuJoCo XML filename.
+        Load and parse a given file.
 
         Args:
-            filename (str): path to the MuJoCo XML file.
+            filename (str): path to the file to parse.
         """
         pass
 

pyrobolearn/utils/parsers/robots/sdf_parser.py

@@ -7,8 +7,8 @@ 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
+from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
+from pyrobolearn.utils.parsers.robots.data_structures import *
 
 
 __author__ = "Brian Delhaisse"
@@ -21,61 +21,350 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class SDFParser(RobotParser):
-    r"""SDF Parser"""
+class SDFParser(WorldParser):
+    r"""SDF Parser and Generator"""
 
     def __init__(self, filename=None):
         """
         Initialize the SDF parser.
 
         Args:
-            filename (str, None): path to the MuJoCo XML file.
+            filename (str, None): path to the SDF file.
         """
         super().__init__(filename)
+        self.worlds = []
 
     def parse(self, filename):
         """
-        Load and parse the given URDF file.
+        Load and parse the given SDF file.
 
         Args:
-            filename (str): path to the MuJoCo XML file.
+            filename (str): path to the SDF file.
         """
         # load and parse the XML file
-        tree = ET.parse(filename)
+        tree_xml = ET.parse(filename)
 
         # get the root
-        root = tree.getroot()
+        root = tree_xml.getroot()
 
-        # check that the root is <robot>
+        # check that the root is <sdf>
         if root.tag != 'sdf':
             raise RuntimeError("Expecting the first XML tag to be 'sdf' but found instead: {}".format(root.tag))
 
-        # build the tree
+        # check world(s)
+        for i, world_tag in enumerate(root.findall('world')):
+            # build the world
+            world = World(name=world_tag.attrib.get('name', 'world_' + str(i)))
 
-    def get_tree(self):
-        """
-        Return the Tree containing all the elements.
+            # check model
+            for idx, model_tag in enumerate(root.findall('model')):
+                tree = self._check_model(model_tag, idx=idx)
+                world.trees[tree.name] = tree
 
-        Returns:
-            Tree: tree data structure.
-        """
-        pass
+            # check physics
 
-    def get_world(self):
-        """
-        Return the world (which is basically a list of Tree).
-        """
-        pass
+            # append the world to the list of worlds
+            self.worlds.append(world)
 
-    def generate(self, tree=None):
+        # check model
+        models = root.findall('model')
+        if len(models) > 0:
+            world = World()
+        for i, model_tag in enumerate(models):
+            tree = self._check_model(model_tag, idx=i)
+            world.trees[tree.name] = tree
+        if len(models) > 0:
+            self.worlds.append(world)
+
+    def _check_model(self, model_tag, idx):
         """
-        Generate the XML tree from the `Tree` data structure.
+        Return the Tree instance from a <model>.
 
         Args:
-            tree (Tree): Tree data structure.
+            model_tag (ET.Element): model XML element
+            idx (int): model index.
+
+        Returns:
+            Tree: tree data structure containing the model.
+        """
+        # create tree
+        tree = Tree(name=model_tag.attrib.get('name'))
+
+        # check bodies/links
+        for i, link_tag in enumerate(model_tag.findall('link')):
+            body = self._check_body(link_tag, idx=i)
+            # add body to tree
+            tree.bodies[body.name] = body
+
+        # check joints
+        for i, joint_tag in enumerate(root.findall('joint')):
+            # get joint instance from tag
+            joint = self._check_joint(joint_tag, idx=i)
+
+            # add joint in trees
+            tree.joints[joint.name] = joint
+
+            # add joint in parent body
+            parent_body = tree.bodies[joint.parent]
+            parent_body.joints[joint.name] = joint
+
+        return tree
+
+    @staticmethod
+    def _check_body(body_tag, idx):
+        """
+        Return Body instance from a <link>.
+
+        Args:
+            body_tag (ET.Element): link XML element.
+            idx (int): link index.
+
+        Returns:
+            Body: body data structure.
+        """
+        # create body/link
+        body = Body(body_id=idx, name=body_tag.attrib.get('name', 'body_' + str(idx)))
+
+        # check <inertial> tag
+        inertial_tag = body_tag.find('inertial')
+        if inertial_tag is not None:
+            inertial = Inertial()
+
+            # pose
+            pose_tag = inertial_tag.find('pose')
+            if pose_tag is not None:
+                inertial.pose = pose_tag.text
+
+            # mass
+            mass_tag = inertial_tag.find('mass')
+            if mass_tag is not None:
+                inertial.mass = mass_tag.text
+
+            # inertia
+            inertia_tag = inertial_tag.find('inertia')
+            if inertia_tag is not None:
+                ixx = inertia_tag.find('ixx')
+                if ixx is not None:
+                    ixx = ixx.text
+                ixy = inertia_tag.find('ixy')
+                if ixy is not None:
+                    ixy = ixy.text
+                ixz = inertia_tag.find('ixz')
+                if ixz is not None:
+                    ixz = ixz.text
+                iyy = inertia_tag.find('iyy')
+                if iyy is not None:
+                    iyy = iyy.text
+                iyz = inertia_tag.find('iyz')
+                if iyz is not None:
+                    iyz = iyz.text
+                izz = inertia_tag.find('izz')
+                if izz is not None:
+                    izz = izz.text
+                inertial.inertia = {'ixx': ixx, 'ixy': ixy, 'ixz': ixz, 'iyy': iyy, 'iyz': iyz, 'izz': izz}
+
+            # set inertial to body
+            body.inertial = inertial
+
+        # check <visual> tag
+        visual_tag = body_tag.find('visual')
+        if visual_tag is not None:
+            visual = Visual()
+
+            # name
+            visual.name = visual_tag.attrib.get('name')
+
+            # pose
+            pose_tag = visual_tag.find('pose')
+            if pose_tag is not None:
+                visual.pose = pose_tag.text
+
+            # geometry
+            geometry_tag = visual_tag.find('geometry')
+            if geometry_tag is not None:
+                for geometry_type in ['box', 'mesh', 'cylinder', 'sphere', 'plane', 'heightmap']:  # polyline, image
+                    geometry_type_tag = geometry_tag.find(geometry_type)
+                    if geometry_type_tag is not None:
+                        dtype = geometry_type
+                        visual.dtype = dtype
+                        if dtype == 'box':
+                            size_tag = geometry_type_tag.find('size')
+                            visual.size = size_tag.text
+                        elif dtype == 'sphere':
+                            radius_tag = geometry_type_tag.find('radius')
+                            visual.size = radius_tag.text
+                        elif dtype == 'cylinder':
+                            radius_tag = geometry_type_tag.find('radius')
+                            length_tag = geometry_type_tag.find('length')
+                            visual.size = (radius_tag.text, length_tag.text)
+                        elif dtype == 'mesh':
+                            uri_tag = geometry_type_tag.find('uri')
+                            scale_tag = geometry_type_tag.find('scale')
+                            visual.filename = uri_tag.text
+                            visual.size = scale_tag.text
+
+            # 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
+            body.visual = visual
+
+        # check <collision> tag
+        collision_tag = body_tag.find('collision')
+        if collision_tag is not None:
+            collision = Collision()
+
+            # name
+            collision.name = collision_tag.attrib.get('name')
+
+            # origin
+            pose_tag = collision_tag.find('pose')
+            if pose_tag is not None:
+                collision.pose = pose_tag.text
+
+            # geometry
+            geometry_tag = collision_tag.find('geometry')
+            if geometry_tag is not None:
+                for geometry_type in ['box', 'mesh', 'cylinder', 'sphere', 'plane', 'heightmap']:  # polyline, image
+                    geometry_type_tag = geometry_tag.find(geometry_type)
+                    if geometry_type_tag is not None:
+                        dtype = geometry_type
+                        collision.dtype = dtype
+                        if dtype == 'box':
+                            size_tag = geometry_type_tag.find('size')
+                            collision.size = size_tag.text
+                        elif dtype == 'sphere':
+                            radius_tag = geometry_type_tag.find('radius')
+                            collision.size = radius_tag.text
+                        elif dtype == 'cylinder':
+                            radius_tag = geometry_type_tag.find('radius')
+                            length_tag = geometry_type_tag.find('length')
+                            collision.size = (radius_tag.text, length_tag.text)
+                        elif dtype == 'mesh':
+                            uri_tag = geometry_type_tag.find('uri')
+                            scale_tag = geometry_type_tag.find('scale')
+                            collision.filename = uri_tag.text
+                            collision.size = scale_tag.text
+
+            # set collision to body
+            body.collision = collision
+
+        # return the body instance
+        return body
+
+    @staticmethod
+    def _check_joint(joint_tag, idx):
+        """
+        Return Joint instance from a <joint> tag.
+
+        Args:
+            joint_tag (ET.Element): joint XML element.
+            idx (int): joint index.
+
+        Returns:
+            Joint: joint data structure.
+        """
+        attrib = joint_tag.attrib
+        joint = Joint(joint_id=idx, name=attrib.get('name', 'joint_' + str(idx)), dtype=attrib['type'])
+
+        # add parent and child body/link
+        parent_tag = joint_tag.find('parent')
+        if parent_tag is None:
+            raise RuntimeError("Expecting the joint '" + joint.name + "' to have a parent link/body")
+        joint.parent = parent_tag.text
+
+        child_tag = joint_tag.find('child')
+        if child_tag is None:
+            raise RuntimeError("Expecting the joint '" + joint.name + "' to have a child link/body")
+        joint.child = child_tag.text
+
+        # pose
+        pose_tag = joint_tag.find('pose')
+        if pose_tag is not None:
+            joint.pose = pose_tag.text
+
+        # axis
+        axis_tag = joint_tag.find('axis')
+        if axis_tag is not None:
+            axis_xyz_tag = axis_tag.find('xyz')
+            if axis_xyz_tag is not None:
+                joint.axis = axis_xyz_tag.text
+
+            # dynamics
+            dynamics_tag = axis_tag.find('dynamics')
+            if dynamics_tag is not None:
+                damping_tag = dynamics_tag.find('damping')
+
+                # damping
+                if damping_tag is not None:
+                    joint.damping = damping_tag.text
+
+                # friction
+                friction_tag = dynamics_tag.find('friction')
+                if friction_tag is not None:
+                    joint.friction = friction_tag.text
+
+            # limits
+            limits_tag = axis_tag.find('limits')
+            if limits_tag is not None:
+                effort_tag = limits_tag.find('effort')
+                if effort_tag is not None:
+                    joint.effort = effort_tag.text
+
+                velocity_tag = limits_tag.find('velocity')
+                if velocity_tag is not None:
+                    joint.velocity = velocity_tag.text
+
+                lower_limit_tag = limits_tag.find('lower')
+                upper_limit_tag = limits_tag.find('upper')
+                if lower_limit_tag is not None and upper_limit_tag is not None:  # TODO: check if we can have one limit
+                    joint.limits = [lower_limit_tag.text, upper_limit_tag.text]
+
+        return joint
+
+    def generate(self, world=None):
+        """
+        Generate the XML world from the `World` data structure.
+
+        Args:
+            world (World, Tree): world / tree data structure.
 
         Returns:
             ET.Element: root element in the XML file.
         """
-        pass
+        if world is None:
+            world = self.worlds[0]
 
+        # create root element
+        root = ET.Element('sdf', attrib={'version': '1.6'})
+
+        # create world tag
+        name = world.name if world.name is not None else 'default'
+        world_tag = ET.SubElement(root, 'world', attrib={'name': name})
+
+        # create models
+        for tree in world.trees:
+            model_tag = ET.SubElement(world_tag, 'model', attrib={'name': tree.name})
+            if tree.position is not None or tree.orientation is not None:
+                pose_tag = ET.SubElement(model_tag, 'pose')
+                pose_tag.text = str(np.asarray(tree.pose))[1:-1]
+
+            # create links
+            for body in tree.bodies:  # TODO
+                pass
+
+            # create joints
+            for joint in tree.joints:  # TODO
+                pass
+
+        # return root XML element
+        return root

pyrobolearn/utils/parsers/robots/urdf_parser.py

@@ -22,14 +22,14 @@ __status__ = "Development"
 
 
 class URDFParser(RobotParser):
-    r"""URDF Parser"""
+    r"""URDF Parser and Generator"""
 
     def __init__(self, filename=None):
         """
         Initialize the URDF parser.
 
         Args:
-            filename (str, None): path to the MuJoCo XML file.
+            filename (str, None): path to the URDF XML file.
         """
         super().__init__(filename)
 
@@ -38,7 +38,7 @@ class URDFParser(RobotParser):
         Load and parse the given URDF file.
 
         Args:
-            filename (str): path to the MuJoCo XML file.
+            filename (str): path to the URDF XML file.
         """
         # load and parse the XML file
         tree_xml = ET.parse(filename)
@@ -54,177 +54,227 @@ class URDFParser(RobotParser):
         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
+        for i, material_tag in enumerate(root.findall('material')):
+            attrib = material_tag.attrib
+            material = Material(name=attrib.get('name', 'material_' + str(i)))
+            color_tag = material_tag.find('color')
+            if color_tag is not None:
+                material.color = color_tag.attrib.get('rgba')
+            texture_tag = material_tag.find('texture')
+            if texture_tag is not None:
+                material.texture = texture_tag.attrib.get('filename')
+            tree.materials[material.name] = material
 
         # 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
+        for i, body_tag in enumerate(root.findall('link')):
+            # get body instance from tag
+            body = self._check_body(tree, body_tag, idx=i)
 
             # add body to tree
-            tree.bodies[b.name] = b
+            tree.bodies[body.name] = body
 
         # 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]
+        for i, joint_tag in enumerate(root.findall('joint')):
+            # get joint instance from tag
+            joint = self._check_joint(joint_tag, idx=i)
 
             # add joint in trees
-            tree.joints[j.name] = j
+            tree.joints[joint.name] = joint
 
             # add joint in parent body
-            tree.bodies[j.parent] = j
+            parent_body = tree.bodies[joint.parent]
+            parent_body.joints[joint.name] = joint
+
+        # TODO: check sensor, plugins, transmission, etc
 
         # set the tree
         self.tree = tree
 
+    @staticmethod
+    def _check_body(tree, body_tag, idx):
+        """
+        Return Body instance from a <link> tag.
+
+        Args:
+            tree (Tree): Tree data structure.
+            body_tag (ET.Element): link XML element.
+            idx (int): link index.
+
+        Returns:
+            Body: body data structure.
+        """
+        attrib = body_tag.attrib
+        body = Body(body_id=idx, name=attrib.get('name', 'body_' + str(idx)))
+
+        # check <inertial> tag
+        inertial_tag = body_tag.find('inertial')
+        if inertial_tag is not None:
+            inertial = Inertial()
+
+            # origin
+            origin_tag = inertial_tag.find('origin')
+            if origin_tag is not None:
+                inertial.position = origin_tag.attrib.get('xyz')
+                inertial.orientation = origin_tag.attrib.get('rpy')
+
+            # mass
+            mass_tag = inertial_tag.find('mass')
+            if mass_tag is not None:
+                inertial.mass = mass_tag.attrib.get('value')
+
+            # inertia
+            inertia_tag = inertial_tag.find('inertia')
+            if inertia_tag is not None:
+                inertial.inertia = {name: inertia_tag.attrib.get(name)
+                                    for name in ['ixx', 'ixy', 'ixz', 'iyy', 'iyz', 'izz']}
+
+            # set inertial to body
+            body.inertial = inertial
+
+        # check <visual> tag
+        visual_tag = body_tag.find('visual')
+        if visual_tag is not None:
+            visual = Visual()
+
+            # name
+            visual.name = visual_tag.attrib.get('name')
+
+            # origin
+            origin_tag = visual_tag.find('origin')
+            if origin_tag is not None:
+                visual.position = origin_tag.attrib.get('xyz')
+                visual.orientation = origin_tag.attrib.get('rpy')
+
+            # geometry
+            geometry_tag = visual_tag.find('geometry')
+            if geometry_tag is not None:
+                for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+                    geometry_type_tag = geometry_tag.find(geometry_type)
+                    if geometry_type_tag is not None:
+                        dtype = geometry_type
+                        visual.dtype = dtype
+                        if dtype == 'box':
+                            visual.size = geometry_type_tag.attrib['size']
+                        elif dtype == 'sphere':
+                            visual.size = geometry_type_tag.attrib['radius']
+                        elif dtype == 'cylinder':
+                            visual.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
+                        elif dtype == 'mesh':
+                            visual.filename = geometry_type_tag.attrib['filename']
+                            visual.size = geometry_type_tag.attrib.get('scale')
+
+            # material
+            material_tag = visual_tag.find('material')
+            if material_tag is not None:
+                material = Material()
+                name = material_tag.attrib.get('name')
+                color = material_tag.find('color')
+                texture = material_tag.find('texture')
+                if color is not None or texture is not None:
+                    material.name = name
+                    if color is not None:
+                        material.color = color.attrib['rgba']
+                    elif texture is not None:
+                        material.texture = texture.attrib['filename']
+                else:
+                    material = tree.materials.get(name)
+                visual.material = material
+
+            # set visual to body
+            body.visual = visual
+
+        # check <collision> tag
+        collision_tag = body_tag.find('collision')
+        if collision_tag is not None:
+            collision = Collision()
+
+            # name
+            collision.name = collision_tag.attrib.get('name')
+
+            # origin
+            origin_tag = collision_tag.find('origin')
+            if origin_tag is not None:
+                collision.position = origin_tag.attrib.get('xyz')
+                collision.orientation = origin_tag.attrib.get('rpy')
+
+            # geometry
+            geometry_tag = collision_tag.find('geometry')
+            if geometry_tag is not None:
+                for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+                    geometry_type_tag = geometry_tag.find(geometry_type)
+                    if geometry_type_tag is not None:
+                        dtype = geometry_type
+                        collision.dtype = dtype
+                        if dtype == 'box':
+                            collision.size = geometry_type_tag.attrib['size']
+                        elif dtype == 'sphere':
+                            collision.size = geometry_type_tag.attrib['radius']
+                        elif dtype == 'cylinder':
+                            collision.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
+                        elif dtype == 'mesh':
+                            collision.filename = geometry_type_tag.attrib['filename']
+                            collision.size = geometry_type_tag.attrib.get('scale')
+
+            # set collision to body
+            body.collision = collision
+
+        return body
+
+    @staticmethod
+    def _check_joint(joint_tag, idx):
+        """
+        Return Joint instance from a <joint> tag.
+
+        Args:
+            joint_tag (ET.Element): joint XML element.
+            idx (int): joint index.
+
+        Returns:
+            Joint: joint data structure.
+        """
+        attrib = joint_tag.attrib
+        joint = Joint(joint_id=idx, name=attrib.get('name', 'joint_' + str(idx)), dtype=attrib['type'])
+
+        # add parent and child body/link
+        parent_tag = joint_tag.find('parent')
+        if parent_tag is None:
+            raise RuntimeError("Expecting the joint '" + joint.name + "' to have a parent link/body")
+        joint.parent = parent_tag.attrib['link']
+
+        child_tag = joint_tag.find('child')
+        if child_tag is None:
+            raise RuntimeError("Expecting the joint '" + joint.name + "' to have a child link/body")
+        joint.child = child_tag.attrib['link']
+
+        # origin
+        origin_tag = joint_tag.find('origin')
+        if origin_tag is not None:
+            joint.position = origin_tag.attrib.get('xyz')
+            joint.orientation = origin_tag.attrib.get('rpy')
+
+        # axis
+        axis_tag = joint_tag.find('axis')
+        if axis_tag is not None:
+            joint.axis = axis_tag.attrib.get('xyz')
+
+        # dynamics
+        dynamics_tag = joint_tag.find('dynamics')
+        if dynamics_tag is not None:
+            joint.damping = dynamics_tag.attrib.get('damping')
+            joint.friction = dynamics_tag.attrib.get('friction')
+
+        # limits
+        limits_tag = joint_tag.find('limits')
+        if limits_tag is not None:
+            joint.effort = limits_tag.attrib.get('effort')
+            joint.velocity = limits_tag.attrib.get('velocity')
+            lower_limit = limits_tag.attrib.get('lower')
+            upper_limit = limits_tag.attrib.get('upper')
+            if lower_limit is not None and upper_limit is not None:  # TODO: check if we can have one limit
+                joint.limits = [lower_limit, upper_limit]
+
+        return joint
+
     def generate(self, tree=None):
         """
         Generate the XML tree from the `Tree` data structure.
@@ -238,4 +288,161 @@ class URDFParser(RobotParser):
         if tree is None:
             tree = self.tree
 
-        pass
+        # create root element
+        root = ET.Element('robot')
+
+        # generate material tags
+        for material in tree.materials:
+            material_tag = ET.SubElement(root, 'material', attrib={'name': material.name})
+            if material.color is not None:
+                ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
+            if material.texture is not None:
+                ET.SubElement(material_tag, 'texture', attrib={'filename': material.texture})
+
+        # define some common functions
+        def set_name(parent_tag, tag, item):
+            attrib = {}
+            if item.name is not None:
+                attrib['name'] = item.name
+            new_tag = ET.SubElement(parent_tag, tag, attrib=attrib)
+            return new_tag
+
+        def set_origin(tag, item):
+            origin = {}
+            if item.position is not None:
+                origin['xyz'] = str(np.asarray(item.position))[1:-1]
+            if item.orientation is not None:
+                origin['rpy'] = str(np.asarray(item.orientation))[1:-1]
+            if len(origin) > 0:
+                ET.SubElement(tag, 'origin', attrib=origin)
+
+        def set_geometry(tag, item):
+            if item.geometry is not None:
+                geometry_tag = ET.SubElement(tag, 'geometry')
+                geometry = item.geometry
+                dtype = geometry.dtype
+
+                if dtype in {'box', 'sphere', 'cylinder', 'mesh'}:
+                    attrib = {}
+                    if dtype == 'box':
+                        attrib['size'] = str(np.asarray(geometry.size))[1:-1]
+                    elif dtype == 'sphere':
+                        attrib['radius'] = str(geometry.size)
+                    elif dtype == 'cylinder':
+                        attrib['radius'] = str(geometry.size[0])
+                        attrib['length'] = str(geometry.size[1])
+                    else:  # mesh
+                        attrib['filename'] = geometry.filename
+                        attrib['scale'] = str(np.asarray(geometry.size))[1:-1]
+
+                    ET.SubElement(geometry_tag, dtype, attrib=attrib)
+
+        # generate <link>
+        for link in tree.bodies:
+            link_tag = ET.SubElement(root, 'link', attrib={'name': link.name})
+
+            # create <inertial> tag
+            inertial = link.inertial
+            if inertial is not None:
+                inertial_tag = ET.SubElement(link_tag, 'inertial')
+
+                # <origin>
+                set_origin(inertial_tag, inertial)
+
+                # <mass>
+                if inertial.mass is not None:
+                    ET.SubElement(inertial_tag, 'mass', attrib={'value': str(inertial.mass)})
+
+                # <inertia>
+                if inertial.inertia is not None:
+                    I = inertial.inertia
+                    inertia = {}
+                    if I.ixx is not None:
+                        inertia['ixx'] = str(I.ixx)
+                    if I.iyy is not None:
+                        inertia['iyy'] = str(I.iyy)
+                    if I.izz is not None:
+                        inertia['izz'] = str(I.izz)
+                    if I.ixy is not None:
+                        inertia['ixy'] = str(I.ixy)
+                    if I.ixz is not None:
+                        inertia['ixz'] = str(I.ixz)
+                    if I.iyz is not None:
+                        inertia['iyz'] = str(I.iyz)
+                    ET.SubElement(inertial_tag, 'inertia', attrib=inertia)
+
+            # create <visual> tag
+            visual = link.visual
+            if visual is not None:
+                # create visual tag with name
+                visual_tag = set_name(link_tag, 'visual', visual)
+
+                # <origin>
+                set_origin(visual_tag, visual)
+
+                # <geometry>
+                set_geometry(visual_tag, visual)
+
+                # <material>
+                if visual.material is not None:
+                    material = visual.material
+                    material_tag = ET.SubElement(visual, 'material', attrib={'name': material.name})
+                    if material.color is not None:
+                        ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
+                    if material.texture is not None:
+                        ET.SubElement(material_tag, 'texture', attrib={'filename': material.texture})
+
+            # create <collision> tag
+            collision = link.collision
+            if collision is not None:
+                # create collision tag with name
+                collision_tag = set_name(link_tag, 'collision', collision)
+
+                # <origin>
+                set_origin(collision_tag, collision)
+
+                # <geometry>
+                set_geometry(collision_tag, collision)
+
+        def set_name_and_type(parent_tag, tag, item):
+            kwargs = {}
+            if item.name is not None:
+                kwargs['name'] = item.name
+            if item.dtype is not None:
+                kwargs['type'] = item.dtype
+            return ET.SubElement(parent_tag, tag, attrib=kwargs)
+
+        # generate <joint>
+        for joint in tree.joints:
+            # set joint name and type
+            joint_tag = set_name_and_type(root, 'joint', joint)
+
+            # <origin>
+            set_origin(joint_tag, joint)
+
+            # <parent>
+            if joint.parent is not None:
+                ET.SubElement(joint_tag, 'parent', attrib={'link': joint.parent})
+
+            # <child>
+            if joint.child is not None:
+                ET.SubElement(joint_tag, 'child', attrib={'link': joint.child})
+
+            # <axis>
+            if joint.axis is not None:
+                ET.SubElement(joint_tag, 'axis', attrib={'xyz': str(np.asarray(joint.axis))[1:-1]})
+
+            # <limit>
+            if joint.limits is not None or joint.effort is not None or joint.velocity is not None:
+                kwargs = {}
+                if joint.effort is not None:
+                    kwargs['effort'] = str(joint.effort)
+                if joint.velocity is not None:
+                    kwargs['velocity'] = str(joint.velocity)
+                if joint.limits is not None:
+                    kwargs['lower'] = str(joint.limits[0])
+                    kwargs['upper'] = str(joint.limits[1])
+                ET.SubElement(joint_tag, 'limit', attrib=kwargs)
+
+        # return root XML element
+        return root

pyrobolearn/utils/parsers/robots/world_parser.py

@@ -0,0 +1,129 @@
+#!/usr/bin/env python
+"""Define the abstract world 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 World, 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 WorldParser(object):
+    r"""World Parser and Generator."""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the world parser.
+
+        Args:
+            filename (str, None): path to the file to parse.
+        """
+        self.root = None
+        self.world = None
+        self.filename = filename
+        if filename is not None:
+            self.parse(filename)
+
+    @property
+    def root(self):
+        return self._root
+
+    @root.setter
+    def root(self, root):
+        if root is not None and not isinstance(root, ET.Element):
+            raise TypeError("Expecting the root to be an instance of `ET.Element`, but got instead: "
+                            "{}".format(type(root)))
+        self._root = root
+
+    @property
+    def world(self):
+        return self._world
+
+    @world.setter
+    def world(self, world):
+        if world is not None and not isinstance(world, World):
+            raise TypeError("Expecting the given world to be an instance of `World` but got instead: "
+                            "{}".format(type(world)))
+        self._world = world
+
+    def parse(self, filename):
+        """
+        Load and parse a given file.
+
+        Args:
+            filename (str): path to the file to parse.
+        """
+        pass
+
+    def get_tree(self, index=None, tag=None):
+        """
+        Get the specified tree(s).
+
+        Args:
+            index (int, None): tree index. If None, it will return all the trees.
+            tag (str, None): tag of the root that we want.
+
+        Returns:
+            (list of) Tree: tree data structure(s).
+        """
+        pass
+
+    def get_world(self):
+        """
+        Return the world containing all the elements that compose that world.
+
+        Returns:
+            World: World data structure.
+        """
+        return self.world
+
+    def generate(self, world=None):
+        """
+        Generate the XML world from the `World` data structure.
+
+        Args:
+            world (World): world 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 world 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'))