update parsers/simulators + add simulator example

2026-09-09 11:31:38 +08:00 · 2019-09-10 10:23:36 +02:00
parent 79b2e6a9d0
commit 40f01cd6e8
29 changed files with 2129 additions and 319 deletions
@@ -22,6 +22,9 @@ joint position values that were returned by the Bullet simulator on the correspo
 values from the ROS topics and change them in the simulator. This works with the `bullet_ros_publisher.py` code 
 presented above. By moving the robot with your mouse in the publisher version, you will see the robot in this 
 subscriber version moves in accordance with. This can be useful if you have access to the real platform as well.
+4. `simulators.py`: example which loads few primitive shapes and the ANYmal robot using a simulator among `Bullet`, 
+`Mujoco`, `Raisim`, and `Dart`. Only the line `sim = <SimulatorName>(render=True)` needs to be changed. Note that 
+the integration of these other simulators is ongoing.

 Later, a `ROS`/`ROS_RBDL` "simulator" (without passing by a real simulator like `Bullet`) will allow you to make 
 your code works on a real platform using ROS without changing any other lines of code. This is one of the big 
@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+"""Simulator tests.
+
+Example on how to load different things with the simulators. This example is still in an experimental phase. For
+now, only Bullet is fully-supported. We are working on the other ones, especially the Mujoco simulator.
+- Bullet: OK
+- Raisim: OK  (todo: for collision bodies, it only accepts OBJ files)
+- MuJoCo: OK  (todo: control still missing)
+- DART: OK, but capsules don't have collision shapes... (todo: fix some URDFs)
+- VREP: Not implemented yet + problem when importing PyRep with pybullet. Also, need to figure out how to call the
+  'loadURDF' plugin.
+- Isaac: not available yet.
+"""
+
+import os
+from itertools import count
+
+from pyrobolearn.simulators.bullet import Bullet
+from pyrobolearn.simulators.raisim import Raisim
+from pyrobolearn.simulators.dart import Dart
+from pyrobolearn.simulators.mujoco import Mujoco
+# from pyrobolearn.simulators.vrep import VREP  # Problem when importing PyRep with Pybullet
+# from pyrobolearn.simulators.isaac import Isaac  # Not available yet
+
+
+sim = Bullet(render=True)
+# sim = Raisim(render=True)
+# sim = Dart(render=True)
+# sim = Mujoco(render=True)
+# sim = VREP(render=True)
+# sim = Isaac(render=True)
+print("Gravity: {}".format(sim.get_gravity()))
+
+# load floor
+floor = sim.load_floor(dimension=20)
+
+# create box
+box = sim.create_primitive_object(sim.GEOM_BOX, position=(0, 0, 2), mass=1, rgba_color=(1, 0, 0, 1))
+sphere = sim.create_primitive_object(sim.GEOM_SPHERE, position=(2, 2, 2), mass=1, rgba_color=(0, 1, 0, 1))
+cylinder = sim.create_primitive_object(sim.GEOM_CYLINDER, position=(0, 2, 2), mass=1)
+capsule = sim.create_primitive_object(sim.GEOM_CAPSULE, position=(0, -2, 2), mass=1, rgba_color=(0, 0, 1, 1),
+                                      radius=0.5, height=0.5)
+
+# load robot
+urdf_path = os.path.dirname(os.path.abspath(__file__)) + '/../../pyrobolearn/robots/urdfs/'
+# path = urdf_path + 'rrbot/rrbot.urdf'
+# path = urdf_path + 'jaco/jaco.urdf'
+# path = urdf_path + 'kuka/kuka_iiwa/iiwa14.urdf'
+# path = urdf_path + 'hyq2max/hyq2max.urdf'
+path = urdf_path + 'anymal/anymal.urdf'
+# path = urdf_path + 'centauro/centauro_stick.urdf'
+
+robot = sim.load_urdf(path, position=(3, -3, 2), use_fixed_base=False)
+
+# perform step
+for t in count():
+    sim.step(sleep_time=sim.dt)
@@ -76,6 +76,13 @@ class Centauro(WheeledRobot, QuadrupedRobot, BiManipulator):

        self.hands = [self.get_link_ids(link) for link in ['arm1_8', 'arm2_8']]

+        # load joint configurations
+        srdf = os.path.dirname(__file__) + '/urdfs/centauro/centauro.srdf'
+        self.load_joint_configurations(srdf)
+        # print(self._joint_configuration.keys())
+        joint_ids, joint_values = self._joint_configuration['home']
+        self.reset_joint_states(q=joint_values, joint_ids=joint_ids)
+

 # Test
 if __name__ == "__main__":
@@ -97,8 +104,8 @@ if __name__ == "__main__":
    print("Number of Legs: {}".format(robot.num_legs))
    print("Number of Arms: {}".format(robot.num_arms))

-    robot.add_joint_slider(robot.right_front_leg)
-    robot.drive(speed=3)
+    # robot.add_joint_slider(robot.left_arm)
+    # robot.drive(speed=3)

    # run simulator
    for _ in count():
@@ -14,6 +14,7 @@ import os
 import time
 import copy
 import collections
+import xml.etree.ElementTree as ET
 # import rbdl
 import numpy as np
 # import quaternion
@@ -59,7 +60,7 @@ class Robot(ControllableBody):
    """

    def __init__(self, simulator, urdf, position=None, orientation=None, fixed_base=False, scale=1., visual_ticks=12,
-                 *args, **kwargs):
+                 parts=None, *args, **kwargs):
        """
        Initialize the robot.

@@ -71,6 +72,7 @@ class Robot(ControllableBody):
            fixed_base (bool, None): if True, the base of the robot will be fixed.
            scale (float): scaling factor.
            visual_ticks (int): the number of ticks to sleep before updating the visuals.
+            parts (list[Robot], None): robotic parts to assemble.  # TODO: this needs to be implemented
        """
        # check parameters
        if position is None:
@@ -1431,6 +1433,10 @@ class Robot(ControllableBody):
        Return the joint positions for the home position defined by the user. This method has to be overwritten in
        the child class.
        """
+        if 'home' in self._joint_configuration:
+            joint_ids, joint_values = self._joint_configuration['home']
+            if len(joint_ids) == self.num_actuated_joints:
+                return joint_values
        return np.zeros(self.num_actuated_joints)

    def set_home_joint_positions(self):
@@ -1476,7 +1482,7 @@ class Robot(ControllableBody):
        if name is None:
            return list(self._joint_configuration.keys())
        if name in self._joint_configuration:
-            item = self._joint_configuration[name]
+            item = self._joint_configuration[name]  # name.lower()
            if isinstance(item, str):  # the item is an alias
                return self._joint_configuration[item]
            return item
@@ -1495,6 +1501,32 @@ class Robot(ControllableBody):
        """
        return name in self._joint_configuration

+    def load_joint_configurations(self, srdf):
+        """
+        Load the joint configurations that are defined in the given SRDF file.
+
+        Args:
+            srdf (str): path to the SRDF file which contains joint configurations with their corresponding name.
+        """
+        if isinstance(srdf, str) and os.path.isfile(srdf):
+            tree_xml = ET.parse(srdf)
+            root = tree_xml.getroot()
+
+            # parse <group_state> tags
+            for group_state_tag in root.findall('group_state'):
+                name = group_state_tag.attrib['name'].lower()
+
+                # parse each <joint>
+                joint_ids, joint_values = [], []
+                for joint_tag in group_state_tag.findall('joint'):
+                    values = [float(c) for c in joint_tag.attrib['value'].split()]
+                    values = values[0] if len(values) == 1 else np.array(values)
+                    joint_name = joint_tag.attrib['name']
+                    joint_ids.append(self.get_joint_ids(joint_name))
+                    joint_values.append(values)
+
+                self._joint_configuration[name] = [joint_ids, np.asarray(joint_values)]
+
    ##################################
    # Links (task/operational space) #
    ##################################
@@ -2643,7 +2675,7 @@ class Robot(ControllableBody):
        Returns:
            np.array[float[4,4]],4]: homogeneous matrix
        """
-        return get_homogeneous_transform(position, orientation)
+        return get_homogeneous_matrix(position, orientation)

    ##############
    # Kinematics #
@@ -96,7 +96,7 @@ The coordinate frame called gaze defines the head position and orientation. The
 and to define the end effectors of the robot

 The coordinate frame called base_link is rigidly attached to the robot root body. It is recommended to choose the robot waist as its root body. The base_link can be attached to the root in any arbitrary position or orientation; for every hardware platform there will be a different place on the base that provides an obvious point of reference. Note that REP 103 [1] specifies a preferred orientation for frames. -->
