update parsers and mujoco/dart simulators

pyrobolearn/robots/lipm.py

@@ -0,0 +1,153 @@
+#!/usr/bin/env python
+"""Provide the various Linear Inverted Pendulum Models (LIPMs).
+
+This includes: LIPM2D, DualLIPM2D, LIPM3D, DualLIPM3D
+"""
+
+# TODO: finish to implement this
+
+import os
+
+from pyrobolearn.robots.legged_robot import LeggedRobot
+
+__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 LIPM2D(LeggedRobot):
+    r"""Linear Inverted Pendulum Model 2D
+
+    This class describes the 2D Linear Inverted Pendulum Model (2D-LIPM), which underlies walking behaviors, and is
+    often used as a template model in locomotion. The 2D version constraints the possible motion to belong to the xz
+    plan.
+
+    See Also:
+        - LIPM3D: the 3D version of the 2D-LIPM
+        - DualLIPM2D: the dual version of the 2D-LIPM
+        - DualLIPM3D: the dual version of the 3D-LIPM
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/lipm2d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(LIPM2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'lipm_2d'
+
+
+class DualLIPM2D(LeggedRobot):
+    r"""Dual Linear Inverted Pendulum Model 2D
+
+    This class describes the dual 2D Linear Inverted Pendulum Model (dual 2D-LIPM), which underlies walking behaviors,
+    and is often used as a template model in locomotion. The 2D version constraints the possible motions to belong to
+    the xz plan.
+
+    See Also:
+        - LIPM3D: the 3D version of the 2D-LIPM
+        - DualLIPM3D: the dual version of the 3D-LIPM
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_lipm2d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(DualLIPM2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'dual_lipm_2d'
+
+
+class LIPM3D(LeggedRobot):
+    r"""Linear Inverted Pendulum Model 3D
+
+    This class describes the 3D Linear Inverted Pendulum Model (3D-LIPM), which underlies walking behaviors, and is
+    often used as a template model in locomotion.
+
+    See Also:
+        - DualLIPM3D: the dual version of the 3D-LIPM
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/lipm3d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(LIPM3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'lipm_3d'
+
+
+class DualLIPM3D(LeggedRobot):
+    r"""Dual Linear Inverted Pendulum Model 3D
+
+    This class describes the dual 3D Linear Inverted Pendulum Model (dual 3D-LIPM), which underlies walking behaviors,
+    and is often used as a template model in locomotion.
+
+    See Also:
+        - SLIP: the spring-loaded inverted pendulum which is more suitable for running and jumping behaviors.
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_lipm3d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(DualLIPM3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'dual_lipm_3d'
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    from pyrobolearn.simulators import BulletSim
+    from pyrobolearn.worlds import BasicWorld
+
+    # Create simulator
+    sim = BulletSim()
+
+    # create world
+    world = BasicWorld(sim)
+
+    # create robot
+    robot = LIPM2D(sim)
+
+    # print information about the robot
+    robot.print_info()
+
+    # run simulation
+    for i in count():
+        # step in simulation
+        world.step(sleep_dt=1./240)

pyrobolearn/robots/slip.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+"""Provide the various Spring-Loaded Inverted Pendulum (SLIP) models.
+
+This includes: SLIP2D, DualSLIP2D, SLIP3D, DualSLIP3D
+"""
+
+# TODO: finish to implement this
+
+import os
+
+from pyrobolearn.robots.legged_robot import LeggedRobot
+
+__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 SLIP2D(LeggedRobot):
+    r"""Spring-Loaded Inverted Pendulum (2D)
+
+    This class describes the 2D Spring-Loaded Inverted Pendulum (2D-SLIP) model, which underlies dynamic locomotive
+    behaviors (such as running and jumping), and is often used as a template model in locomotion. The 2D version
+    constraints the possible motion to belong to the xz plan.
+
+    See Also:
+        - SLIP3D: the 3D version of the 2D-SLIP
+        - DualSLIP2D: the dual version of the 2D-SLIP
+        - DualSLIP3D: the dual version of the 3D-SLIP
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/SLIP2d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(SLIP2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'SLIP_2d'
+
+
+class DualSLIP2D(LeggedRobot):
+    r"""Dual Linear Inverted Pendulum Model 2D
+
+    This class describes the dual 2D Spring-Loaded Inverted Pendulum (dual 2D-SLIP) model, which underlies dynamic
+    locomotive behaviors (such as running and jumping), and is often used as a template model in locomotion. The 2D
+    version constraints the possible motion to belong to the xz plan.
+
+    See Also:
+        - SLIP3D: the 3D version of the 2D-SLIP
+        - DualSLIP3D: the dual version of the 3D-SLIP
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_SLIP2d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(DualSLIP2D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'dual_SLIP_2d'
+
+
+class SLIP3D(LeggedRobot):
+    r"""Linear Inverted Pendulum Model 3D
+
+    This class describes the 3D Spring-Loaded Inverted Pendulum (3D-SLIP) model, which underlies dynamic locomotive
+    behaviors (such as running and jumping), and is often used as a template model in locomotion.
+
+    See Also:
+        - DualSLIP3D: the dual version of the 3D-SLIP
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/SLIP3d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(SLIP3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'SLIP_3d'
+
+
+class DualSLIP3D(LeggedRobot):
+    r"""Dual Linear Inverted Pendulum Model 3D
+
+    This class describes the dual 3D Spring-Loaded Inverted Pendulum (dual 3D-SLIP) model, which underlies dynamic
+    locomotive behaviors (such as running and jumping), and is often used as a template model in locomotion.
+    """
+
+    def __init__(self, simulator, position=(0., 0., 0.), orientation=(0, 0, 0, 1), fixed_base=False, scale=1.,
+                 urdf=os.path.dirname(__file__) + '/urdfs/templates/dual_SLIP3d.xml'):
+        # check parameters
+        if position is None:
+            position = (0., 0., 0.)
+        if len(position) == 2:  # assume x, y are given
+            position = tuple(position) + (0.,)
+        if orientation is None:
+            orientation = (0, 0, 0, 1.)
+        if fixed_base is None:
+            fixed_base = False
+
+        super(DualSLIP3D, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
+        self.name = 'dual_SLIP_3d'
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    from pyrobolearn.simulators import BulletSim
+    from pyrobolearn.worlds import BasicWorld
+
+    # Create simulator
+    sim = BulletSim()
+
+    # create world
+    world = BasicWorld(sim)
+
+    # create robot
+    robot = SLIP2D(sim)
+
+    # print information about the robot
+    robot.print_info()
+
+    # run simulation
+    for i in count():
+        # step in simulation
+        world.step(sleep_dt=1./240)

pyrobolearn/simulators/bullet.py

@@ -3513,10 +3513,10 @@ class Bullet(Simulator):
             np.array[N]: joint torques computed using the rigid-body equation of motion
 
         References:
-            [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
-            [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
-            [3] "Springer Handbook of Robotics", Siciliano et al., 2008
-            [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
+            - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
+            - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
+            - [3] "Springer Handbook of Robotics", Siciliano et al., 2008
+            - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
                 http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
         """
         # convert numpy arrays to lists
@@ -3570,10 +3570,10 @@ class Bullet(Simulator):
             np.array[N]: joint accelerations computed using the rigid-body equation of motion
 
         References:
-            [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
-            [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
-            [3] "Springer Handbook of Robotics", Siciliano et al., 2008
-            [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
+            - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
+            - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
+            - [3] "Springer Handbook of Robotics", Siciliano et al., 2008
+            - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
                 http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
         """
         # convert numpy arrays to lists

pyrobolearn/simulators/mujoco.py

@@ -62,7 +62,6 @@ 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
 from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
 
 

pyrobolearn/simulators/simulator.py

@@ -12,9 +12,9 @@ dynamics, etc.
 Dependencies in PRL: None
 
 References:
-    [1] PyBullet: https://pybullet.org
-    [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
-    [3] PEP8: https://www.python.org/dev/peps/pep-0008/
+    - [1] PyBullet: https://pybullet.org
+    - [2] PyBullet Quickstart Guide: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
+    - [3] PEP8: https://www.python.org/dev/peps/pep-0008/
 """
 
 from pyrobolearn.utils.data_structures.orderedset import OrderedSet
@@ -42,8 +42,8 @@ class Simulator(object):
         sim = GazeboROS()
 
     References:
-    [1] PyBullet: https://pybullet.org
-    [2] PEP8: https://www.python.org/dev/peps/pep-0008/
+        - [1] PyBullet: https://pybullet.org
+        - [2] PEP8: https://www.python.org/dev/peps/pep-0008/
     """
 
     # keep track of the instantiated simulators
@@ -2350,10 +2350,10 @@ class Simulator(object):
             np.array[N]: joint torques computed using the rigid-body equation of motion
 
         References:
-            [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
-            [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
-            [3] "Springer Handbook of Robotics", Siciliano et al., 2008
-            [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
+            - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
+            - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
+            - [3] "Springer Handbook of Robotics", Siciliano et al., 2008
+            - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
                 http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
         """
         pass
@@ -2398,10 +2398,10 @@ class Simulator(object):
             np.array[N]: joint accelerations computed using the rigid-body equation of motion
 
         References:
-            [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
-            [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
-            [3] "Springer Handbook of Robotics", Siciliano et al., 2008
-            [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
+            - [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
+            - [2] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
+            - [3] "Springer Handbook of Robotics", Siciliano et al., 2008
+            - [4] Lecture on "Impedance Control" by Prof. De Luca, Universita di Roma,
                 http://www.diag.uniroma1.it/~deluca/rob2_en/15_ImpedanceControl.pdf
         """
         pass

pyrobolearn/utils/parsers/robots/__init__.py

@@ -7,3 +7,9 @@ from .mujoco_parser import MuJoCoParser
 
 # import sdf parser
 from .sdf_parser import SDFParser
+
+# import skel parser
+from .skel_parser import SkelParser
+
+# import proto parser
+# from .proto_parser import ProtoParser

pyrobolearn/utils/parsers/robots/data_structures.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-"""Provide the data structures that are shared among the various parsers and converters.
+"""Provide the common data structures that are shared among the various parsers, generators, and converters.
 """
 
 import numpy as np
@@ -62,101 +62,18 @@ class PhysicsEngine(object):
         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.)):
+    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):
-    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 = {}
-        self.position = None
-        self.orientation = None
+        # self.forward_axis = forward_axis
+        # self.up_axis = up_axis
 
     @property
     def position(self):
@@ -188,11 +105,11 @@ class Tree(object):
 
     @property
     def rpy(self):
-        return self._orientation
+        return self.orientation
 
     @property
     def quaternion(self):
-        return get_quaternion_from_rpy(self._orientation)
+        return get_quaternion_from_rpy(self.orientation)
 
     @property
     def rot(self):
@@ -200,6 +117,8 @@ class Tree(object):
 
     @property
     def pose(self):
+        if self.position is None and self.orientation is None:
+            return None
         return self.position, self.orientation
 
     @pose.setter
@@ -222,6 +141,266 @@ class Tree(object):
                 raise TypeError("Expecting the pose to be a str, list, tuple or np.ndarray")
 
 
+class Physics(object):
+    r"""Physical properties of the world.
+
+    This includes gravity, friction, viscosity, etc.
+    """
+
+    def __init__(self, gravity=(0., 0., -9.81), timestep=None):
+        # gravity depends on the world frame; the frame axis convention that we use.
+        # By default, x points forward, y on the left, and z upward.
+        self.gravity = gravity
+        self.timestep = timestep
+
+    @property
+    def gravity(self):
+        return self._gravity
+
+    @gravity.setter
+    def gravity(self, gravity):
+        if gravity is not None:
+            if isinstance(gravity, str):
+                gravity = [float(g) for g in gravity.split()]
+            gravity = np.asarray(gravity).reshape(-1)
+        self._gravity = gravity
+
+    @property
+    def timestep(self):
+        return self._timestep
+
+    @timestep.setter
+    def timestep(self, timestep):
+        if timestep is not None:
+            timestep = float(timestep)
+        self._timestep = timestep
+
+
+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
+        self.lights = OrderedDict()
+
+    @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, ambient=None, diffuse=None, specular=None,
+                 attenuation=None, direction=None, spot=None, position=None, orientation=None, active=True):
+        """
+        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 (np.array[3], str): orientation of the light expressed as roll-pitch-yaw angles.
+            active (bool): if True, the light is on.
+        """
+        self.name = name
+        self.dtype = dtype
+        self.shadows = cast_shadows
+        self.ambient = ambient
+        self.diffuse = diffuse
+        self.specular = specular
+        self.attenuation = attenuation
+        self.direction = direction
+        self.spot = spot
+        self.active = active
+        self.frame = Frame(position=position, orientation=orientation)
+
+    @property
+    def shadows(self):
+        return self._shadows
+
+    @shadows.setter
+    def shadows(self, enable):
+        if enable is not None:
+            if isinstance(enable, str):
+                enable = enable.lower()
+                if len(enable) == 1:
+                    enable = int(enable)
+                elif enable == 'false':
+                    enable = 0
+                elif enable == 'true':
+                    enable = 1
+            enable = bool(enable)
+        self._shadows = enable
+
+    @property
+    def position(self):
+        return self.frame.position
+
+    @position.setter
+    def position(self, position):
+        self.frame.position = position
+
+    @property
+    def orientation(self):
+        return self.frame.orientation
+
+    @orientation.setter
+    def orientation(self, orientation):
+        self.frame.orientation = orientation
+
+    @property
+    def rpy(self):
+        return self.frame.rpy
+
+    @property
+    def quaternion(self):
+        return self.frame.quaternion
+
+    @property
+    def rot(self):
+        return self.frame.quaternion
+
+    @property
+    def pose(self):
+        return self.frame.pose
+
+    @pose.setter
+    def pose(self, pose):
+        self.frame.pose = pose
+
+    @property
+    def direction(self):
+        return self._direction
+
+    @direction.setter
+    def direction(self, direction):
+        if direction is not None:
+            if isinstance(direction, str):
+                direction = [float(d) for d in direction.split()]
+            direction = np.asarray(direction).reshape(-1)
+        self._direction = direction
+
+    @property
+    def ambient(self):
+        return self._ambient
+
+    @ambient.setter
+    def ambient(self, ambient):
+        if ambient is not None:
+            if isinstance(ambient, str):  # e.g. '0.5 0.1 1. 1.'
+                ambient = (float(c) for c in ambient.split())
+            if not isinstance(ambient, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                ambient = tuple(ambient)
+        self._ambient = ambient
+
+    color = ambient
+
+    @property
+    def diffuse(self):
+        return self._diffuse
+
+    @diffuse.setter
+    def diffuse(self, diffuse):
+        if diffuse is not None:
+            if isinstance(diffuse, str):  # e.g. '0.5 0.1 1. 1.'
+                diffuse = (float(c) for c in diffuse.split())
+            if not isinstance(diffuse, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                diffuse = tuple(diffuse)
+        self._diffuse = diffuse
+
+    @property
+    def specular(self):
+        return self._specular
+
+    @specular.setter
+    def specular(self, specular):
+        if specular is not None:
+            if isinstance(specular, str):  # e.g. '0.5 0.1 1. 1.'
+                specular = (float(c) for c in specular.split())
+            if not isinstance(specular, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                specular = tuple(specular)
+        self._specular = specular
+
+
+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 = {}
+        self.frame = Frame()
+
+    @property
+    def position(self):
+        return self.frame.position
+
+    @position.setter
+    def position(self, position):
+        self.frame.position = position
+
+    @property
+    def orientation(self):
+        return self.frame.orientation
+
+    @orientation.setter
+    def orientation(self, orientation):
+        self.frame.orientation = orientation
+
+    @property
+    def rpy(self):
+        return self.frame.rpy
+
+    @property
+    def quaternion(self):
+        return self.frame.quaternion
+
+    @property
+    def rot(self):
+        return self.frame.quaternion
+
+    @property
+    def pose(self):
+        return self.frame.pose
+
+    @pose.setter
+    def pose(self, pose):
+        self.frame.pose = pose
+
+
 class Body(object):
     r"""Body / Link data structure."""
 
@@ -255,6 +434,11 @@ class Joint(object):
     - 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.
+
+    - URDF: continuous, fixed, floating, planar, prismatic, revolute
+    - SDF: ball, fixed, gearbox, prismatic, revolute, revolute2, screw, universal
+    - Dart: ball, free, euler, prismatic, weld (=fixed), revolute, universal
+    - MuJoCo: ball, free, hinge (=revolute), slide
     """
 
     def __init__(self, joint_id, name=None, dtype=None, limits=None, parent=None, child=None, axis=None,
@@ -266,13 +450,15 @@ class Joint(object):
         self.parent = parent
         self.child = child
         self.axis = axis
-        self.position = position
-        self.orientation = orientation
+        self.frame = Frame(position=position, orientation=orientation)
         self.friction = friction
         self.damping = damping
         self.effort = effort
         self.velocity = velocity
 
+        self.init_position = None
+        self.init_velocity = None
+
     @property
     def limits(self):
         return self._limits
@@ -304,66 +490,39 @@ class Joint(object):
 
     @property
     def position(self):
-        return self._position
+        return self.frame.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
+        self.frame.position = position
 
     @property
     def orientation(self):
-        return self._orientation
+        return self.frame.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
+        self.frame.orientation = orientation
 
     @property
     def rpy(self):
-        return self._orientation
+        return self.frame.rpy
 
     @property
     def quaternion(self):
-        return get_quaternion_from_rpy(self._orientation)
+        return self.frame.quaternion
 
     @property
     def rot(self):
-        return get_matrix_from_rpy(self.rpy)
+        return self.frame.quaternion
 
     @property
     def pose(self):
-        return self.position, self.orientation
+        return self.frame.pose
 
     @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")
+        self.frame.pose = pose
 
     @property
     def friction(self):
@@ -405,6 +564,44 @@ class Joint(object):
             velocity = float(velocity)
         self._velocity = velocity
 
+    @property
+    def init_position(self):
+        return self._init_position
+
+    @init_position.setter
+    def init_position(self, position):
+        if position is not None:
+            if isinstance(position, str):
+                position = np.asarray([float(s) for s in position.split()])
+                if len(position) == 1:
+                    position = position[0]
+            elif isinstance(position, (tuple, list, np.ndarray)):
+                position = np.asarray([float(s) for s in position])
+                if len(position) == 1:
+                    position = position[0]
+            elif not isinstance(position, (float, int)):
+                raise TypeError("Expecting the init_position to be a float, int, list, tuple or np.ndarray")
+        self._init_position = position
+
+    @property
+    def init_velocity(self):
+        return self._init_velocity
+
+    @init_velocity.setter
+    def init_velocity(self, velocity):
+        if velocity is not None:
+            if isinstance(velocity, str):
+                velocity = np.asarray([float(s) for s in velocity.split()])
+                if len(velocity) == 1:
+                    velocity = velocity[0]
+            elif isinstance(velocity, (tuple, list, np.ndarray)):
+                velocity = np.asarray([float(s) for s in velocity])
+                if len(velocity) == 1:
+                    velocity = velocity[0]
+            elif not isinstance(velocity, (float, int)):
+                raise TypeError("Expecting the init_velocity to be a float, int, list, tuple or np.ndarray")
+        self._init_velocity = velocity
+
 
 class Inertia(object):
     r"""Inertia data structure"""
@@ -459,7 +656,7 @@ class Inertia(object):
 
     @ixx.setter
     def ixx(self, ixx):
-        if ixx is None:
+        if ixx is not None:
             ixx = float(ixx)
         self._ixx = ixx
 
@@ -529,8 +726,7 @@ class Inertial(object):
         """
         self.mass = mass
         self.inertia = inertia
-        self.position = position
-        self.orientation = orientation
+        self.frame = Frame(position=position, orientation=orientation)
 
     @property
     def mass(self):
@@ -603,70 +799,49 @@ class Inertial(object):
 
     @property
     def position(self):
-        return self._position
+        return self.frame.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
+        self.frame.position = position
 
     @property
     def orientation(self):
-        return self._orientation
+        return self.frame.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
+        self.frame.orientation = orientation
 
     @property
     def rpy(self):
-        return self._orientation
+        return self.frame.rpy
 
     @property
     def quaternion(self):
-        return get_quaternion_from_rpy(self._orientation)
+        return self.frame.quaternion
 
     @property
     def rot(self):
-        return get_matrix_from_rpy(self.rpy)
+        return self.frame.quaternion
 
     @property
     def pose(self):
-        return self.position, self.orientation
+        return self.frame.pose
 
     @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")
+        self.frame.pose = pose
 
 
 class Geometry(object):  # Shape
-    """Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, heightmap, etc."""
+    """Geometry: plane, sphere, box, mesh, cylinder, ellipsoid, capsule, cone, heightmap, etc.
+
+    - URDF: box, cylinder, mesh, sphere
+    - SDF: box, cylinder, heightmap, image, mesh, plane, polyline, sphere
+    - Skel: box, capsule, cone, cylinder, ellipsoid, mesh, multi_sphere, sphere
+    - MuJoCo: box, capsule, cylinder, ellipsoid, hfield (=height field), mesh, plane, sphere
+    """
 
     def __init__(self, dtype=None, size=None, filename=None):
         self.dtype = dtype
@@ -706,14 +881,11 @@ 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):
+    def __init__(self, name=None, dtype=None, size=None, color=None, filename=None, position=None, orientation=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
+        self.frame = Frame(position=position, orientation=orientation)
+        self.material = Material(color=color)
 
     @property
     def dtype(self):
@@ -741,18 +913,11 @@ class Visual(object):
 
     @property
     def color(self):
-        return self._color
+        return self.material.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
+        self.material.color = color
 
     @property
     def format(self):
@@ -762,66 +927,39 @@ class Visual(object):
 
     @property
     def position(self):
-        return self._position
+        return self.frame.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
+        self.frame.position = position
 
     @property
     def orientation(self):
-        return self._orientation
+        return self.frame.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
+        self.frame.orientation = orientation
 
     @property
     def rpy(self):
-        return self._orientation
+        return self.frame.rpy
 
     @property
     def quaternion(self):
-        return get_quaternion_from_rpy(self._orientation)
+        return self.frame.quaternion
 
     @property
     def rot(self):
-        return get_matrix_from_rpy(self.rpy)
+        return self.frame.quaternion
 
     @property
     def pose(self):
-        return self.position, self.orientation
+        return self.frame.pose
 
     @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")
+        self.frame.pose = pose
 
 
 class Collision(object):
@@ -830,8 +968,7 @@ class Collision(object):
     def __init__(self, name=None, dtype=None, size=None, filename=None, position=None, orientation=None):
         self.name = name
         self.geometry = Geometry(dtype=dtype, size=size, filename=filename)
-        self.position = position
-        self.orientation = orientation
+        self.frame = Frame(position=position, orientation=orientation)
 
     @property
     def dtype(self):
@@ -864,82 +1001,63 @@ class Collision(object):
 
     @property
     def position(self):
-        return self._position
+        return self.frame.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
+        self.frame.position = position
 
     @property
     def orientation(self):
-        return self._orientation
+        return self.frame.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
+        self.frame.orientation = orientation
 
     @property
     def rpy(self):
-        return self._orientation
+        return self.frame.rpy
 
     @property
     def quaternion(self):
-        return get_quaternion_from_rpy(self._orientation)
+        return self.frame.quaternion
 
     @property
     def rot(self):
-        return get_matrix_from_rpy(self.rpy)
+        return self.frame.quaternion
 
     @property
     def pose(self):
-        return self.position, self.orientation
+        return self.frame.pose
 
     @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")
+        self.frame.pose = pose
 
 
 class Material(object):
-    r"""Material info."""
+    r"""Material info.
+
+    Type of colors:
+    - ambient: color of an object when no lights are pointing at it.
+    - diffuse: color of an object under a pure white light.
+    - specular: color and intensity of a highlight from a specular reflection (higher values make an object more shiny).
+    - emissive: color where the light appears to being emitted from the object.
+    """
 
     def __init__(self, name=None, color=None, texture=None):
         self.name = name
-        self.color = color
+        self.color = color  # ambient color
         self.texture = texture
 
-    @property
-    def color(self):
-        return self._color
+        # RGBA color
+        self.diffuse = None
+        self.specular = None
+        self.emissive = None
 
-    @color.setter
-    def color(self, color):
+    @staticmethod
+    def _check_color(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())
@@ -947,7 +1065,15 @@ class Material(object):
                 raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
             else:
                 color = tuple(color)
-        self._color = color
+        return color
+
+    @property
+    def color(self):
+        return self._color
+
+    @color.setter
+    def color(self, color):
+        self._color = self._check_color(color)
 
     @property
     def rgb(self):
@@ -963,10 +1089,59 @@ class Material(object):
             return tuple(self.color) + (1.,)
         return tuple(self.color)
 
+    @property
+    def diffuse(self):
+        return self._diffuse
+
+    @diffuse.setter
+    def diffuse(self, diffuse):
+        if diffuse is not None:
+            if isinstance(diffuse, str):  # e.g. '0.5 0.1 1. 1.'
+                diffuse = (float(c) for c in diffuse.split())
+            if not isinstance(diffuse, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                diffuse = tuple(diffuse)
+        self._diffuse = diffuse
+
+    @property
+    def specular(self):
+        return self._specular
+
+    @specular.setter
+    def specular(self, specular):
+        if specular is not None:
+            if isinstance(specular, str):  # e.g. '0.5 0.1 1. 1.'
+                specular = (float(c) for c in specular.split())
+            if not isinstance(specular, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                specular = tuple(specular)
+        self._specular = specular
+
+    @property
+    def emissive(self):
+        return self._emissive
+
+    @emissive.setter
+    def emissive(self, emissive):
+        if emissive is not None:
+            if isinstance(emissive, str):  # e.g. '0.5 0.1 1. 1.'
+                emissive = (float(c) for c in emissive.split())
+            if not isinstance(emissive, (list, tuple)):
+                raise TypeError("Expecting the color to be a tuple or list of 3 or 4 float")
+            else:
+                emissive = tuple(emissive)
+        self._emissive = emissive
+
 
 class Sensor(object):
     pass
 
 
+class Actuator(object):  # Motor
+    pass
+
+
 class Heightmap(object):
     pass

pyrobolearn/utils/parsers/robots/mujoco_parser.py

@@ -1,12 +1,31 @@
 #!/usr/bin/env python
-"""Define the MuJoCo parser.
+"""Define the MuJoCo parser/generator.
+
+Notes:
+    - MuJoCo only accepts STL meshes
+    - MuJoCo can load PNG files for textures and heightmap.
+
+References:
+    - MuJoCo overview: http://www.mujoco.org/book/index.html
+    - MuJoCo XML format: http://www.mujoco.org/book/XMLreference.html
 """
 
 # import XML parser
 import xml.etree.ElementTree as ET
 
+# import mesh converter (from .obj to .stl)
+try:
+    import pymesh    # rapid prototyping platform focused on geometry processing
+    # doc: https://pymesh.readthedocs.io/en/latest/user_guide.html
+
+    import pyassimp  # library to import and export various 3d-model-formats
+    # doc: http://www.assimp.org/index.php
+    # github: https://github.com/assimp/assimp
+except ImportError as e:
+    raise ImportError(str(e) + "\nTry to install pymesh pyassimp: `pip install pymesh pyassimp`")
+
 from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
-from pyrobolearn.utils.parsers.robots.data_structures import Tree, World
+from pyrobolearn.utils.parsers.robots.data_structures import *
 
 
 __author__ = "Brian Delhaisse"
@@ -30,6 +49,12 @@ class MuJoCoParser(WorldParser):
             filename (str, None): path to the MuJoCo XML file.
         """
         super().__init__(filename)
+        self.simulator = None
+        self.assets = OrderedDict()
+        self.compiler = dict()  # set options for the built-in parser and compiler
+        self.options = dict()   # simulation options
+        self.defaults = dict()  # default values for the attributes when they are not specified
+        self.assets = dict()    # assets (textures, meshes, etc)
 
     def parse(self, filename):
         """
@@ -49,14 +74,71 @@ class MuJoCoParser(WorldParser):
             raise RuntimeError("Expecting the first XML tag to be 'mujoco' but found instead: {}".format(root.tag))
 
         # build the world
-        world = World()
+        world = World(name=root.attrib.get('model', 'world'))
+
+        # check compiler
+        compiler_tag = root.find('compiler')  # TODO: check other
+        if compiler_tag is not None:
+
+            def update_compiler(attributes):
+                for attribute in attributes:
+                    attrib = compiler_tag.attrib.get(attribute)
+                    if attrib is not None:
+                        self.compiler[attribute] = attrib
+
+            coordinate = compiler_tag.attrib.get('coordinate')
+            if coordinate == 'global':
+                raise NotImplementedError("Currently, we only support local coordinate frames.")
+
+            update_compiler(['coordinate', 'angle', 'meshdir', 'texturedir', 'eulerseq', 'discardvisual',
+                             'convexhull', 'inertiafromgeom', 'fitaabb', 'fusestatic'])
 
         # 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
+            def update_default(tag, attributes):
+                tag = default_tag.find(tag)
+                if tag is not None:
+                    self.defaults[tag] = {}
+                    for attribute in attributes:
+                        item = tag.attrib.get(attribute)
+                        if item is not None:
+                            self.defaults['tag'][attribute] = item
+
+            update_default('mesh', ['scale'])
+            update_default('material', ['texture', 'emission', 'specular', 'shininess', 'reflectance', 'rgba'])
+            update_default('joint', ['type', 'pos', 'axis', 'limited', 'range', 'springdamper', 'stiffness',
+                                     'damping', 'frictionloss', 'armature', 'margin', 'ref', 'springref'])
+            update_default('geom', ['type', 'contype', 'conaffinity', 'condim', 'size', 'material', 'rgba',
+                                    'friction', 'mass', 'density', 'margin', 'fromto', 'pos', 'quat', 'axisangle',
+                                    'xyaxes', 'zaxis', 'euler', 'hfield', 'mesh'])
+            update_default('site', ['type', 'material', 'rgba', 'size', 'fromto', 'pos', 'quat', 'axisangle',
+                                    'xyaxes', 'zaxis', 'euler'])
+            update_default('camera', ['mode', 'target', 'fovy', 'ipd', 'pos', 'quat', 'axisangle', 'xyaxes', 'zaxis',
+                                      'euler'])
+            update_default('light', ['mode', 'target', 'directional', 'castshadow', 'active', 'pos', 'dir',
+                                     'attenuation', 'cutoff', 'exponent', 'ambient', 'diffuse', 'specular'])
+            update_default('pair', ['condim', 'friction', 'margin', 'gap'])
+            update_default('equality', ['active'])
+            # update_default('tendon', [''])
+            update_default('general', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+                                       'gear', 'cranklength', 'dyntype', 'gaintype', 'biastype', 'dynprm', 'gainprm',
+                                       'biasprm'])
+            # name, class, joint, jointinparent, site, tendon, slidersite, cranksite
+            update_default('motor', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear',
+                                     'cranklength'])
+            update_default('position', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+                                        'gear', 'cranklength', 'kp'])
+            update_default('velocity', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+                                        'gear', 'cranklength', 'kv'])
+            update_default('cylinder', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange',
+                                        'gear', 'cranklength', 'timeconst', 'area', 'diameter', 'bias'])
+            update_default('muscle', ['ctrllimited', 'ctrlrange', 'forcelimited', 'forcerange', 'lengthrange', 'gear',
+                                      'cranklength', 'timeconst', 'range', 'force', 'scale', 'lmin', 'lmax', 'vmax',
+                                      'fpmax', 'fvmax'])
+
+        # check options
 
         # check assets
         asset_tag = root.find('asset')
@@ -66,7 +148,24 @@ class MuJoCoParser(WorldParser):
         # check world body
         worldbody_tag = root.find('worldbody')
         if worldbody_tag is not None:
-            pass
+
+            # light
+            for i, light_tag in enumerate(worldbody_tag.findall('light')):
+                attrib = light_tag.attrib
+                light = Light(name=attrib.get('name', 'light_' + str(i)), cast_shadows=attrib.get('castshadow'),
+                              position=attrib.get('pos'), direction=attrib.get('dir'), ambient=attrib.get('ambient'),
+                              diffuse=attrib.get('diffuse'), specular=attrib.get('specular'))
+
+                world.lights[light.name] = light
+
+            # check each multi-body
+            for i, body_tag in enumerate(worldbody_tag.findall('body')):
+                # create tree
+                tree = Tree(name=body_tag.attrib.get('name', 'prl_multibody_' + str(i)))
+
+                # check recursively body
+                body = self._check_body(tree, body_tag, idx=i)
+                world.trees[tree.name] = tree
 
         # check contact
 
@@ -79,18 +178,62 @@ class MuJoCoParser(WorldParser):
         # set the world
         self.world = world
 
-    def _check_body(self, body_tag, idx):
+    def _check_model(self, model_tag, idx):
         """
-        Return Body instance from a <body>.
+        Return the Tree instance from a <model>.
 
         Args:
+            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
+
+    def _check_body(self, tree, body_tag, idx):
+        """
+        Construct recursively the given tree, and return Body instance from a <body>.
+
+        Args:
+            tree (Tree): tree data structure containing the model.
             body_tag (ET.Element): body XML element.
             idx (int): link index.
 
         Returns:
             Body: body data structure.
         """
-        pass
+        # create body
+        body = Body(body_id=idx, name=body_tag.attrib.get('name', 'prl_body_' + str(idx)))
+
+        # check geom
+
+        # check joints
+
+        # check bodies
+
+        # check include
 
     def _check_joint(self, joint_tag, idx):
         """
@@ -105,15 +248,32 @@ class MuJoCoParser(WorldParser):
         """
         pass
 
-    def generate(self, tree=None):
+    def generate(self, world=None):
         """
-        Generate the XML tree from the `Tree` data structure.
+        Generate the XML world from the `World` data structure.
 
         Args:
-            tree (Tree): Tree data structure.
+            world (World): world data structure.
 
         Returns:
             ET.Element: root element in the XML file.
         """
-        pass
+        if world is None:
+            world = self.world
 
+        # create root element
+        root = ET.Element('mujoco', attrib={'model': world.name})
+
+        # create compiler
+
+        # create asset
+
+        # create world
+        worldbody_tag = ET.SubElement(root, 'worldbody')  # name = 'world'
+
+        # create models
+        for tree in world.trees:
+
+            # create bodies
+            for i, body in enumerate(tree.bodies):
+                pass

pyrobolearn/utils/parsers/robots/proto_parser.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-"""Define the Proto parser.
+"""Define the Proto parser/generator.
 
 Proto files are notably used in Webots.
 """

pyrobolearn/utils/parsers/robots/sdf_parser.py

@@ -1,7 +1,10 @@
 #!/usr/bin/env python
-"""Define the SDF parser.
+"""Define the SDF parser/generator.
 
 SDF files are notably used in Gazebo, and Bullet.
+
+References:
+    - SDF file format: http://sdformat.org/
 """
 
 # import XML parser
@@ -32,7 +35,6 @@ class SDFParser(WorldParser):
             filename (str, None): path to the SDF file.
         """
         super().__init__(filename)
-        self.worlds = []
 
     def parse(self, filename):
         """
@@ -56,8 +58,26 @@ class SDFParser(WorldParser):
             # build the world
             world = World(name=world_tag.attrib.get('name', 'world_' + str(i)))
 
+            # check light
+            for l, light_tag in enumerate(world_tag.findall('light')):
+                attrib = light_tag.attrib
+                light = Light(name=attrib.get('name', 'light_' + str(l)), dtype=attrib.get('type'))
+
+                for tag in ['cast_shadows', 'diffuse', 'specular', 'direction']:
+                    item_tag = light_tag.find(tag)
+                    if item_tag is not None:
+                        if hasattr(light, tag):
+                            setattr(light, tag, item_tag.text)
+
+                pose_tag = light_tag.find('pose')
+                if pose_tag is not None:
+                    light.pose = pose_tag.text
+
+                # add the light in the world
+                world.lights[light.name] = light
+
             # check model
-            for idx, model_tag in enumerate(root.findall('model')):
+            for idx, model_tag in enumerate(world_tag.findall('model')):
                 tree = self._check_model(model_tag, idx=idx)
                 world.trees[tree.name] = tree
 
@@ -76,6 +96,9 @@ class SDFParser(WorldParser):
         if len(models) > 0:
             self.worlds.append(world)
 
+        # set the first world
+        self.world = self.worlds[0]
+
     def _check_model(self, model_tag, idx):
         """
         Return the Tree instance from a <model>.
@@ -88,7 +111,7 @@ class SDFParser(WorldParser):
             Tree: tree data structure containing the model.
         """
         # create tree
-        tree = Tree(name=model_tag.attrib.get('name'))
+        tree = Tree(name=model_tag.attrib.get('name', 'model_' + str(idx)))
 
         # check bodies/links
         for i, link_tag in enumerate(model_tag.findall('link')):
@@ -97,7 +120,7 @@ class SDFParser(WorldParser):
             tree.bodies[body.name] = body
 
         # check joints
-        for i, joint_tag in enumerate(root.findall('joint')):
+        for i, joint_tag in enumerate(model_tag.findall('joint')):
             # get joint instance from tag
             joint = self._check_joint(joint_tag, idx=i)
 
@@ -203,7 +226,7 @@ class SDFParser(WorldParser):
                             visual.filename = uri_tag.text
                             visual.size = scale_tag.text
 
-            # material
+            # material  # TODO
             # material = visual.find('material')
             # if material is not None:
             #     name = material.attrib.get('name')
@@ -329,6 +352,11 @@ class SDFParser(WorldParser):
                 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]
 
+            # init_position
+            init_pos_tag = axis_tag.find('init_position')
+            if init_pos_tag is not None:
+                joint.init_position = init_pos_tag.text
+
         return joint
 
     def generate(self, world=None):
@@ -351,20 +379,221 @@ class SDFParser(WorldParser):
         name = world.name if world.name is not None else 'default'
         world_tag = ET.SubElement(root, 'world', attrib={'name': name})
 
+        def set_pose(parent_tag, item):
+            if item.position is None and item.orientation is None:
+                return None
+            position = [0., 0., 0.] if item.position is None else item.position
+            orientation = [0., 0., 0.] if item.orientation is None else item.orientation
+            pose = np.concatenate((position, orientation))
+            pose_tag = ET.SubElement(parent_tag, 'pose')
+            pose_tag.text = str(pose)[1:-1]
+
+        def set_geometry(parent_tag, item):
+            if item.geometry.dtype is not None:
+                geometry = item.geometry
+                dtype = geometry.dtype
+                geometry_tag = ET.SubElement(parent_tag, 'geometry')
+
+                if dtype in {'box', 'sphere', 'cylinder', 'mesh'}:
+                    dtype_tag = ET.SubElement(geometry_tag, dtype)
+                    if dtype == 'box':
+                        size_tag = ET.SubElement(dtype_tag, 'size')
+                        size_tag.text = str(np.asarray(geometry.size))[1:-1]
+                    elif dtype == 'sphere':
+                        radius_tag = ET.SubElement(dtype_tag, 'radius')
+                        radius_tag.text = str(geometry.size)
+                    elif dtype == 'cylinder':
+                        radius_tag = ET.SubElement(dtype_tag, 'radius')
+                        length_tag = ET.SubElement(dtype_tag, 'length')
+                        radius_tag.text = str(geometry.size[0])
+                        length_tag.text = str(geometry.size[1])
+                    elif dtype == 'mesh':
+                        uri_tag = ET.SubElement(dtype_tag, 'uri')
+                        uri_tag.text = geometry.filename
+                        if geometry.size is not None:
+                            scale_tag = ET.SubElement(dtype_tag, 'scale')
+                            scale_tag.text = str(np.asarray())[1:-1]
+                    # elif dtype == 'plane':
+                    #     pass
+                    # elif dtype == 'heightmap':
+                    #     pass
+
         # 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]
+
+            # pose
+            set_pose(model_tag, tree)
 
             # create links
-            for body in tree.bodies:  # TODO
-                pass
+            for i, body in enumerate(tree.bodies):
+                link_tag = ET.SubElement(model_tag, 'link', attrib={'name': body.name})
+
+                # create inertial
+                if body.inertial is not None:
+                    inertial = body.inertial
+                    inertial_tag = ET.SubElement(link_tag, 'inertial')
+
+                    # pose
+                    set_pose(inertial_tag, inertial)
+
+                    # mass
+                    if inertial.mass is not None:
+                        mass_tag = ET.SubElement(inertial_tag, 'mass')
+                        mass_tag.text = str(inertial.mass)
+
+                    # inertia
+                    if inertial.inertia is not None:
+                        inertia = inertial.inertia
+                        inertia_tag = ET.SubElement(inertial_tag, 'inertia')
+
+                        if inertia.ixx is not None:
+                            ixx_tag = ET.SubElement(inertia_tag, 'ixx')
+                            ixx_tag.text = str(inertia.ixx)
+                        if inertia.ixy is not None:
+                            ixy_tag = ET.SubElement(inertia_tag, 'ixy')
+                            ixy_tag.text = str(inertia.ixy)
+                        if inertia.ixz is not None:
+                            ixz_tag = ET.SubElement(inertia_tag, 'ixz')
+                            ixz_tag.text = str(inertia.ixz)
+                        if inertia.iyy is not None:
+                            iyy_tag = ET.SubElement(inertia_tag, 'iyy')
+                            iyy_tag.text = str(inertia.iyy)
+                        if inertia.iyz is not None:
+                            iyz_tag = ET.SubElement(inertia_tag, 'iyz')
+                            iyz_tag.text = str(inertia.iyz)
+                        if inertia.izz is not None:
+                            izz_tag = ET.SubElement(inertia_tag, 'izz')
+                            izz_tag.text = str(inertia.izz)
+
+                # create visual
+                if body.visual is not None:
+                    visual = body.visual
+                    name = 'visual_' + str(i) if visual.name is None else visual.name
+                    visual_tag = ET.SubElement(link_tag, 'visual', attrib={'name': name})
+
+                    # pose
+                    set_pose(visual_tag, visual)
+
+                    # material
+                    if visual.material is not None:
+                        material = visual.material
+                        material_tag = ET.SubElement(visual_tag, 'material')
+
+                        if material.color is not None:
+                            ambient_tag = ET.SubElement(material_tag, 'ambient')
+                            ambient_tag.text = str(np.asarray(material.rgba))[1:-1]
+
+                        if material.diffuse is not None:
+                            diffuse_tag = ET.SubElement(material_tag, 'diffuse')
+                            diffuse_tag.text = str(np.asarray(material.diffuse))[1:-1]
+
+                        if material.specular is not None:
+                            specular_tag = ET.SubElement(material_tag, 'specular')
+                            specular_tag.text = str(np.asarray(material.specular))[1:-1]
+
+                        if material.emissive is not None:
+                            emissive_tag = ET.SubElement(material_tag, 'emissive')
+                            emissive_tag.text = str(np.asarray(material.emissive))[1:-1]
+
+                        if material.texture is not None:
+                            script_tag = ET.SubElement(material_tag, 'script')
+                            name = 'material_' + str(i) if material.name is None else material.name
+                            name_tag = ET.SubElement(script_tag, 'name')
+                            name_tag.text = name
+
+                            # create Ogre script
+                            with open(name + '.material', "w") as f:
+                                s = "material {}" \
+                                    "\n{" \
+                                    "\n\ttechnique" \
+                                    "\n\t{" \
+                                    "\n\t\tpass" \
+                                    "\n\t\t{" \
+                                    "\n\t\t\ttexture_unit" \
+                                    "\n\t\t\t{" \
+                                    "\n\t\t\t\ttexture {}" \
+                                    "\n\t\t\t}" \
+                                    "\n\t\t}" \
+                                    "\n\t}" \
+                                    "\n}".format(name, material.texture)
+                                f.write(s)
+
+                            uri_tag = ET.SubElement(script_tag, 'uri')
+                            uri_tag.text = "file://" + name + '.material'
+
+                    # geometry
+                    set_geometry(visual_tag, visual)
+
+                # create collision
+                if body.collision is not None:
+                    collision = body.collision
+                    name = 'collision_' + str(i) if collision.name is None else collision.name
+                    collision_tag = ET.SubElement(link_tag, 'collision', attrib={'name': name})
+
+                    # pose
+                    set_pose(collision_tag, collision)
+
+                    # geometry
+                    set_geometry(collision_tag, collision)
 
             # create joints
-            for joint in tree.joints:  # TODO
-                pass
+            for i, joint in enumerate(tree.joints):
+                joint_tag = ET.SubElement(model_tag, 'joint', attrib={'name': joint.name, 'type': joint.dtype})
+                parent_tag = ET.SubElement(joint_tag, 'parent')
+                parent_tag.text = joint.parent
+                child_tag = ET.SubElement(joint_tag, 'child')
+                child_tag.text = joint.child
+
+                # pose
+                set_pose(joint_tag, joint)
+
+                # axis
+                axis_tag = None
+                if joint.axis is not None:
+                    axis_tag = ET.SubElement(joint_tag, 'axis')
+                    xyz_tag = ET.SubElement(axis_tag, 'xyz')
+                    xyz_tag.text = str(np.asarray(joint.axis))[1:-1]
+
+                # dynamics
+                if joint.damping is not None or joint.friction is not None:
+                    if axis_tag is None:
+                        axis_tag = ET.SubElement(joint_tag, 'axis')
+                    dynamics_tag = ET.SubElement(axis_tag, 'dynamics')
+
+                    if joint.friction is not None:
+                        friction_tag = ET.SubElement(dynamics_tag, 'friction')
+                        friction_tag.text = str(joint.friction)
+                    if joint.damping is not None:
+                        damping_tag = ET.SubElement(dynamics_tag, 'damping')
+                        damping_tag.text = str(joint.damping)
+
+                # limits
+                if joint.limits is not None or joint.effort is not None or joint.velocity is not None:
+                    if axis_tag is None:
+                        axis_tag = ET.SubElement(joint_tag, 'axis')
+                    limit_tag = ET.SubElement(axis_tag, 'limit')
+
+                    if joint.limits is not None:
+                        lower_tag = ET.SubElement(limit_tag, 'lower')
+                        upper_tag = ET.SubElement(limit_tag, 'upper')
+                        lower_tag.text = str(joint.limits[0])
+                        upper_tag.text = str(joint.limits[1])
+
+                    if joint.velocity is not None:
+                        velocity_tag = ET.SubElement(limit_tag, 'velocity')
+                        velocity_tag.text = str(joint.velocity)
+
+                    if joint.effort is not None:
+                        effort_tag = ET.SubElement(limit_tag, 'effort')
+                        effort_tag.text = str(joint.effort)
+
+                # init_position
+                if isinstance(joint.init_position, (float, int)):
+                    if axis_tag is None:
+                        axis_tag = ET.SubElement(joint_tag, 'axis')
+                    initial_pos_tag = ET.SubElement(axis_tag, 'initial_position')
+                    initial_pos_tag.text = str(joint.init_position)
 
         # return root XML element
         return root

pyrobolearn/utils/parsers/robots/skel_parser.py

@@ -0,0 +1,554 @@
+#!/usr/bin/env python
+"""Define the Skel parser/generator.
+
+Skel files are notably used in Dart.
+
+References:
+    - SKEL file format: https://dartsim.github.io/skel_file_format.html
+        - Note: the specification is incomplete on the above website, check the examples in the next link.
+    - Examples of Skeleton: https://github.com/dartsim/dart/tree/master/data/skel
+"""
+
+# import XML parser
+import xml.etree.ElementTree as ET
+
+from pyrobolearn.utils.parsers.robots.world_parser import WorldParser
+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 SkelParser(WorldParser):
+    r"""Skel Parser and Generator"""
+
+    def __init__(self, filename=None):
+        """
+        Initialize the Skel parser.
+
+        Args:
+            filename (str, None): path to the Skel file.
+        """
+        super().__init__(filename)
+        self.worlds = []
+
+    def parse(self, filename):
+        """
+        Load and parse the given Skel file.
+
+        Args:
+            filename (str): path to the Skel file.
+        """
+        # load and parse the XML file
+        tree_xml = ET.parse(filename)
+
+        # get the root
+        root = tree_xml.getroot()
+
+        # check that the root is <skel>
+        if root.tag != 'skel':
+            raise RuntimeError("Expecting the first XML tag to be 'skel' but found instead: {}".format(root.tag))
+
+        # 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)))
+
+            # check physics
+            physics_tag = world_tag.find('physics')
+            physics = None
+            if physics_tag is not None:
+                physics = Physics()
+                timestep_tag = physics_tag.find('time_step')
+                if timestep_tag is not None:
+                    physics.timestep = timestep_tag.text
+                gravity_tag = physics_tag.find('gravity')
+                if gravity_tag is not None:
+                    physics.gravity = gravity_tag.text  # TODO: check the convention that DART uses; g = (0, -9.81, 0)?
+                # TODO: collision collector
+
+            if physics is not None:
+                world.physics = physics
+
+            # check skeleton
+            for idx, skeleton_tag in enumerate(world_tag.findall('skeleton')):
+                tree = self._check_skeleton(skeleton_tag, idx=idx)
+                world.trees[tree.name] = tree
+
+            # append the world to the list of worlds
+            self.worlds.append(world)
+
+        # set the first world
+        self.world = self.worlds[0]
+
+    def _check_skeleton(self, skeleton_tag, idx):
+        """
+        Return the Tree instance from a <skeleton>.
+
+        Args:
+            skeleton_tag (ET.Element): skeleton XML element
+            idx (int): skeleton index.
+
+        Returns:
+            Tree: tree data structure containing the skeleton.
+        """
+        # create tree
+        tree = Tree(name=skeleton_tag.attrib.get('name', 'skeleton_' + str(idx)))
+
+        # check bodies/links
+        for i, body_tag in enumerate(skeleton_tag.findall('body')):
+            body = self._check_body(body_tag, idx=i)
+            # add body to tree
+            tree.bodies[body.name] = body
+
+        # check joints
+        for i, joint_tag in enumerate(skeleton_tag.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 <body>.
+
+        Args:
+            body_tag (ET.Element): body XML element.
+            idx (int): body index.
+
+        Returns:
+            Body: body data structure.
+        """
+        # create body/link
+        body = Body(body_id=idx, name=body_tag.attrib.get('name', 'body_' + str(idx)))
+
+        # check <inertia> tag
+        inertial_tag = body_tag.find('inertia')
+        if inertial_tag is not None:
+            inertial = Inertial()
+
+            # transformation
+            pose_tag = inertial_tag.find('transformation')
+            if pose_tag is not None:
+                inertial.pose = pose_tag.text
+
+            # offset
+            offset_tag = inertial_tag.find('offset')
+            if offset_tag is not None:
+                inertial.position = offset_tag.text
+
+            # mass
+            mass_tag = inertial_tag.find('mass')
+            if mass_tag is not None:
+                inertial.mass = mass_tag.text
+
+            # moment_of_inertia
+            inertia_tag = inertial_tag.find('moment_of_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 <visualization_shape> tag
+        visual_tag = body_tag.find('visualization_shape')
+        if visual_tag is not None:
+            visual = Visual()
+
+            # name
+            visual.name = visual_tag.attrib.get('name')
+
+            # transformation
+            pose_tag = visual_tag.find('transformation')
+            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
+
+            # set visual to body
+            body.visual = visual
+
+        # check <collision_shape> tag
+        collision_tag = body_tag.find('collision_shape')
+        if collision_tag is not None:
+            collision = Collision()
+
+            # name
+            # collision.name = collision_tag.attrib.get('name')
+
+            # origin
+            pose_tag = collision_tag.find('transformation')
+            if pose_tag is not None:
+                collision.pose = pose_tag.text
+
+            # geometry
+            geometry_tag = collision_tag.find('geometry')
+            if geometry_tag is not None:
+                geometry_types = ['box', 'capsule', 'cone', 'cylinder', 'ellipsoid', 'mesh', 'sphere']  # multi_sphere
+                for geometry_type in geometry_types:
+                    geometry_type_tag = geometry_tag.find(geometry_type)
+                    if geometry_type_tag is not None:
+                        dtype = geometry_type
+                        collision.dtype = dtype
+                        if dtype == 'box' or dtype == 'ellipsoid':
+                            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' or dtype == 'capsule' or dtype == 'cone':
+                            radius_tag = geometry_type_tag.find('radius')
+                            height_tag = geometry_type_tag.find('height')
+                            collision.size = (radius_tag.text, height_tag.text)
+                        elif dtype == 'mesh':
+                            filename_tag = geometry_type_tag.find('file_name')
+                            scale_tag = geometry_type_tag.find('scale')
+                            collision.filename = filename_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
+
+        # transformation
+        pose_tag = joint_tag.find('transformation')
+        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]
+
+        # init_pos
+        init_pos_tag = joint_tag.find('init_pos')
+        if init_pos_tag is not None:
+            joint.init_position = init_pos_tag.text
+
+        # init_vel
+        init_vel_tag = joint_tag.find('init_vel')
+        if init_vel_tag is not None:
+            joint.init_velocity = init_vel_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.
+        """
+        if world is None:
+            world = self.worlds[0]
+
+        # create root element
+        root = ET.Element('skel', attrib={'version': '1.0'})
+
+        # create world tag
+        name = world.name if world.name is not None else 'world'
+        world_tag = ET.SubElement(root, 'world', attrib={'name': name})
+
+        def set_transformation(parent_tag, item):
+            if item.position is None and item.orientation is None:
+                return None
+            position = [0., 0., 0.] if item.position is None else item.position
+            orientation = [0., 0., 0.] if item.orientation is None else item.orientation
+            pose = np.concatenate((position, orientation))
+            pose_tag = ET.SubElement(parent_tag, 'transformation')
+            pose_tag.text = str(pose)[1:-1]
+
+        def set_geometry(parent_tag, item):
+            if item.geometry.dtype is not None:
+                geometry = item.geometry
+                dtype = geometry.dtype
+                geometry_tag = ET.SubElement(parent_tag, 'geometry')
+
+                if dtype in {'box', 'capsule', 'cone', 'cylinder', 'ellipsoid', 'mesh', 'sphere'}:
+                    dtype_tag = ET.SubElement(geometry_tag, dtype)
+                    if dtype == 'box' or dtype == 'ellipsoid':
+                        size_tag = ET.SubElement(dtype_tag, 'size')
+                        size_tag.text = str(np.asarray(geometry.size))[1:-1]
+                    elif dtype == 'sphere':
+                        radius_tag = ET.SubElement(dtype_tag, 'radius')
+                        radius_tag.text = str(geometry.size)
+                    elif dtype == 'cylinder' or dtype == 'capsule' or dtype == 'cone':
+                        radius_tag = ET.SubElement(dtype_tag, 'radius')
+                        height_tag = ET.SubElement(dtype_tag, 'height')
+                        radius_tag.text = str(geometry.size[0])
+                        height_tag.text = str(geometry.size[1])
+                    elif dtype == 'mesh':
+                        filename_tag = ET.SubElement(dtype_tag, 'file_name')
+                        filename_tag.text = geometry.filename
+                        if geometry.size is not None:
+                            scale_tag = ET.SubElement(dtype_tag, 'scale')
+                            scale_tag.text = str(np.asarray())[1:-1]
+                    # elif dtype == 'plane':
+                    #     pass
+                    # elif dtype == 'heightmap':
+                    #     pass
+
+        # create skeletons
+        for tree in world.trees:
+            skeleton_tag = ET.SubElement(world_tag, 'skeleton', attrib={'name': tree.name})
+
+            # transformation
+            set_transformation(skeleton_tag, tree)
+
+            # create bodies/links
+            for i, body in enumerate(tree.bodies):
+                body_tag = ET.SubElement(skeleton_tag, 'body', attrib={'name': body.name})
+
+                # create inertial
+                if body.inertial is not None:
+                    inertial = body.inertial
+                    inertial_tag = ET.SubElement(body_tag, 'inertia')
+
+                    # offset
+                    if inertial.position is not None:
+                        if inertial.orientation is not None:
+                            set_transformation(inertial_tag, inertial)
+                        else:
+                            offset_tag = ET.SubElement(inertial_tag, 'offset')
+                            offset_tag.text = str(np.asarray(inertial.position))[1:-1]
+
+                    # mass
+                    if inertial.mass is not None:
+                        mass_tag = ET.SubElement(inertial_tag, 'mass')
+                        mass_tag.text = str(inertial.mass)
+
+                    # inertia
+                    if inertial.inertia is not None:
+                        inertia = inertial.inertia
+                        inertia_tag = ET.SubElement(inertial_tag, 'moment_of_inertia')
+
+                        if inertia.ixx is not None:
+                            ixx_tag = ET.SubElement(inertia_tag, 'ixx')
+                            ixx_tag.text = str(inertia.ixx)
+                        if inertia.ixy is not None:
+                            ixy_tag = ET.SubElement(inertia_tag, 'ixy')
+                            ixy_tag.text = str(inertia.ixy)
+                        if inertia.ixz is not None:
+                            ixz_tag = ET.SubElement(inertia_tag, 'ixz')
+                            ixz_tag.text = str(inertia.ixz)
+                        if inertia.iyy is not None:
+                            iyy_tag = ET.SubElement(inertia_tag, 'iyy')
+                            iyy_tag.text = str(inertia.iyy)
+                        if inertia.iyz is not None:
+                            iyz_tag = ET.SubElement(inertia_tag, 'iyz')
+                            iyz_tag.text = str(inertia.iyz)
+                        if inertia.izz is not None:
+                            izz_tag = ET.SubElement(inertia_tag, 'izz')
+                            izz_tag.text = str(inertia.izz)
+
+                # create visual
+                if body.visual is not None:
+                    visual = body.visual
+                    # name = 'visual_' + str(i) if visual.name is None else visual.name
+                    visual_tag = ET.SubElement(body_tag, 'visualization_shape')  # attrib={'name': name})
+
+                    # transformation
+                    set_transformation(visual_tag, visual)
+
+                    # geometry
+                    set_geometry(visual_tag, visual)
+
+                # create collision
+                if body.collision is not None:
+                    collision = body.collision
+                    # name = 'collision_' + str(i) if collision.name is None else collision.name
+                    collision_tag = ET.SubElement(body_tag, 'collision_shape')  # , attrib={'name': name})
+
+                    # transformation
+                    set_transformation(collision_tag, collision)
+
+                    # geometry
+                    set_geometry(collision_tag, collision)
+
+            # create joints
+            for i, joint in enumerate(tree.joints):
+                joint_tag = ET.SubElement(skeleton_tag, 'joint', attrib={'name': joint.name, 'type': joint.dtype})
+                parent_tag = ET.SubElement(joint_tag, 'parent')
+                parent_tag.text = joint.parent
+                child_tag = ET.SubElement(joint_tag, 'child')
+                child_tag.text = joint.child
+
+                # transformation
+                set_transformation(joint_tag, joint)
+
+                # axis
+                axis_tag = None
+                if joint.axis is not None:
+                    axis_tag = ET.SubElement(joint_tag, 'axis')
+                    xyz_tag = ET.SubElement(axis_tag, 'xyz')
+                    xyz_tag.text = str(np.asarray(joint.axis))[1:-1]
+
+                # dynamics
+                if joint.damping is not None or joint.friction is not None:
+                    if axis_tag is None:
+                        axis_tag = ET.SubElement(joint_tag, 'axis')
+                    dynamics_tag = ET.SubElement(axis_tag, 'dynamics')
+
+                    if joint.friction is not None:
+                        friction_tag = ET.SubElement(dynamics_tag, 'friction')
+                        friction_tag.text = str(joint.friction)
+                    if joint.damping is not None:
+                        damping_tag = ET.SubElement(dynamics_tag, 'damping')
+                        damping_tag.text = str(joint.damping)
+
+                # limits
+                if joint.limits is not None or joint.effort is not None or joint.velocity is not None:
+                    if axis_tag is None:
+                        axis_tag = ET.SubElement(joint_tag, 'axis')
+                    limit_tag = ET.SubElement(axis_tag, 'limit')
+
+                    if joint.limits is not None:
+                        lower_tag = ET.SubElement(limit_tag, 'lower')
+                        upper_tag = ET.SubElement(limit_tag, 'upper')
+                        lower_tag.text = str(joint.limits[0])
+                        upper_tag.text = str(joint.limits[1])
+
+                    if joint.velocity is not None:
+                        velocity_tag = ET.SubElement(limit_tag, 'velocity')
+                        velocity_tag.text = str(joint.velocity)
+
+                    if joint.effort is not None:
+                        effort_tag = ET.SubElement(limit_tag, 'effort')
+                        effort_tag.text = str(joint.effort)
+
+                # init_pos
+                if joint.init_position is not None:
+                    init_pos_tag = ET.SubElement(joint_tag, 'init_pos')
+                    if isinstance(joint.init_position, (float, int)):
+                        init_pos_tag.text = str(joint.init_position)
+                    else:
+                        init_pos_tag.text = str(np.asarray(joint.init_position))[1:-1]
+
+                # init_vel
+                if joint.init_velocity is not None:
+                    init_vel_tag = ET.SubElement(joint_tag, 'init_vel')
+                    if isinstance(joint.init_velocity, (float, int)):
+                        init_vel_tag.text = str(joint.init_velocity)
+                    else:
+                        init_vel_tag.text = str(np.asarray(joint.init_velocity))[1:-1]
+
+        # return root XML element
+        return root

pyrobolearn/utils/parsers/robots/urdf_parser.py

@@ -1,7 +1,11 @@
 #!/usr/bin/env python
-"""Define the URDF parser.
+"""Define the URDF parser/generator.
 
 URDF files are notably used in ROS, Gazebo, Bullet, Dart, and MuJoCo.
+
+References:
+    - URDF XML specifications: http://wiki.ros.org/urdf/XML
+    - Tutorial: Using a URDF in Gazebo: http://gazebosim.org/tutorials/?tut=ros_urdf
 """
 
 # import XML parser
@@ -148,7 +152,7 @@ class URDFParser(RobotParser):
             # geometry
             geometry_tag = visual_tag.find('geometry')
             if geometry_tag is not None:
-                for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+                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
@@ -200,7 +204,7 @@ class URDFParser(RobotParser):
             # geometry
             geometry_tag = collision_tag.find('geometry')
             if geometry_tag is not None:
-                for geometry_type in ['box', 'mesh', 'cylinder', 'sphere']:
+                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

pyrobolearn/utils/parsers/robots/world_parser.py

@@ -30,6 +30,7 @@ class WorldParser(object):
         """
         self.root = None
         self.world = None
+        self.worlds = []
         self.filename = filename
         if filename is not None:
             self.parse(filename)