-  <!--link name="base_link"> <!-- if you put mass/inertia to 0 for the base_link in pybullet then the robot will not be affected by gravity (i.e. it will float). Just removing the base_link fixes the problem.>
+  <!--link name="base_link"--> <!-- if you put mass/inertia to 0 for the base_link in pybullet then the robot will not be affected by gravity (i.e. it will float). Just removing the base_link fixes the problem.>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <mass value="0.0000001"/>
@@ -15,11 +15,11 @@
  <!-- One servo has a weight of approximately 0.055kg -->
  <link name="thorax">
    <inertial> <!-- these inertia are calculated based on the visual meshes using Meshlab, and using a density of 1kg/m^3 (http://gazebosim.org/tutorials?tut=inertia) -->
-      <!--mass value="1"/> <!-- volume = 0.061867e-3 -->
+      <!--mass value="1"/--> <!-- volume = 0.061867e-3 -->
      <mass value="0.16765957"/> <!-- volume = 0.061867e-3m^3, density = 2,710kg/m^3 -->
      <origin xyz="0.000137 0.000006 0.002405"/>
      <!--inertia ixx="0.011935e-5" ixy="0.0" ixz="-0.000021e-5" iyy="0.022562e-5" iyz="-0.000001e-5" izz="0.026533e-5"/-->
-      <!--inertia ixx="0.00192913831" ixy="0.0" ixz="-0.00000339437" iyy="0.00364685535" iyz="-1.6163706e-7" izz="0.00428871611"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00192913831" ixy="0.0" ixz="-0.00000339437" iyy="0.00364685535" iyz="-1.6163706e-7" izz="0.00428871611"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00032343849" ixy="0.0" ixz="-5.69098615e-7" iyy="0.00061143019" iyz="-2.71e-8" izz="0.00071904429"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -66,7 +66,7 @@
      <mass value="0.120"/> <!-- volume = 0.033302e-3, mass of 2 servos = 0,110kg + mass of a little bit of alloy = 0.010kg (approx) -->
      <origin xyz="0.014769 -0.007977 0.004815"/>
      <!--inertia ixx="0.001380e-5" ixy="-0.000093e-5" ixz="-0.000177e-5" iyy="0.001442e-5" iyz="-0.000030e-5" izz="0.001371e-5"/-->
-      <!--inertia ixx="0.00041438952" ixy="-0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="-0.00000900846" izz="0.00041168698"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00041438952" ixy="-0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="-0.00000900846" izz="0.00041168698"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00004972674" ixy="-0.00000335115" ixz="-0.00000637799" iyy="0.00005196084" iyz="-0.00000108101" izz="0.00004940243"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -97,7 +97,7 @@
      <mass value="0.00834951"/> <!-- volume = 0.003081e-3, density = 2,710kg/m^3 -->
      <origin xyz="0.0400002 0.0017746 0.0213350"/>
      <!--inertia ixx="0.000012e-5" ixy="0.0e-5" ixz="0.0e-5" iyy="0.000319e-5" iyz="0.0e-5" izz="0.000330e-5"/-->
-      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="3.25199972e-7" ixy="0.0" ixz="0.0" iyy="0.00000864489" iyz="0.0" izz="0.00000894299"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -128,7 +128,7 @@
      <mass value="0.05564726"/> <!-- volume = 0.016106e-3, density = 2,710kg + 0.013kg for the servo (a little bit random) -->
      <origin xyz="-0.0025176 0.0017559 0.0028322"/>
      <!--inertia ixx="0.000317e-5" ixy="-0.000054e-5" ixz="0.000061e-5" iyy="0.001368e-5" iyz="0.000011e-5" izz="0.001244e-5"/-->
-      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="0.00003787408" iyy="0.0008493729" iyz="0.00000682975" izz="0.00077238296"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="0.00003787408" iyy="0.0008493729" iyz="0.00000682975" izz="0.00077238296"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00001095255" ixy="-0.00000186573" ixz="0.00000210758" iyy="0.00004726527" iyz="3.80056874e-7" izz="0.00004298099"/> <!-- divided by the volume, just need to be multiplied by the mass -->
    </inertial>
    <visual>
@@ -183,7 +183,7 @@
      <mass value="0.120"/> <!-- volume = 0.033302e-3, mass of 2 servos = 0,110kg + mass of a little bit of alloy = 0.010kg (approx) -->
      <origin xyz="0.014769 -0.007977 0.004815"/>
      <!--inertia ixx="0.001380e-5" ixy="-0.000093e-5" ixz="-0.000177e-5" iyy="0.001442e-5" iyz="-0.000030e-5" izz="0.001371e-5"/-->
-      <!--inertia ixx="0.00041438952" ixy="-0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="-0.00000900846" izz="0.00041168698"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00041438952" ixy="-0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="-0.00000900846" izz="0.00041168698"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00004972674" ixy="-0.00000335115" ixz="-0.00000637799" iyy="0.00005196084" iyz="-0.00000108101" izz="0.00004940243"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -214,7 +214,7 @@
      <mass value="0.00834951"/> <!-- volume = 0.003081e-3, density = 2,710kg/m^3 -->
      <origin xyz="0.0400002 0.0017746 0.0213350"/>
      <!--inertia ixx="0.000012e-5" ixy="0.0e-5" ixz="0.0e-5" iyy="0.000319e-5" iyz="0.0e-5" izz="0.000330e-5"/-->
-      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="3.25199972e-7" ixy="0.0" ixz="0.0" iyy="0.00000864489" iyz="0.0" izz="0.00000894299"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -245,7 +245,7 @@
      <mass value="0.05564726"/> <!-- volume = 0.016106e-3, density = 2,710kg + 0.013kg for the servo (a little bit random) -->
      <origin xyz="-0.0025176 0.0017559 0.0028322"/>
      <!--inertia ixx="0.000317e-5" ixy="-0.000054e-5" ixz="0.000061e-5" iyy="0.001368e-5" iyz="0.000011e-5" izz="0.001244e-5"/-->
-      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="0.00003787408" iyy="0.0008493729" iyz="0.00000682975" izz="0.00077238296"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="0.00003787408" iyy="0.0008493729" iyz="0.00000682975" izz="0.00077238296"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00001095255" ixy="-0.00000186573" ixz="0.00000210758" iyy="0.00004726527" iyz="3.80056874e-7" izz="0.00004298099"/> <!-- divided by the volume, just need to be multiplied by the mass -->
    </inertial>
    <visual>
@@ -300,7 +300,7 @@
      <mass value="0.120"/> <!-- volume = 0.033302e-3, mass of 2 servos = 0,110kg + mass of a little bit of alloy = 0.010kg (approx) -->
      <origin xyz="0.014769 -0.007977 0.004815"/>
      <!--inertia ixx="0.001380e-5" ixy="-0.000093e-5" ixz="-0.000177e-5" iyy="0.001442e-5" iyz="-0.000030e-5" izz="0.001371e-5"/-->
-      <!--inertia ixx="0.00041438952" ixy="-0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="-0.00000900846" izz="0.00041168698"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00041438952" ixy="-0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="-0.00000900846" izz="0.00041168698"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00004972674" ixy="-0.00000335115" ixz="-0.00000637799" iyy="0.00005196084" iyz="-0.00000108101" izz="0.00004940243"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -331,7 +331,7 @@
      <mass value="0.00834951"/> <!-- volume = 0.003081e-3, density = 2,710kg/m^3 -->
      <origin xyz="0.0400002 0.0017746 0.0213350"/>
      <!--inertia ixx="0.000012e-5" ixy="0.0e-5" ixz="0.0e-5" iyy="0.000319e-5" iyz="0.0e-5" izz="0.000330e-5"/-->
-      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="3.25199972e-7" ixy="0.0" ixz="0.0" iyy="0.00000864489" iyz="0.0" izz="0.00000894299"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -362,7 +362,7 @@
      <mass value="0.05564726"/> <!-- volume = 0.016106e-3, density = 2,710kg + 0.013kg for the servo (a little bit random) -->
      <origin xyz="-0.0025176 0.0017559 0.0028322"/>
      <!--inertia ixx="0.000317e-5" ixy="-0.000054e-5" ixz="0.000061e-5" iyy="0.001368e-5" iyz="0.000011e-5" izz="0.001244e-5"/-->
-      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="0.00003787408" iyy="0.0008493729" iyz="0.00000682975" izz="0.00077238296"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="0.00003787408" iyy="0.0008493729" iyz="0.00000682975" izz="0.00077238296"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00001095255" ixy="-0.00000186573" ixz="0.00000210758" iyy="0.00004726527" iyz="3.80056874e-7" izz="0.00004298099"/> <!-- divided by the volume, just need to be multiplied by the mass -->
    </inertial>
    <visual>
@@ -417,7 +417,7 @@
      <mass value="0.120"/> <!-- volume = 0.033302e-3, mass of 2 servos = 0,110kg + mass of a little bit of alloy = 0.010kg (approx) -->
      <origin xyz="0.014769 0.007977 0.004815"/>
      <!--inertia ixx="0.001380e-5" ixy="0.000093e-5" ixz="-0.000177e-5" iyy="0.001442e-5" iyz="0.000030e-5" izz="0.001371e-5"/-->
-      <!--inertia ixx="0.00041438952" ixy="0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="0.00000900846" izz="0.00041168698"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00041438952" ixy="0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="0.00000900846" izz="0.00041168698"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00004972674" ixy="0.00000335115" ixz="-0.00000637799" iyy="0.00005196084" iyz="0.00000108101" izz="0.00004940243"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -448,7 +448,7 @@
      <mass value="0.00834951"/> <!-- volume = 0.003081e-3, density = 2,710kg/m^3 -->
      <origin xyz="0.0400002 0.0017746 -0.0213350"/>
      <!--inertia ixx="0.000012e-5" ixy="0.0e-5" ixz="0.0e-5" iyy="0.000319e-5" iyz="0.0e-5" izz="0.000330e-5"/-->
-      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="3.25199972e-7" ixy="0.0" ixz="0.0" iyy="0.00000864489" iyz="0.0" izz="0.00000894299"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -479,7 +479,7 @@
      <mass value="0.05564726"/> <!-- volume = 0.016106e-3, density = 2,710kg + 0.013kg for the servo (a little bit random) -->
      <origin xyz="-0.0025176 0.0017559 -0.0028322"/>
      <!--inertia ixx="0.000317e-5" ixy="-0.000054e-5" ixz="-0.000061e-5" iyy="0.001368e-5" iyz="-0.000011e-5" izz="0.001244e-5"/-->
-      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="-0.00003787408" iyy="0.0008493729" iyz="-0.00000682975" izz="0.00077238296"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="-0.00003787408" iyy="0.0008493729" iyz="-0.00000682975" izz="0.00077238296"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00001095255" ixy="-0.00000186573" ixz="-0.00000210758" iyy="0.00004726527" iyz="-3.80056874e-7" izz="0.00004298099"/> <!-- divided by the volume, just need to be multiplied by the mass -->
    </inertial>
    <visual>
@@ -534,7 +534,7 @@
      <mass value="0.120"/> <!-- volume = 0.033302e-3, mass of 2 servos = 0,110kg + mass of a little bit of alloy = 0.010kg (approx) -->
      <origin xyz="0.014769 0.007977 0.004815"/>
      <!--inertia ixx="0.001380e-5" ixy="0.000093e-5" ixz="-0.000177e-5" iyy="0.001442e-5" iyz="0.000030e-5" izz="0.001371e-5"/-->
-      <!--inertia ixx="0.00041438952" ixy="0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="0.00000900846" izz="0.00041168698"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00041438952" ixy="0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="0.00000900846" izz="0.00041168698"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00004972674" ixy="0.00000335115" ixz="-0.00000637799" iyy="0.00005196084" iyz="0.00000108101" izz="0.00004940243"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -565,7 +565,7 @@
      <mass value="0.00834951"/> <!-- volume = 0.003081e-3, density = 2,710kg/m^3 -->
      <origin xyz="0.0400002 0.0017746 -0.0213350"/>
      <!--inertia ixx="0.000012e-5" ixy="0.0e-5" ixz="0.0e-5" iyy="0.000319e-5" iyz="0.0e-5" izz="0.000330e-5"/-->
-      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="3.25199972e-7" ixy="0.0" ixz="0.0" iyy="0.00000864489" iyz="0.0" izz="0.00000894299"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -596,7 +596,7 @@
      <mass value="0.05564726"/> <!-- volume = 0.016106e-3, density = 2,710kg + 0.013kg for the servo (a little bit random) -->
      <origin xyz="-0.0025176 0.0017559 -0.0028322"/>
      <!--inertia ixx="0.000317e-5" ixy="-0.000054e-5" ixz="-0.000061e-5" iyy="0.001368e-5" iyz="-0.000011e-5" izz="0.001244e-5"/-->
-      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="-0.00003787408" iyy="0.0008493729" iyz="-0.00000682975" izz="0.00077238296"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="-0.00003787408" iyy="0.0008493729" iyz="-0.00000682975" izz="0.00077238296"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00001095255" ixy="-0.00000186573" ixz="-0.00000210758" iyy="0.00004726527" iyz="-3.80056874e-7" izz="0.00004298099"/> <!-- divided by the volume, just need to be multiplied by the mass -->
    </inertial>
    <visual>
@@ -651,7 +651,7 @@
      <mass value="0.120"/> <!-- volume = 0.033302e-3, mass of 2 servos = 0,110kg + mass of a little bit of alloy = 0.010kg (approx) -->
      <origin xyz="0.014769 0.007977 0.004815"/>
      <!--inertia ixx="0.001380e-5" ixy="0.000093e-5" ixz="-0.000177e-5" iyy="0.001442e-5" iyz="0.000030e-5" izz="0.001371e-5"/-->
-      <!--inertia ixx="0.00041438952" ixy="0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="0.00000900846" izz="0.00041168698"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00041438952" ixy="0.00002792625" ixz="-0.00005314996" iyy="0.00043300702" iyz="0.00000900846" izz="0.00041168698"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00004972674" ixy="0.00000335115" ixz="-0.00000637799" iyy="0.00005196084" iyz="0.00000108101" izz="0.00004940243"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -682,7 +682,7 @@
      <mass value="0.00834951"/> <!-- volume = 0.003081e-3, density = 2,710kg/m^3 -->
      <origin xyz="0.0400002 0.0017746 -0.0213350"/>
      <!--inertia ixx="0.000012e-5" ixy="0.0e-5" ixz="0.0e-5" iyy="0.000319e-5" iyz="0.0e-5" izz="0.000330e-5"/-->
-      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00003894839" ixy="0.0" ixz="0.0" iyy="0.00103537812" iyz="0.0" izz="0.00107108081"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="3.25199972e-7" ixy="0.0" ixz="0.0" iyy="0.00000864489" iyz="0.0" izz="0.00000894299"/> <!-- divided by the volume and multiplied by the mass -->
    </inertial>
    <visual>
@@ -713,7 +713,7 @@
      <mass value="0.05564726"/> <!-- volume = 0.016106e-3, density = 2,710kg + 0.013kg for the servo (a little bit random) -->
      <origin xyz="-0.0025176 0.0017559 -0.0028322"/>
      <!--inertia ixx="0.000317e-5" ixy="-0.000054e-5" ixz="-0.000061e-5" iyy="0.001368e-5" iyz="-0.000011e-5" izz="0.001244e-5"/-->
-      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="-0.00003787408" iyy="0.0008493729" iyz="-0.00000682975" izz="0.00077238296"/> <!-- divided by the volume, just need to be multiplied by the mass -->
+      <!--inertia ixx="0.00019682106" ixy="-0.00003352787" ixz="-0.00003787408" iyy="0.0008493729" iyz="-0.00000682975" izz="0.00077238296"/--> <!-- divided by the volume, just need to be multiplied by the mass -->
      <inertia ixx="0.00001095255" ixy="-0.00000186573" ixz="-0.00000210758" iyy="0.00004726527" iyz="-3.80056874e-7" izz="0.00004298099"/> <!-- divided by the volume, just need to be multiplied by the mass -->
    </inertial>
    <visual>
@@ -36,7 +36,7 @@ is the world frame). For more, see http://www.ros.org/wiki/xacro -->
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="trunk"/>
-  </joint>
+  </joint-->
  <!-- Links -->
  <!-- Footprint link -->
  <!--link name="base_link">
@@ -45,7 +45,7 @@ is the world frame). For more, see http://www.ros.org/wiki/xacro -->
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </visual>
-  </link>
+  </link-->
  <!-- ***********  MODEL COLORS *********** -->
  <material name="black">
    <color rgba="0.1 0.1 0.1 1"/>
@@ -5,7 +5,7 @@
 <!-- =================================================================================== -->
 <robot name="jaco_robot" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
  <!--link name="robot_root">
-  </link>
+  </link-->
  <!-- fake cylinder which is actually a box -->
  <!--xacro:macro name="cyl_inertia" params="mass r h ">
      <mass value="${mass}"/>
@@ -28,7 +28,7 @@
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="robot_root"/>
    <child link="jaco_0_baseA"/>
-  </joint>
+  </joint-->
  <!-- for some reason, material only applies if full name specified...
       these only seem to work in older gazebo versions -->
  <gazebo reference="jaco_ring_">
@@ -1,6 +1,7 @@
-<robot name="LittleDog" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+<?xml version="1.0" ?>
+<robot name="LittleDog"> <!--xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 xsi:schemaLocation="http://drake.mit.edu drake-distro/drake/doc/drakeURDF.xsd"
- xmlns="http://drake.mit.edu">
+ xmlns="http://drake.mit.edu"-->
  
  <!-- converted from https://svn.csail.mit.edu/littledog/trunk/simulation/little_dog.sd -->

@@ -72,7 +73,7 @@
      <geometry>
        <mesh filename="meshes/front_left_upper.obj" scale=".0254 .0254 .0254"/>
      </geometry>
-      <!--origin xyz="-0.0265 0 -0.048" /> <!-- this has to be tuned: press `w` in pybullet -->
+      <!--origin xyz="-0.0265 0 -0.048" /--> <!-- this has to be tuned: press `w` in pybullet -->
      <!--geometry>
        <capsule radius="0.012" length="0.09" />
      </geometry-->
@@ -106,7 +107,7 @@
      <material name="black" />
    </visual>
    <!--collision group="left_lower_legs">
-      <origin xyz="-0.0265 0 -0.048" /> <!-- note this is approximate -->
+      <origin xyz="-0.0265 0 -0.048" /--> <!-- note this is approximate -->
      <!--geometry>
        <capsule radius="0.012" length="0.09" />
      </geometry>
@@ -223,7 +224,7 @@
      </geometry>
    </collision>
    <!--collision group="right_lower_legs">
-      <origin xyz="-0.0265 0 -0.048" /> <!-- note this is approximate -->
+      <origin xyz="-0.0265 0 -0.048" /--> <!-- note this is approximate -->
      <!--geometry>
        <capsule radius="0.012" length="0.09" />
      </geometry>
@@ -328,7 +329,7 @@
      <material name="black" />
    </visual>
    <!--collision group="left_lower_legs">
-      <origin xyz="0.0265 0 -0.048" /> <!-- note this is approximate -->
+      <origin xyz="0.0265 0 -0.048" /--> <!-- note this is approximate -->
      <!--geometry>
        <capsule radius="0.012" length="0.09" />
      </geometry>
@@ -439,7 +440,7 @@
      <material name="black" />
    </visual>
    <!--collision group="right_lower_legs">
-      <origin xyz="0.0265 0 -0.048" /> <!-- note this is approximate -->
+      <origin xyz="0.0265 0 -0.048" /--> <!-- note this is approximate -->
      <!--geometry>
        <capsule radius="0.012" length="0.09" />
      </geometry>
@@ -13,7 +13,7 @@
  
  -->
  <!-- Uncomment this to have the robot rigidly connected to the world -->
-  <!--link name="world"> <!-- if you put mass/inertia to 0 for the base_link in pybullet then the robot will not be affected by gravity (i.e. it will float)>
+  <!--link name="world"--> <!-- if you put mass/inertia to 0 for the base_link in pybullet then the robot will not be affected by gravity (i.e. it will float)>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <mass value="0.0"/>
@@ -421,7 +421,7 @@ class Dart(Simulator):
                self.viewer = dart.gui.osg.Viewer()
                gui_node = dart.gui.osg.WorldNode(self.world)
                self.viewer.addWorldNode(gui_node)
-                self.viewer.setUpViewInWindow(0, 0, 640, 480)
+                self.viewer.setUpViewInWindow(0, 0, 1280, 960)
                self.viewer.setCameraHomePosition([8., -8., 4.], [0, 0, -0.25], [0, 0, 0.5])
                self._frame_cnt = 0
                # self.viewer.run()  # TODO: call self.viewer.frame() instead (need to update the python wrapper)
@@ -0,0 +1 @@
+This folder contains generated meshes when using some simulators that requires a specific format. You can remove them.
@@ -76,6 +76,7 @@ import glfw
 from pyrobolearn.simulators.simulator import Simulator
 from pyrobolearn.utils.parsers.robots import URDFParser, MuJoCoParser, SDFParser
 import pyrobolearn.utils.parsers.robots.data_structures as struct
+from pyrobolearn.utils.transformation import get_homogeneous_matrix


 # check Python version
@@ -141,25 +142,46 @@ class Texture(object):
 #         return self.body.attrib.get("name")


-class MultiBody(object):
-    """MultiBody."""
+class Body(object):
+    """Body."""

-    def __init__(self, tree):
+    def __init__(self, body_id, body_tag, body=None, fixed_base=False):
        """
        Initialize the MultiBody.

        Args:
-            tree (Tree): tree / multi-body data structure.
+            body_id (int): unique body id in the Mujoco model.
+            body_tag (ET.Element): body tag element in the XML file.
+            body (MultiBody, None): multibody data structure.
+            fixed_base (bool): if True, the body is fixed in the world.
        """
-        if not isinstance(tree, struct.Tree):
-            raise TypeError("Expecting the given 'tree' to be an instance of `Tree`, but got instead: "
-                            "{}".format(type(tree)))
-        self.tree = tree
+        self.id = body_id
+        self.tag = body_tag

        self.q_start = 0  # starting index in the whole state
        self.q_end = 0  # end index in the whole state
-        static = self.tree.static
-        self.fixed_base = static if static is not None else False
+        self.fixed_base = fixed_base
+
+        if body is not None:
+            self.num_bodies = body.num_bodies  # number of links
+            self.num_joints = body.num_joints  # number of joints (including fixed joints)
+            self.num_actuated_joints = body.num_actuated_joints  # number of actuated joints
+
+    @property
+    def num_dofs(self):
+        """Return the number of DoFs."""
+        return self.q_end - self.q_start
+
+    @property
+    def name(self):
+        """Return the body name."""
+        # note that we remove the generated prefix and suffix by the parser/generator
+        return '_'.join(self.tag_name.split('_')[1:-1])
+
+    @property
+    def tag_name(self):
+        """Return the body tag name."""
+        return self.tag.attrib.get("name")


 class Mujoco(Simulator):
@@ -232,10 +254,14 @@ class Mujoco(Simulator):
        # create counters
        self._visual_cnt = 0
        self._collision_cnt = 0
-        self._body_cnt = 0  # 0 is for the world
+        self._body_cnt = 0  # 0 is for the world floor
        self._texture_cnt = 0
        self._constraint_cnt = 0
+
+        # counters for Mujoco
        self._q_cnt = 0
+        self._link_cnt = 0
+        self._mjc_body_id = 0

        self.default_timestep = 0.002
        self.dt = self.default_timestep
@@ -250,7 +276,8 @@ class Mujoco(Simulator):

        # add light
        self._parser.add_element("light", self._worldbody,
-                                 attributes={"diffuse": ".5 .5 .5", "pos": "0 0 3", "dir": "0 0 -1"})
+                                 attributes={"diffuse": "0.5 0.5 0.5", "pos": "0 0 3", "directional": "true",
+                                             "dir": "0 0 -1"})
        # add floor
        self.load_floor()

@@ -260,6 +287,9 @@ class Mujoco(Simulator):
        # define saving states
        self.__simulator_saving_states = {}

+        # define directory mesh path (to write the converted mesh files as Mujoco only accepts STL)
+        self.mesh_directory_path = os.path.dirname(os.path.abspath(__file__)) + '/meshes/'
+
    ##############
    # Properties #
    ##############
@@ -441,7 +471,7 @@ class Mujoco(Simulator):
        """
        return self.sim.model.opt.timestep

-    def set_time_step(self, time_step):
+    def set_time_step(self, time_step):  # TODO: modify the option tag
        """Set the specified time step in the simulator.

        "Warning: in many cases it is best to leave the timeStep to default, which is 240Hz. Several parameters are
@@ -455,13 +485,16 @@ class Mujoco(Simulator):
        Args:
            time_step (float): Each time you call 'step' the time step will proceed with 'time_step'.
        """
-        self.sim.model.opt.timestep = time_step
+        # self.sim.model.opt.timestep = time_step
+        time_step = self._parser.convert_attribute_to_string(time_step)
+        self._parser.option_tag.attrib.setdefault('timestep', time_step)
+        self._update_sim()

    def get_gravity(self):
        """Return the gravity set in the simulator."""
        return self.sim.model.opt.gravity

-    def set_gravity(self, gravity=(0, 0, -9.81)):
+    def set_gravity(self, gravity=(0, 0, -9.81)):  # TODO: modify the option tag
        """Set the gravity in the simulator with the given acceleration.

        By default, there is no gravitational force enabled in the simulator.
@@ -469,7 +502,10 @@ class Mujoco(Simulator):
        Args:
            gravity (list/tuple[float[3]]): acceleration in the x, y, z directions.
        """
-        self.sim.model.opt.gravity = gravity
+        # self.sim.model.opt.gravity = gravity
+        gravity = self._parser.convert_attribute_to_string(gravity)
+        self._parser.option_tag.attrib.setdefault('gravity', gravity)
+        self._update_sim()

    def save(self, filename=None, *args, **kwargs):
        """
@@ -537,22 +573,28 @@ class Mujoco(Simulator):
        Returns:
            int (non-negative): unique id associated to the load model.
        """
-        print(filename)
-        print(os.path.dirname(filename))
        # parse URDF file
        urdf_parser = URDFParser(filename=filename)
        tree = urdf_parser.tree

-        # update position and orientation
-        tree.position = position
-        tree.orientation = orientation
+        # update tree position and orientation
+        position = np.zeros(3) if position is None else np.asarray(position)
+        orientation = np.array([0., 0., 0., 1.]) if orientation is None else np.asarray(orientation)
+        h = get_homogeneous_matrix(position, orientation)
+        tree.homogeneous = h.dot(tree.homogeneous)

-        # add the parse tree to the MJCF parser/generator
-        self._parser.add_multibody(tree, mesh_directory_path=os.path.dirname(os.path.abspath(__file__)) + '/meshes/')
+        # update tree base if it is static or not
+        tree.static = use_fixed_base
+        # if not static, add free joint to body
+        if not use_fixed_base:
+            name, body = "joint", tree.root
+            joint = struct.Joint(joint_id=-1, name=name, dtype='free', child=body)
+            body.add_parent_joint(joint)
+            tree.add_joint(joint, idx=0)

-        print(self._parser.get_string(pretty_format=True))
+        return self._create_body(tree, fixed_base=use_fixed_base, verbose=1)

-    def load_sdf(self, filename, scaling=1., *args, **kwargs):
+    def load_sdf(self, filename, scaling=1., *args, **kwargs):  # TODO
        """Load a SDF file in the simulator.

        Args:
@@ -562,15 +604,17 @@ class Mujoco(Simulator):
        Returns:
            list(int): list of object unique id for each object loaded
        """
-        # parse sdf file
-        sdf_parser = SDFParser(filename=filename)
+        raise NotImplementedError

-        for tree in sdf_parser.world.trees:
-            # # update the position and orientation
-            # tree.position = position
-            # tree.orientation = orientation
-
-            self._parser.add_multibody(tree)
+        # # parse sdf file
+        # sdf_parser = SDFParser(filename=filename)
+        #
+        # for tree in sdf_parser.world.trees:
+        #     # # update the position and orientation
+        #     # tree.position = position
+        #     # tree.orientation = orientation
+        #
+        #     self._parser.add_multibody(tree)

    def load_mjcf(self, filename, scaling=1., *args, **kwargs):
        """Load a Mujoco file in the simulator.
@@ -623,6 +667,7 @@ class Mujoco(Simulator):
            int: unique id of the mesh in the world
        """
        # convert file '.obj' to '.stl' as MuJoCo only supports STL formats.
+        filename = self._parser.convert_mesh(filename, mesh_dirname=self.mesh_directory_path)

        # try to look for textures and colors in the '.mtl' file

@@ -652,18 +697,83 @@ class Mujoco(Simulator):
        # if floor already loaded, remove it
        if self._floor_id is not None:
            floor = self._bodies.pop(self._floor_id)
-            self._worldbody.remove(floor)
+            self._worldbody.remove(floor.tag)
            self._model_changed = True

        # create floor
        dim = dimension/2.
        floor = self._parser.add_element(name="geom", parent_element=self._worldbody,
                                         attributes={"type": "plane", "size": str(dim) + " " + str(dim) + " 1."})
-        self._body_cnt += 1
-        self._bodies[self._body_cnt] = floor
-        self._floor_id = self._body_cnt
+
+        body = Body(body_id=0, body_tag=floor, fixed_base=True)  # 0 is only for the floor
+        self._bodies[0] = body  # the floor is always the first body in the ordered dict
+        self._floor_id = 0
+
+        # update mujoco model if necessary
+        self._update_sim()
+
        return self._floor_id

+    def _update_sim(self):
+        # update mujoco model if necessary
+        if self._update_dynamically:
+            self._create_sim(render=self._render)
+        else:
+            # notify that the Mujoco model has changed (this will be checked in the `step` method)
+            self._model_changed = True
+
+    def _create_body(self, tree, body_id=None, fixed_base=False, verbose=1):
+        """
+        Create inner body in the simulator; given the tree data structure, it generates all the XML tags using
+        the inner parser/generator, wraps the returned tree XML tag to a Body structure, sets its attributes (such as
+        its q indices), update the simulator if necessary and return the unique body id.
+
+        Args:
+            tree (struct.MultiBody): the tree / multi-body data structure.
+            body_id (int, None): unique body id to save the wrapped body.
+            fixed_base (bool): if the base of the multibody/tree is fixed or not.
+            verbose (bool, int): if True, it will print the current Mujoco XML file that we have which is useful for
+              debug. If int, it represents the level of verbosity.
+
+        Returns:
+            int: unique body id.
+        """
+        # check body id
+        if body_id is None:
+            self._body_cnt += 1
+            body_id = self._body_cnt
+
+        # create tree tag
+        tree_tag = self._parser.add_multibody(tree, mesh_directory_path=self.mesh_directory_path)
+
+        # save body
+        self._mjc_body_id += 1
+        body = Body(body_id=self._mjc_body_id, body_tag=tree_tag, body=tree, fixed_base=fixed_base)
+        self._bodies[body_id] = body
+        num_dofs = tree.num_dofs
+
+        if verbose > 0:
+            print("\nNum DoFs: {}".format(tree.num_dofs))
+            print("Num links/bodies: {}".format(tree.num_bodies))
+            print("Num joints: {}".format(tree.num_joints))
+            print("Num actuated joints: {}\n".format(tree.num_actuated_joints))
+
+        # set generalized coordinates counter
+        body.q_start = self._q_cnt
+        num_dofs = num_dofs if fixed_base else num_dofs + 7
+        self._q_cnt += num_dofs
+        body.q_end = self._q_cnt
+
+        # update mujoco model if necessary
+        self._update_sim()
+
+        # if verbose, print current xml file
+        if verbose > 1:
+            print(self._parser.get_string(pretty_format=True))
+
+        # return body id
+        return body_id
+
    def create_body(self, visual_shape_id=-1, collision_shape_id=-1, mass=0., position=(0., 0., 0.),
                    orientation=(0., 0., 0., 1.), *args, **kwargs):  # DONE
        """Create a body in the simulator.
@@ -705,29 +815,11 @@ class Mujoco(Simulator):

        # wrap body into multi-body data structure
        tree = struct.Tree(name=body.name, root=body, position=body.position, orientation=body.orientation)
-
-        # save multi-body data structure
-        self._parser.add_multibody(tree)
-        body = MultiBody(tree)
-        self._bodies[self._body_cnt] = body
-
-        # set generalized coordinates counter
+        tree.add_body(body)
        if not static:
-            body.q_start = self._q_cnt
-            body.q_end = self._q_cnt + 7
-            self._q_cnt += 7
+            tree.add_joint(joint)

-        # update mujoco model if necessary
-        if self._update_dynamically:
-            self._create_sim(render=self._render)
-        else:
-            # notify that the Mujoco model has changed (this will be checked in the `step` method)
-            self._model_changed = True
-
-        # print(self._parser.get_string(pretty_format=True))
-
-        # return body id
-        return self._body_cnt
+        return self._create_body(tree, body_id=self._body_cnt, fixed_base=static, verbose=1)

    def remove_body(self, body_id):  # DONE
        """Remove a particular body in the simulator.
@@ -736,14 +828,28 @@ class Mujoco(Simulator):
            body_id (int): unique body id.
        """
        # remove body from the bodies
+        # This is an O(N) operation because I have to modify the id, q_start, and q_end of the bodies that appears
+        # after the given body
+        body_to_remove = self._bodies[body_id]
+        found, num_dofs = False, 0
+        for body in self._bodies:
+            if body_to_remove == body:
+                found = True
+                num_dofs = body.num_dofs
+            if found:  # for the bodies after body_to_remove, shift their id and q_start/q_end to the left
+                body.id -= 1  # shift id to the left
+                body.q_start -= num_dofs
+                body.q_end -= num_dofs
+                self._q_cnt = body.q_end
+
        body = self._bodies.pop(body_id)
+        self._mjc_body_id -= 1

        # remove it from the worldbody
-        self._worldbody.remove(body)
+        self._worldbody.remove(body.tag)

        # if the model / sim were loaded, notify that the Mujoco has changed
-        if self.sim is not None and self.model is not None:
-            self._model_changed = True
+        self._update_sim()

    def num_bodies(self):  # DONE
        """Return the number of bodies present in the simulator.
@@ -764,7 +870,8 @@ class Mujoco(Simulator):
        Returns:
            str: base name
        """
-        return self._bodies[body_id].name
+        # return self._bodies[body_id].name
+        return self.get_base_name(body_id)

    def get_body_id(self, index):  # DONE
        """
@@ -776,7 +883,8 @@ class Mujoco(Simulator):
        Returns:
            int: unique body id.
        """
-        return list(self._bodies.items())[index][0]
+        # O(N)
+        return list(self._bodies.keys())[index][0]

    ###############
    # Constraints #
@@ -937,7 +1045,8 @@ class Mujoco(Simulator):
        Returns:
            str: base name
        """
-        return self.sim.model.body_id2name(box2)
+        body_id = self._bodies[body_id].id
+        return self.sim.model.body_id2name(body_id)

    def get_center_of_mass_position(self, body_id, link_ids=None):  # TODO
        """
@@ -22,6 +22,7 @@ from xml.dom import minidom  # to print in a pretty way the XML file
 # import mesh related libraries
 try:
    import trimesh  # processing triangular meshes
+    from trimesh.exchange.export import export_mesh
    # import pymesh    # rapid prototyping platform focused on geometry processing
    import pyassimp  # library to import and export various 3d-model-formats
 except ImportError as e:
@@ -38,18 +39,25 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"


-def convert_mesh(from_filename, to_filename):
+def convert_mesh(from_filename, to_filename, library='pyassimp'):
    """
    Convert the given file containing the original mesh to the other specified format using the `pyassimp` library.

    Args:
        from_filename (str): filename of the mesh to convert.
        to_filename (str): filename of the converted mesh.
+        library (str): library to use to convert the meshes. Select between 'pyassimp' and 'trimesh'.
    """
-    scene = pyassimp.load(from_filename)
-    extension = to_filename.split('.')[-1]
-    pyassimp.export(scene, to_filename, file_type=extension)
-    pyassimp.release(scene)
+    if library == 'pyassimp':
+        scene = pyassimp.load(from_filename)
+        extension = to_filename.split('.')[-1]
+        pyassimp.export(scene, to_filename, file_type=extension)
+        pyassimp.release(scene)
+    elif library == 'trimesh':
+        export_mesh(trimesh.load(from_filename), to_filename)
+    else:
+        raise NotImplementedError("The given library '{}' is currently not supported, select between 'pyassimp' and "
+                                  "'trimesh'".format(library))


 def mesh_to_urdf(filename, name=None, mass=None, inertia=None, density=1000, visual=True, collision=True, scale=1.,
@@ -802,8 +802,8 @@ class MultiBody(object):
        """
        self.name = name
        self.root = root
-        self.bodies = OrderedDict()
-        self.joints = OrderedDict()
+        self.bodies = OrderedDict()  # {name: Body}
+        self.joints = OrderedDict()  # {name: Joint}
        self.materials = {}
        self.frame = Frame(position, orientation, dtype='world')

@@ -823,17 +823,42 @@ class MultiBody(object):
    def num_dofs(self):
        """Return the total number of degrees of freedom."""
        num_dofs = 0
-        for joint in self.joints:
+        for joint in self.joints.values():
            num_dofs += joint.num_dofs
        return num_dofs

+    @property
+    def num_bodies(self):
+        """Return the total number of bodies in this multi-body."""
+        return len(self.bodies)
+
+    @property
+    def num_joints(self):
+        """Return the total number of joints in this multi-body (this accounts for fixed joints as well, but not
+        free joints)."""
+        # return len(self.joints)
+        num_joints = 0
+        for joint in self.joints.values():
+            if joint.dtype != 'floating':
+                num_joints += 1
+        return num_joints
+
+    @property
+    def num_actuated_joints(self):
+        """Return the total number of joints which are not fixed nor free."""
+        num_actuated_joints = 0
+        for joint in self.joints.values():
+            if joint.dtype != 'fixed' and joint.dtype != 'floating':
+                num_actuated_joints += 1
+        return num_actuated_joints
+
    @property
    def root(self):
        """Return the root body element."""
        if self._root is not None:
            return self._root
        if len(self.bodies):
-            return next(iter(self.bodies))  # get first element
+            return self.bodies[next(iter(self.bodies))]  # get first element

    @root.setter
    def root(self, root):
@@ -849,6 +874,12 @@ class MultiBody(object):
        if self.root is not None:
            return self.root.static

+    @static.setter
+    def static(self, static):
+        """Set the root element in the tree to be static or not."""
+        if self.root is not None:
+            self.root.static = static
+
    @property
    def position(self):
        """Return the tree frame position."""
@@ -908,6 +939,39 @@ class MultiBody(object):
    def homogeneous(self, matrix):
        """Set the given homogeneous matrix."""
        self.frame.homogeneous = matrix
+        if self.root is not None:
+            self.root.homogeneous = matrix
+
+    def add_body(self, body):
+        """Add a body to the tree.
+
+        Args:
+            body (Body): body data structure.
+        """
+        if not isinstance(body, Body):
+            raise TypeError("Expecting the given 'body' to be an instance of `Body` but got instead: "
+                            "{}".format(type(body)))
+        self.bodies[body.name] = body
+
+    def add_joint(self, joint, idx=None):
+        """Add a joint to the tree.
+
+        Args:
+            joint (Joint): joint data structure.
+            idx (int): index to insert a joint. This has a O(N) complexity as we have to copy everything.
+        """
+        if not isinstance(joint, Joint):
+            raise TypeError("Expecting the given 'joint' to be an instance of `Joint`, but got instead: "
+                            "{}".format(type(joint)))
+        if idx is not None:
+            self.joints[joint.name] = joint
+        else:
+            # this insert
+            joints = OrderedDict()
+            for i, (joint_name, joint_instance) in enumerate(self.joints.items()):
+                if i == idx:
+                    joints[joint.name] = joint
+                joint[joint_name] = joint_instance


 # alias
@@ -1319,6 +1383,7 @@ class Joint(object):
            velocity (float, str): joint maximum allowed velocity.
        """
        self.id = int(joint_id)
+        self.num_dofs = 0
        self.name = name
        self.dtype = dtype
        self.limits = limits
@@ -1330,7 +1395,6 @@ class Joint(object):
        self.damping = damping
        self.effort = effort
        self.velocity = velocity
-        self.num_dofs = 0

        self.init_position = None
        self.init_velocity = None
@@ -1379,6 +1443,16 @@ class Joint(object):

        self._dtype = dtype

+    @property
+    def num_dofs(self):
+        """Return the number of DoFs for the specified joint."""
+        return self._num_dofs
+
+    @num_dofs.setter
+    def num_dofs(self, dofs):
+        """Set the number of DoFs for the specified joint."""
+        self._num_dofs = int(dofs)
+
    @property
    def limits(self):
        """Return the joint limits."""
@@ -106,13 +106,20 @@ class MuJoCoParser(WorldParser):

        # create root XML element
        self.create_root("mujoco")
-        # add compiler
-        self.add_element("compiler", self._root, attributes={'coordinate': 'local', 'angle': 'radian'})
-        # add size
+
+        # add <compiler>
+        self.compiler_tag = self.add_element("compiler", self._root,
+                                             attributes={'coordinate': 'local', 'angle': 'radian'})
+
+        # add <size>
        self.nconmax = 200  # increase this if necessary (this depends on how many models are loaded)
-        self.add_element("size", self._root, attributes={"nconmax": str(self.nconmax)})
-        # add worldbody
-        self.worldbody = self.add_element(name="worldbody", parent_element=self.root)
+        self.size_tag = self.add_element("size", self._root, attributes={"nconmax": str(self.nconmax)})
+
+        # add <option>
+        self.option_tag = self.add_element("option", self._root, attributes={"timestep": str(0.002)})
+
+        # add <worldbody>
+        self.worldbody = self.add_element("worldbody", parent_element=self.root)

        # set some counters
        self._world_cnt = 0
@@ -846,6 +853,31 @@ class MuJoCoParser(WorldParser):
    # Generator #
    #############

+    def convert_attribute_to_string(self, attribute, slice=None):
+        """
+        Convert the attribute to a string to be set in the attribute dictionary of an XML tag.
+
+        Args:
+            attribute (str, bool, list, tuple, np.ndarray, int, float): attribute to convert.
+            slice (slice, None): slice to apply if the attribute is a list/tuple/np.array.
+
+        Returns:
+            str: converted attribute.
+        """
+        if isinstance(attribute, str):
+            return attribute
+        elif isinstance(attribute, bool):
+            return 'true' if attribute else 'false'
+        elif isinstance(attribute, (list, tuple, np.ndarray)):
+            attribute = np.asarray(attribute)
+            if slice is not None:
+                attribute = attribute[slice]
+            return str(attribute)[1:-1]
+        elif isinstance(attribute, (int, float)):
+            return str(attribute)
+        else:
+            raise NotImplementedError("The given attribute type is not supported: {}".format(type(attribute)))
+
    def _update_attribute_dict(self, dictionary, element, name, key=None, slice=None, fct=None, required=False,
                               *args, **kwargs):
        """
@@ -875,6 +907,10 @@ class MuJoCoParser(WorldParser):
            if fct is not None:
                attribute = fct(attribute, *args, **kwargs)

+            # check attribute type and convert it to a string
+            if attribute is not None:
+                dictionary[key] = self.convert_attribute_to_string(attribute, slice=slice)
+
            if attribute is not None:  # this is for the returned attribute by the fct
                # check attribute type and convert it to a string
                if isinstance(attribute, str):
@@ -1158,7 +1194,7 @@ class MuJoCoParser(WorldParser):
            root (ET.Element, None): root XML element.

        Returns:
-            ET.Element: body XML element.
+            ET.Element: tree root XML element.
        """
        # check arguments
        if not isinstance(parent_tag, ET.Element):
@@ -1175,8 +1211,11 @@ class MuJoCoParser(WorldParser):

        h_bodies, h_joints, h_visuals, h_collisions, h_inertials = {}, {}, {}, {}, {}
        for i, body in enumerate(tree.bodies.values()):
-            print(body.name, body.homogeneous)
-            h_inv = get_inverse_homogeneous(body.homogeneous)  # get link/joint frame
+            # print(body.name, body.homogeneous)
+            if i == 0:  # root
+                h_inv = np.identity(4)
+            else:
+                h_inv = get_inverse_homogeneous(body.homogeneous)  # get link/joint frame
            for visual in body.visuals:  # because geom is described wrt body frame and not link/joint frame
                h_visuals[visual] = h_inv.dot(visual.homogeneous)
            for collision in body.collisions:  # because geom is described wrt body frame and not link/joint frame
@@ -1184,6 +1223,7 @@ class MuJoCoParser(WorldParser):
            for inertial in body.inertials:  # because inertial is described wrt body frame and not link/joint frame
                h_inertials[inertial] = h_inv.dot(inertial.homogeneous)
            for joint in body.joints.values():
+                # print(joint.name, joint.homogeneous)
                if joint.child is not None:
                    h_child = joint.child.homogeneous
                    h = h_inv.dot(joint.homogeneous).dot(h_child)
@@ -1206,14 +1246,14 @@ class MuJoCoParser(WorldParser):
            inertial.homogeneous = homogeneous

        # generate bodies (inertial, visual, collision) and joints in a recursive manner
-        body_tag = self.generate_body(parent_tag, body=tree.root, root=root)
+        tree_root_tag = self.generate_body(parent_tag, body=tree.root, root=root)

        # empty the temporary assets
        self._assets_tmp = set([])

-        return body_tag
+        return tree_root_tag

-    def _convert_mesh(self, filename):
+    def convert_mesh(self, filename, mesh_dirname=None):
        """
        Convert mesh (from any format) to an STL mesh format. This is because MuJoCo only accepts STL meshes.

@@ -1221,6 +1261,7 @@ class MuJoCoParser(WorldParser):

        Args:
            filename (str): path to the mesh file.
+            mesh_dirname (str, None): path to the mesh directory to write the new mesh if not a STL file.

        Returns:
            str: filename with the correct extension (.stl)
@@ -1228,13 +1269,16 @@ class MuJoCoParser(WorldParser):
        # if mesh, make sure it is a STL file
        extension = filename.split('.')[-1]
        if extension.lower() != 'stl':
+            if mesh_dirname is None:
+                mesh_dirname = self._mesh_dirname
+
            # create filename with the correction extension (STL)
-            dirname = os.path.dirname(filename)
            basename = os.path.basename(filename)
            basename_without_extension = ''.join(basename.split('.')[:-1])
+            # dirname = os.path.dirname(filename)
            # filename_without_extension = dirname + basename_without_extension
            # new_filename = filename_without_extension + '.stl'
-            new_filename = self._mesh_dirname + '/' + basename_without_extension + '.stl'
+            new_filename = mesh_dirname + '/' + basename_without_extension + '.stl'

            # if file does not already exists, convert it
            if not os.path.isfile(new_filename):
@@ -1322,36 +1366,85 @@ class MuJoCoParser(WorldParser):
                    inertial_tag = ET.SubElement(body_tag, 'inertial', attrib=attrib)

        # create <geom> tags
-        # TODO: consider when multiple visual and collision shapes
        visual = body.visual
        collision = body.collision
        if visual is None:  # if no visual, use collision shape
            if collision is not None:  # if collision shape is defined
-                attrib = {"rgba": "0 0 0 0"}  # transparent
-                self._update_attribute_dict(attrib, collision, 'name', fct=self._generate_name, cnt=self._body_cnt)
-                self._update_attribute_dict(attrib, collision, 'position', key='pos')
-                self._update_attribute_dict(attrib, collision, 'quaternion', key='quat', fct=self._convert_xyzw_to_wxyz)
-                self._update_attribute_dict(attrib, collision, 'dtype', key='type')
-                self._update_attribute_dict(attrib, collision, 'size')
+                for collision in body.collisions:
+                    attrib = {"rgba": "0 0 0 0"}  # transparent
+                    self._update_attribute_dict(attrib, collision, 'name', fct=self._generate_name, cnt=self._body_cnt)
+                    self._update_attribute_dict(attrib, collision, 'position', key='pos')
+                    self._update_attribute_dict(attrib, collision, 'quaternion', key='quat',
+                                                fct=self._convert_xyzw_to_wxyz)
+                    self._update_attribute_dict(attrib, collision, 'dtype', key='type')
+                    self._update_attribute_dict(attrib, collision, 'size')

-                # check type and change size
-                if 'type' in attrib and 'size' in attrib:
-                    if collision.dtype == 'box':  # divide by 2 the dimensions
-                        attrib['size'] = str(np.asarray(collision.size) / 2.)[1:-1]
-                    elif collision.dtype == 'cylinder' or collision.dtype == 'capsule':  # divide by 2 the height
-                        radius, length = collision.size
-                        attrib['size'] = str(np.asarray([radius, length/2]))[1:-1]
-                    elif collision.dtype == 'mesh':  # set the scale
-                        attrib['fitscale'] = str(collision.size)
+                    # check type and change size
+                    if 'type' in attrib and 'size' in attrib:
+                        if collision.dtype == 'box':  # divide by 2 the dimensions
+                            attrib['size'] = str(np.asarray(collision.size) / 2.)[1:-1]
+                        elif collision.dtype == 'cylinder' or collision.dtype == 'capsule':  # divide by 2 the height
+                            radius, length = collision.size
+                            attrib['size'] = str(np.asarray([radius, length/2]))[1:-1]
+                        elif collision.dtype == 'mesh':  # set the scale
+                            attrib['fitscale'] = str(collision.size)

+                    if inertial is not None and inertial_tag is None:
+                        self._update_attribute_dict(attrib, inertial, 'mass', required=True)
+
+                    # create geom tag
+                    geom = ET.SubElement(body_tag, "geom", attrib=attrib)
+
+                    # check if mesh in attrib
+                    if collision.dtype == 'mesh':
+                        # check <asset> tag in xml
+                        asset_tag = root.find("asset")
+
+                        # if no <asset> tag, create one
+                        if asset_tag is None:
+                            asset_tag = ET.SubElement(root, "asset")
+
+                        # create <mesh> tag in <asset>
+                        mesh_path = self.convert_mesh(collision.filename, self._mesh_dirname)  # convert to STL
+                        mesh_name = os.path.basename(mesh_path).split('.')[0]
+                        if mesh_name not in self._assets_tmp:  # if the <mesh> doesn't already exists
+                            self._assets_tmp.add(mesh_name)
+                            attrib = {'name': mesh_name, 'file': mesh_path}
+                            self._update_attribute_dict(attrib, collision, 'size', key='scale')
+                            ET.SubElement(asset_tag, "mesh", attrib=attrib)
+
+                        # set the mesh asset name
+                        geom.attrib["mesh"] = mesh_name
+
+        else:
+            # if visual is given, use this one instead
+            for visual in body.visuals:
+                attrib = {}
+
+                self._update_attribute_dict(attrib, visual, 'name', fct=self._generate_name, cnt=self._body_cnt)
+                self._update_attribute_dict(attrib, visual, 'position', key='pos')
+                self._update_attribute_dict(attrib, visual, 'quaternion', key='quat', fct=self._convert_xyzw_to_wxyz)
+                self._update_attribute_dict(attrib, visual, 'dtype', key='type')
+                self._update_attribute_dict(attrib, visual, 'size')
+                self._update_attribute_dict(attrib, visual, 'rgba')
                if inertial is not None and inertial_tag is None:
                    self._update_attribute_dict(attrib, inertial, 'mass', required=True)

-                # create geom tag
+                # check type and change size
+                if 'type' in attrib and 'size' in attrib:
+                    if visual.dtype == 'box':  # divide by 2 the dimensions
+                        attrib['size'] = str(np.asarray(visual.size) / 2.)[1:-1]
+                    elif visual.dtype == 'cylinder' or visual.dtype == 'capsule':  # divide by 2 the height
+                        radius, length = visual.size
+                        attrib['size'] = str(np.asarray([radius, length / 2]))[1:-1]
+                    elif visual.dtype == 'mesh':  # set the scale
+                        attrib['fitscale'] = str(visual.size)
+
+                # create <geom> tag
                geom = ET.SubElement(body_tag, "geom", attrib=attrib)

-                # check if mesh in attrib
-                if collision.dtype == 'mesh':
+                # if primitive shape type is a mesh
+                if visual.dtype == "mesh":
                    # check <asset> tag in xml
                    asset_tag = root.find("asset")

@@ -1359,69 +1452,22 @@ class MuJoCoParser(WorldParser):
                    if asset_tag is None:
                        asset_tag = ET.SubElement(root, "asset")

-                    # create <mesh> tag in <asset>
-                    mesh_path = self._convert_mesh(collision.filename)  # convert to STL if necessary
+                    # create mesh tag
+                    mesh_path = self.convert_mesh(visual.filename, self._mesh_dirname)  # convert to STL if necessary
                    mesh_name = os.path.basename(mesh_path).split('.')[0]
-                    if mesh_name not in self._assets_tmp:  # if the <mesh> doesn't already exists
+                    if mesh_name not in self._assets_tmp:  # if the mesh doesn't already exists
                        self._assets_tmp.add(mesh_name)
                        attrib = {'name': mesh_name, 'file': mesh_path}
-                        self._update_attribute_dict(attrib, collision, 'size', key='scale')
+                        self._update_attribute_dict(attrib, visual, 'size', key='scale')
                        ET.SubElement(asset_tag, "mesh", attrib=attrib)

                    # set the mesh asset name
                    geom.attrib["mesh"] = mesh_name

-        else:
-            # if visual is given, use this one instead
-            attrib = {}
-
-            self._update_attribute_dict(attrib, visual, 'name', fct=self._generate_name, cnt=self._body_cnt)
-            self._update_attribute_dict(attrib, visual, 'position', key='pos')
-            self._update_attribute_dict(attrib, visual, 'quaternion', key='quat', fct=self._convert_xyzw_to_wxyz)
-            self._update_attribute_dict(attrib, visual, 'dtype', key='type')
-            self._update_attribute_dict(attrib, visual, 'size')
-            self._update_attribute_dict(attrib, visual, 'rgba')
-            if inertial is not None and inertial_tag is None:
-                self._update_attribute_dict(attrib, inertial, 'mass', required=True)
-
-            # check type and change size
-            if 'type' in attrib and 'size' in attrib:
-                if visual.dtype == 'box':  # divide by 2 the dimensions
-                    attrib['size'] = str(np.asarray(visual.size) / 2.)[1:-1]
-                elif visual.dtype == 'cylinder' or visual.dtype == 'capsule':  # divide by 2 the height
-                    radius, length = visual.size
-                    attrib['size'] = str(np.asarray([radius, length / 2]))[1:-1]
-                elif visual.dtype == 'mesh':  # set the scale
-                    attrib['fitscale'] = str(visual.size)
-
-            # create <geom> tag
-            geom = ET.SubElement(body_tag, "geom", attrib=attrib)
-
-            # if primitive shape type is a mesh
-            if visual.dtype == "mesh":
-                # check <asset> tag in xml
-                asset_tag = root.find("asset")
-
-                # if no <asset> tag, create one
-                if asset_tag is None:
-                    asset_tag = ET.SubElement(root, "asset")
-
-                # create mesh tag
-                mesh_path = self._convert_mesh(visual.filename)  # convert to STL if necessary
-                mesh_name = os.path.basename(mesh_path).split('.')[0]
-                if mesh_name not in self._assets_tmp:  # if the mesh doesn't already exists
-                    self._assets_tmp.add(mesh_name)
-                    attrib = {'name': mesh_name, 'file': mesh_path}
-                    self._update_attribute_dict(attrib, visual, 'size', key='scale')
-                    ET.SubElement(asset_tag, "mesh", attrib=attrib)
-
-                # set the mesh asset name
-                geom.attrib["mesh"] = mesh_name
-
-            # if no collision shape
-            if collision is None:
-                geom.attrib["contype"] = "0"
-                geom.attrib["conaffinity"] = "0"
+                # if no collision shape
+                if collision is None:
+                    geom.attrib["contype"] = "0"
+                    geom.attrib["conaffinity"] = "0"

        # create <joint>
        for joint in body.parent_joints.values():  # parent_joints
@@ -5,7 +5,7 @@ Proto files are notably used in Webots.
 """

 from pyrobolearn.utils.parsers.robots.robot_parser import RobotParser
-from pyrobolearn.utils.parsers.robots.data_structures import Tree
+from pyrobolearn.utils.parsers.robots.data_structures import MultiBody


 __author__ = "Brian Delhaisse"
@@ -44,7 +44,7 @@ class ProtoParser(RobotParser):
        Return the Tree containing all the elements.

        Returns:
-            Tree: tree data structure.
+            MultiBody: tree data structure.
        """
        pass

@@ -53,7 +53,7 @@ class ProtoParser(RobotParser):
        Generate the XML tree from the `Tree` data structure.

        Args:
-            tree (Tree): Tree data structure.
+            tree (MultiBody): Tree data structure.

        Returns:
            ET.Element: root element in the XML file.
@@ -38,12 +38,18 @@ class RobotParser(object):
            self.dirname = os.path.dirname(filename) + '/'
            self.parse(filename)

+    ##############
+    # Properties #
+    ##############
+
    @property
    def root(self):
+        """Return the root XML tag element."""
        return self._root

    @root.setter
    def root(self, root):
+        """Set the root XML tag element."""
        if root is not None and not isinstance(root, ET.Element):
            raise TypeError("Expecting the root to be an instance of `ET.Element`, but got instead: "
                            "{}".format(type(root)))
@@ -51,24 +57,45 @@ class RobotParser(object):

    @property
    def tree(self):
+        """Return the tree / multi-body data structure."""
        return self._tree

    @tree.setter
    def tree(self, tree):
+        """Set the tree / multi-body data structure."""
        if tree is not None and not isinstance(tree, MultiBody):
            raise TypeError("Expecting the given tree to be an instance of `Tree`, but got instead: "
                            "{}".format(type(tree)))
        self._tree = tree

+    ###########
+    # Methods #
+    ###########
+
    def parse(self, filename):
        """
        Load and parse a given file.

        Args:
            filename (str): path to the file to parse.
+
+        Returns:
+            object: data structure that can be passed to other generators.
        """
        pass

+    # def parse_string(self, string):
+    #     """
+    #     Parse the given string.
+    #
+    #     Args:
+    #         string (str): string to parse.
+    #
+    #     Returns:
+    #         object: data structure that can be passed to other generators.
+    #     """
+    #     pass
+
    def get_tree(self):
        """
        Return the Tree containing all the elements.
@@ -108,10 +108,10 @@ class SDFParser(WorldParser):
            idx (int): model index.

        Returns:
-            Tree: tree data structure containing the model.
+            MultiBody: tree data structure containing the model.
        """
        # create tree
-        tree = Tree(name=model_tag.attrib.get('name', 'model_' + str(idx)))
+        tree = MultiBody(name=model_tag.attrib.get('name', 'model_' + str(idx)))

        # check bodies/links
        for i, link_tag in enumerate(model_tag.findall('link')):
@@ -97,10 +97,10 @@ class SkelParser(WorldParser):
            idx (int): skeleton index.

        Returns:
-            Tree: tree data structure containing the skeleton.
+            MultiBody: tree data structure containing the skeleton.
        """
        # create tree
-        tree = Tree(name=skeleton_tag.attrib.get('name', 'skeleton_' + str(idx)))
+        tree = MultiBody(name=skeleton_tag.attrib.get('name', 'skeleton_' + str(idx)))

        # check bodies/links
        for i, body_tag in enumerate(skeleton_tag.findall('body')):
@@ -0,0 +1,155 @@
+#!/usr/bin/env python
+"""Define the SRDF parser.
+
+The Semantic Robot Description Format (SRDF) format allows to represent semantic information about the robot
+structure [1]. Specifically, it allows to "specify joint groups, default robot configurations, additional collision
+checking information, and additional transforms that may be needed to completely specify the robot’s pose." [2]
+
+References:
+    - [1] SRDF: http://wiki.ros.org/srdf
+    - [2] URDF and SRDF: http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
+"""
+
+import numpy as np
+import xml.etree.ElementTree as ET
+
+
+__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 SRDFParser(object):
+    r"""SRDF Parser
+
+    The Semantic Robot Description Format (SRDF) format allows to represent semantic information about the robot
+    structure [1]. Specifically, it allows to "specify joint groups, default robot configurations, additional collision
+    checking information, and additional transforms that may be needed to completely specify the robot’s pose." [2]
+
+    Warnings: this currently only parse the groups and the group states!! Note that every name has been lower cased.
+
+    Example of a simple SRDF file from [1]:
+
+    <robot name="robot_name">
+        <group name="group_link_name1">
+            <chain base_link="base_link_name" tip_link="tip_link_name"/>
+        </group>
+        <group name="group_link_name2">
+            <group name="group_link_name1"/>
+            <link name="link_name"/>
+        </group>
+
+        <group_state name="home" group="group_name_to_use">
+            <joint name="joint_name1" value="joint_value1"/>
+            <joint name="joint_name2" value="joint_value2"/>
+            ...
+        </group_state>
+
+    </robot>
+
+    References:
+        - [1] SRDF: http://wiki.ros.org/srdf
+        - [2] URDF and SRDF: http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/urdf_srdf/urdf_srdf_tutorial.html
+    """
+
+    def __init__(self, filename=None):
+        """
+        Initialize the SRDF parser.
+
+        Args:
+            filename (str, None): path to the file to parse.
+        """
+        self.filename = filename
+        self.dirname = ''
+        if filename is not None:
+            self.dirname = os.path.dirname(filename) + '/'
+            self.parse(filename)
+
+    @staticmethod
+    def parse(filename):
+        """
+        Load and parse a given file.
+
+        Args:
+            filename (str): path to the file to parse.
+
+        Returns:
+            dict: dictionary containing the tags and their values (which are converted to their correct data type if
+              necessary).
+        """
+        # load and parse the XML file
+        tree_xml = ET.parse(filename)
+
+        # get the root
+        root = tree_xml.getroot()
+
+        results = {}
+
+        # parse <group> tags
+        for group_tag in root.findall('group'):
+            name = group_tag.attrib['name']
+            results.setdefault('group', {}).setdefault(name, {})
+            group = results['group'][name]
+
+            # parse each <link>
+            for link_tag in group_tag.findall('link'):
+                group.setdefault('link', []).append(link_tag.attrib['name'])
+
+            # parse each <chain>
+            for chain_tag in group_tag.findall('chain'):
+                group.setdefault('chain', []).append([chain_tag.attrib['base_link'], chain_tag.attrib['tip_link']])
+
+            # parse each <joint>
+            for joint_tag in group_tag.findall('joint'):
+                values = [float(c) for c in joint_tag.attrib['value'].split()]
+                values = values[0] if len(values) == 1 else np.array(values)
+                group.setdefault('joint', {})[joint_tag.attrib['name']] = values
+
+            # parse each <group>
+            for group_subtag in group_tag.findall('group'):
+                group.setdefault('group', []).append(group_subtag.attrib['name'])
+
+        # parse <group_state> tags
+        for group_state_tag in root.findall('group_state'):
+            name = group_state_tag.attrib['name'].lower()
+            group_name = group_state_tag.attrib['group']
+            results.setdefault('group_state', {}).setdefault(name, {})
+            group_state = results['group_state'][name]
+            group_state['group'] = group_name
+
+            # parse each <joint>
+            for joint_tag in group_state_tag.findall('joint'):
+                values = [float(c) for c in joint_tag.attrib['value'].split()]
+                values = values[0] if len(values) == 1 else np.array(values)
+                group_state[joint_tag.attrib['name']] = values
+
+        return results
+
+    # def parse_string(self, string):
+    #     """
+    #     Parse the given string.
+    #
+    #     Args:
+    #         string (str): string to parse.
+    #
+    #     Returns:
+    #         dict: dictionary containing the tags and their values.
+    #     """
+    #     return
+
+    # def _parse(self, root):
+    #     """
+    #     Parse the given root element.
+    #
+    #     Args:
+    #         root (ET.Element): root XML tag element.
+    #
+    #     Returns:
+    #         dict: dictionary containing the tags and their values.
+    #     """
+    #     pass
@@ -43,6 +43,9 @@ class URDFParser(RobotParser):

        Args:
            filename (str): path to the URDF XML file.
+
+        Returns:
+            MultiBody: multi-body / tree data structure representing the elements in the URDF file.
        """
        # load and parse the XML file
        tree_xml = ET.parse(filename)
@@ -72,7 +75,7 @@ class URDFParser(RobotParser):
        # check bodies / links
        for i, body_tag in enumerate(root.findall('link')):
            # get body instance from tag
-            body = self._check_body(tree, body_tag, idx=i)
+            body = self._parse_body(tree, body_tag, idx=i)

            # add body to tree
            tree.bodies[body.name] = body
@@ -80,7 +83,7 @@ class URDFParser(RobotParser):
        # check joints
        for i, joint_tag in enumerate(root.findall('joint')):
            # get joint instance from tag
-            joint = self._check_joint(joint_tag, idx=i, tree=tree)
+            joint = self._parse_joint(joint_tag, idx=i, tree=tree)

            # add joint in trees
            tree.joints[joint.name] = joint
@@ -104,7 +107,7 @@ class URDFParser(RobotParser):

        return tree

-    def _check_body(self, tree, body_tag, idx):
+    def _parse_body(self, tree, body_tag, idx):
        """
        Return Body instance from a <link> tag.

@@ -144,97 +147,99 @@ class URDFParser(RobotParser):
            # set inertial to body
            body.add_inertial(inertial)

-        # check <visual> tag
-        visual_tag = body_tag.find('visual')
-        if visual_tag is not None:
-            visual = Visual()
+        # check <visual> tag(s)
+        # visual_tag = body_tag.find('visual')
+        for visual_tag in body_tag.findall('visual'):
+            if visual_tag is not None:
+                visual = Visual()

-            # name
-            visual.name = visual_tag.attrib.get('name')
+                # name
+                visual.name = visual_tag.attrib.get('name')

-            # origin
-            origin_tag = visual_tag.find('origin')
-            if origin_tag is not None:
-                visual.position = origin_tag.attrib.get('xyz')
-                visual.orientation = origin_tag.attrib.get('rpy')
+                # origin
+                origin_tag = visual_tag.find('origin')
+                if origin_tag is not None:
+                    visual.position = origin_tag.attrib.get('xyz')
+                    visual.orientation = origin_tag.attrib.get('rpy')

-            # geometry
-            geometry_tag = visual_tag.find('geometry')
-            if geometry_tag is not None:
-                for geometry_type in {'box', 'mesh', 'cylinder', 'sphere'}:
-                    geometry_type_tag = geometry_tag.find(geometry_type)
-                    if geometry_type_tag is not None:
-                        dtype = geometry_type
-                        visual.dtype = dtype
-                        if dtype == 'box':
-                            visual.size = geometry_type_tag.attrib['size']
-                        elif dtype == 'sphere':
-                            visual.size = geometry_type_tag.attrib['radius']
-                        elif dtype == 'cylinder':
-                            visual.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
-                        elif dtype == 'mesh':
-                            visual.filename = self.dirname + geometry_type_tag.attrib['filename']
-                            visual.size = geometry_type_tag.attrib.get('scale')
+                # geometry
+                geometry_tag = visual_tag.find('geometry')
+                if geometry_tag is not None:
+                    for geometry_type in {'box', 'mesh', 'cylinder', 'sphere'}:
+                        geometry_type_tag = geometry_tag.find(geometry_type)
+                        if geometry_type_tag is not None:
+                            dtype = geometry_type
+                            visual.dtype = dtype
+                            if dtype == 'box':
+                                visual.size = geometry_type_tag.attrib['size']
+                            elif dtype == 'sphere':
+                                visual.size = geometry_type_tag.attrib['radius']
+                            elif dtype == 'cylinder':
+                                visual.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
+                            elif dtype == 'mesh':
+                                visual.filename = self.dirname + geometry_type_tag.attrib['filename']
+                                visual.size = geometry_type_tag.attrib.get('scale')

-            # material
-            material_tag = visual_tag.find('material')
-            if material_tag is not None:
-                material = Material()
-                name = material_tag.attrib.get('name')
-                color = material_tag.find('color')
-                texture = material_tag.find('texture')
-                if color is not None or texture is not None:
-                    material.name = name
-                    if color is not None:
-                        material.color = color.attrib['rgba']
-                    elif texture is not None:
-                        material.texture = self.dirname + texture.attrib['filename']
-                else:
-                    material = tree.materials.get(name)
-                visual.material = material
+                # material
+                material_tag = visual_tag.find('material')
+                if material_tag is not None:
+                    material = Material()
+                    name = material_tag.attrib.get('name')
+                    color = material_tag.find('color')
+                    texture = material_tag.find('texture')
+                    if color is not None or texture is not None:
+                        material.name = name
+                        if color is not None:
+                            material.color = color.attrib['rgba']
+                        elif texture is not None:
+                            material.texture = self.dirname + texture.attrib['filename']
+                    else:
+                        material = tree.materials.get(name)
+                    visual.material = material

-            # set visual to body
-            body.add_visual(visual)
+                # set visual to body
+                body.add_visual(visual)

-        # check <collision> tag
-        collision_tag = body_tag.find('collision')
-        if collision_tag is not None:
-            collision = Collision()
+        # check <collision> tag(s)
+        # collision_tag = body_tag.find('collision')
+        for collision_tag in body_tag.findall('collision'):
+            if collision_tag is not None:
+                collision = Collision()

-            # name
-            collision.name = collision_tag.attrib.get('name')
+                # name
+                collision.name = collision_tag.attrib.get('name')

-            # origin
-            origin_tag = collision_tag.find('origin')
-            if origin_tag is not None:
-                collision.position = origin_tag.attrib.get('xyz')
-                collision.orientation = origin_tag.attrib.get('rpy')
+                # origin
+                origin_tag = collision_tag.find('origin')
+                if origin_tag is not None:
+                    collision.position = origin_tag.attrib.get('xyz')
+                    collision.orientation = origin_tag.attrib.get('rpy')

-            # geometry
-            geometry_tag = collision_tag.find('geometry')
-            if geometry_tag is not None:
-                for geometry_type in {'box', 'mesh', 'cylinder', 'sphere'}:
-                    geometry_type_tag = geometry_tag.find(geometry_type)
-                    if geometry_type_tag is not None:
-                        dtype = geometry_type
-                        collision.dtype = dtype
-                        if dtype == 'box':
-                            collision.size = geometry_type_tag.attrib['size']
-                        elif dtype == 'sphere':
-                            collision.size = geometry_type_tag.attrib['radius']
-                        elif dtype == 'cylinder':
-                            collision.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
-                        elif dtype == 'mesh':
-                            collision.filename = self.dirname + geometry_type_tag.attrib['filename']
-                            collision.size = geometry_type_tag.attrib.get('scale')
+                # geometry
+                geometry_tag = collision_tag.find('geometry')
+                if geometry_tag is not None:
+                    for geometry_type in {'box', 'mesh', 'cylinder', 'sphere'}:
+                        geometry_type_tag = geometry_tag.find(geometry_type)
+                        if geometry_type_tag is not None:
+                            dtype = geometry_type
+                            collision.dtype = dtype
+                            if dtype == 'box':
+                                collision.size = geometry_type_tag.attrib['size']
+                            elif dtype == 'sphere':
+                                collision.size = geometry_type_tag.attrib['radius']
+                            elif dtype == 'cylinder':
+                                collision.size = (geometry_type_tag.attrib['radius'], geometry_type_tag.attrib['length'])
+                            elif dtype == 'mesh':
+                                collision.filename = self.dirname + geometry_type_tag.attrib['filename']
+                                collision.size = geometry_type_tag.attrib.get('scale')

-            # set collision to body
-            body.add_collision(collision)
+                # set collision to body
+                body.add_collision(collision)

        return body

    @staticmethod
-    def _check_joint(joint_tag, idx, tree):
+    def _parse_joint(joint_tag, idx, tree):
        """
        Return Joint instance from a <joint> tag.

@@ -385,38 +390,40 @@ class URDFParser(RobotParser):
                        inertia['iyz'] = str(I.iyz)
                    ET.SubElement(inertial_tag, 'inertia', attrib=inertia)

-            # create <visual> tag
-            visual = link.visual
-            if visual is not None:
-                # create visual tag with name
-                visual_tag = set_name(link_tag, 'visual', visual)
+            # create <visual> tags
+            # visual = link.visual
+            for visual in link.visuals:
+                if visual is not None:
+                    # create visual tag with name
+                    visual_tag = set_name(link_tag, 'visual', visual)

-                # <origin>
-                set_origin(visual_tag, visual)
+                    # <origin>
+                    set_origin(visual_tag, visual)

-                # <geometry>
-                set_geometry(visual_tag, visual)
+                    # <geometry>
+                    set_geometry(visual_tag, visual)

-                # <material>
-                if visual.material is not None:
-                    material = visual.material
-                    material_tag = ET.SubElement(visual_tag, 'material', attrib={'name': material.name})
-                    if material.color is not None:
-                        ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
-                    if material.texture is not None:
-                        ET.SubElement(material_tag, 'texture', attrib={'filename': material.texture})
+                    # <material>
+                    if visual.material is not None:
+                        material = visual.material
+                        material_tag = ET.SubElement(visual_tag, 'material', attrib={'name': material.name})
+                        if material.color is not None:
+                            ET.SubElement(material_tag, 'color', attrib={'rgba': str(np.asarray(material.rgba))[1:-1]})
+                        if material.texture is not None:
+                            ET.SubElement(material_tag, 'texture', attrib={'filename': material.texture})

-            # create <collision> tag
-            collision = link.collision
-            if collision is not None:
-                # create collision tag with name
-                collision_tag = set_name(link_tag, 'collision', collision)
+            # create <collision> tags
+            # collision = link.collision
+            for collision in link.collisions:
+                if collision is not None:
+                    # create collision tag with name
+                    collision_tag = set_name(link_tag, 'collision', collision)

-                # <origin>
-                set_origin(collision_tag, collision)
+                    # <origin>
+                    set_origin(collision_tag, collision)

-                # <geometry>
-                set_geometry(collision_tag, collision)
+                    # <geometry>
+                    set_geometry(collision_tag, collision)

        def set_name_and_type(parent_tag, tag, item):
            kwargs = {}
@@ -6,7 +6,7 @@
 import xml.etree.ElementTree as ET
 from xml.dom import minidom  # to print in a pretty way the XML file

-from pyrobolearn.utils.parsers.robots.data_structures import World, Tree
+# from pyrobolearn.utils.parsers.robots.data_structures import World, Tree

 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -215,8 +215,7 @@ class WorldParser(object):
        Returns:
            ET.Element: the new created element.
        """
-        element = ET.SubElement(parent_element, name, attrib=attributes)
-        return element
+        return ET.SubElement(parent_element, name, attrib=attributes)

    @staticmethod
    def remove_element(element, parent_element):
@@ -59,7 +59,7 @@ def get_adjoint_from_rotation(rotation_matrix):
    return block_diag(rotation_matrix, rotation_matrix)


-def get_homogeneous_transform(position, orientation):
+def get_homogeneous_matrix(position, orientation):
    r"""
    Return the Homogeneous transform matrix given the position vector and the orientation.

@@ -126,6 +126,17 @@ def get_inverse_homogeneous(matrix):
                      np.array([[0, 0, 0, 1]])))


+def get_identity_homogeneous_matrix():
+    r"""
+    Return the identity homogeneous matrix which corresponds to no rotation (rotation matrix is the identity matrix),
+    and no translation (the translation vector is equal to 0). Note that this is equivalent to `np.identity(4)`.
+
+    Returns:
+        np.array[float[4,4]]: identity homogeneous matrix.
+    """
+    return np.identity(4)
+
+
 def homogeneous_to_pose(matrix):
    r"""
    Return a pose (7D vector: position + quaternion) from a homogeneous matrix.
@@ -153,7 +164,7 @@ def pose_to_homogeneous(pose):
    """
    pose = np.array(pose).flatten()
    position, orientation = pose[:3], pose[-4:]
-    return get_homogeneous_transform(position=position, orientation=orientation)
+    return get_homogeneous_matrix(position=position, orientation=orientation)


 def get_quaternion(orientation, convert_to_quat=False, convention='xyzw'):
				`@@ -0,0 +1 @@`
				`This folder contains generated meshes when using some simulators that requires a specific format. You can remove them.`