From 4e7fb2220273c1d215fe900889b755024e74682c Mon Sep 17 00:00:00 2001
From: Brian Delhaisse <briandelhaisse@gmail.com>
Date: Thu, 12 Sep 2019 15:19:32 +0200
Subject: [PATCH] update parsers and simulators

---
 pyrobolearn/simulators/bullet.py              |  23 +-
 pyrobolearn/simulators/dart.py                | 302 +++++++--
 pyrobolearn/simulators/mujoco.py              | 615 ++++++++++++++----
 pyrobolearn/simulators/simulator.py           |  24 +-
 .../utils/parsers/robots/data_structures.py   | 145 ++++-
 .../utils/parsers/robots/mujoco_parser.py     |   3 +
 .../utils/parsers/robots/urdf_parser.py       |   2 +-
 pyrobolearn/worlds/world.py                   |   3 +-
 8 files changed, 883 insertions(+), 234 deletions(-)

diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py
index 7aae413..9c43b35 100644
--- a/pyrobolearn/simulators/bullet.py
+++ b/pyrobolearn/simulators/bullet.py
@@ -3323,7 +3323,8 @@ class Bullet(Simulator):
     def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
                         rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
-                        local_inertia_diagonal=None, joint_damping=None):
+                        local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
+                        joint_damping=None, joint_friction=None):
         """
         Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
@@ -3334,21 +3335,25 @@ class Bullet(Simulator):
             lateral_friction (float): lateral (linear) contact friction
             spinning_friction (float): torsional friction around the contact normal
             rolling_friction (float): torsional friction orthogonal to contact normal
-            restitution (float): bouncyness of contact. Keep it a bit less than 1.
+            restitution (float): bounciness of contact. Keep it a bit less than 1.
             linear_damping (float): linear damping of the link (0.04 by default)
             angular_damping (float): angular damping of the link (0.04 by default)
             contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
             contact_damping (float): damping of the contact constraints for this body/link. Used together with
-                `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
-                section.
+              `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
+              section.
             friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
-                default, unless set in the URDF contact section)
+              default, unless set in the URDF contact section)
             local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
-                and links are centered around the center of mass and aligned with the principal axes of inertia so
-                there are no off-diagonal elements in the inertia tensor.
+              and links are centered around the center of mass and aligned with the principal axes of inertia so
+              there are no off-diagonal elements in the inertia tensor.
+            inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
+            inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
+              with respect to the link frame.
             joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
-                joint damping field. Keep the value close to 0.
-                `joint_damping_force = -damping_coefficient * joint_velocity`.
+              joint damping field. Keep the value close to 0.
+              `joint_damping_force = -damping_coefficient * joint_velocity`.
+            joint_friction (float): joint friction coefficient.
         """
         kwargs = {}
         if mass is not None:
diff --git a/pyrobolearn/simulators/dart.py b/pyrobolearn/simulators/dart.py
index dbd6842..d1addcd 100644
--- a/pyrobolearn/simulators/dart.py
+++ b/pyrobolearn/simulators/dart.py
@@ -303,7 +303,7 @@ class Dart(Simulator):
         self.collision_shapes = {}  # {collision_id: Collision}
         self._bodies = OrderedDict()  # {body_id: Body}
         self.textures = {}  # {texture_id: Texture}
-        self.constraints = OrderedDict()  # {constraint_id: Constraint}
+        self._constraints = OrderedDict()  # {constraint_id: Constraint}
 
         # create counters
         self._visual_cnt = 0
@@ -367,6 +367,74 @@ class Dart(Simulator):
     # Methods #
     ###########
 
+    ###########
+    # Private #
+    ###########
+
+    @staticmethod
+    def _convert_wxyz_to_xyzw(q):
+        """Convert a quaternion in the (w,x,y,z) format to (x,y,z,w)."""
+        return np.roll(q, shift=-1)
+
+    @staticmethod
+    def _convert_xyzw_to_wxyz(q):
+        """Convert a quaternion in the (x,y,z,w) format to (w,x,y,z)."""
+        return np.roll(q, shift=1)
+
+    @staticmethod
+    def _get_matrix_from_transform(transform):
+        """Return the homogeneous matrix from the given transform.
+
+        Args:
+            transform (dartpy.math.Isometry3): transform.
+
+        Returns:
+            np.array[float[4,4]]: homogeneous matrix.
+        """
+        rot = transform.rotation()
+        pos = transform.translation()
+        return np.vstack((np.hstack((rot, pos.reshape(-1, 1))),
+                          np.array([0., 0., 0., 1.])))
+
+    @staticmethod
+    def _get_pose_from_transform(transform):
+        """Return the pose from the transform.
+
+        Args:
+            transform (dartpy.math.Isometry3): transform.
+
+        Returns:
+            np.array[float[3]]: position vector.
+            np.array[float[4]]: quaternion (expressed as [x,y,z,w]).
+        """
+        quat = Dart._convert_wxyz_to_xyzw(transform.quaternion().wxyz())
+        pos = transform.translation()
+        return pos, quat
+
+    @staticmethod
+    def _get_quat_from_transform(transform):
+        """Return the quaternion from the given transform.
+
+        Args:
+            transform (dartpy.math.Isometry3): transform.
+
+        Returns:
+            np.array[float[4]]: quaternion (expressed as [x,y,z,w]).
+        """
+        return Dart._convert_wxyz_to_xyzw(transform.quaternion().wxyz())
+
+    @staticmethod
+    def _get_pos_from_transform(transform):
+        """Return the position from the given transform.
+
+        Args:
+            transform (dartpy.math.Isometry3): transform.
+
+        Returns:
+            np.array[float[3]]: position vector.
+        """
+        return transform.translation()
+
     ##############
     # Simulators #
     ##############
@@ -569,13 +637,17 @@ class Dart(Simulator):
         skeleton = self._urdf_parser.parseSkeleton(filename)
         self.world.addSkeleton(skeleton)
         rpy = get_rpy_from_quaternion(orientation) if orientation is not None else None
-        for i in range(6):
-            if i < 3:
-                if rpy is not None:
-                    skeleton.setPosition(index=i, position=rpy[i])
-            else:
-                if position is not None:
-                    skeleton.setPosition(index=i, position=position[i-3])
+
+        # set orientation
+        if rpy is not None:
+            for i in range(3):
+                skeleton.setPosition(index=i, position=rpy[i])
+
+        # set position
+        if position is not None:
+            for i in range(3):
+                skeleton.setPosition(index=i+3, position=position[i])
+
         return self.world.getNumSkeletons() - 1
 
     def load_sdf(self, filename, scaling=1., *args, **kwargs):
@@ -774,14 +846,12 @@ class Dart(Simulator):
         dynamics_aspect = shape_node.createDynamicsAspect()
 
         # set the position and orientation
-        if mass != 0:
-            orientation = get_rpy_from_quaternion(orientation) if orientation is not None else None
-            for i in range(6):
-                if i < 3:
-                    if orientation is not None:
-                        joint.setPosition(index=i, position=orientation[i])
-                else:
-                    joint.setPosition(index=i, position=position[i-3])
+        orientation = get_rpy_from_quaternion(orientation) if orientation is not None else None
+        if orientation is not None:
+            for i in range(3):
+                joint.setPosition(i, orientation[i])
+        for i in range(3):
+            joint.setPosition(i+3, position[i])
 
         # # increment body counter and remember the body
         # self._body_cnt += 1
@@ -855,11 +925,13 @@ class Dart(Simulator):
     def get_body_info(self, body_id):
         """Get the specified body information.
 
+        Specifically, it returns the base name extracted from the URDF, SDF, MJCF, or other file.
+
         Args:
             body_id (int): unique body id.
 
         Returns:
-            dict, list: info
+            str: base name
         """
         pass
 
@@ -872,7 +944,7 @@ class Dart(Simulator):
         Returns:
             int: unique body id.
         """
-        pass
+        return index
 
     ###############
     # constraints #
@@ -909,7 +981,27 @@ class Dart(Simulator):
         # ['BallJointConstraint', 'BoxedLcpConstraintSolver', 'BoxedLcpSolver', 'ConstraintBase', 'ConstraintSolver',
         # 'DantzigBoxedLcpSolver', 'JointConstraint', 'JointCoulombFrictionConstraint', 'JointLimitConstraint',
         # 'PgsBoxedLcpSolver', 'PgsBoxedLcpSolverOption', 'WeldJointConstraint']
-        pass
+
+        parent = self.world.getSkeleton(parent_body_id)
+        parent_node = parent.getBodyNode(parent_link_id + 1)
+
+        if child_body_id != -1:  # if child is a skeleton
+            child = self.world.getSkeleton(child_body_id)
+            child_node = child.getBodyNode(child_link_id + 1)
+        else:  # if child is the world
+            pass
+
+        # create constraint based on the type
+        if joint_type == Simulator.JOINT_FIXED:
+            constraint = dart.constraint.WeldJointConstraint(parent_node, child_node)
+
+        self._constraint_cnt += 1
+        self._constraints[self._constraint_cnt] = constraint
+
+        # add constraint
+        self.world.getConstraintSolver().addConstraint(constraint)
+
+        return self._constraint_cnt
 
     def remove_constraint(self, constraint_id):
         """
@@ -918,7 +1010,8 @@ class Dart(Simulator):
         Args:
             constraint_id (int): constraint unique id.
         """
-        pass
+        constraint = self._constraints.pop(constraint_id)
+        self.world.getConstraintSolver().removeConstraint(constraint)
 
     def change_constraint(self, constraint_id, *args, **kwargs):
         """
@@ -927,6 +1020,7 @@ class Dart(Simulator):
         Args:
             constraint_id (int): constraint unique id.
         """
+        constraint = self._constraints.pop(constraint_id)
         pass
 
     def num_constraints(self):
@@ -936,7 +1030,7 @@ class Dart(Simulator):
         Returns:
             int: number of constraints created.
         """
-        pass
+        return len(self._constraints)
 
     def get_constraint_id(self, index):
         """
@@ -948,7 +1042,7 @@ class Dart(Simulator):
         Returns:
             int: constraint unique id.
         """
-        pass
+        return list(self._constraints.keys())[index]
 
     def get_constraint_info(self, constraint_id):
         """
@@ -1028,18 +1122,18 @@ class Dart(Simulator):
         skeleton = self.world.getSkeleton(body_id)
 
         if link_ids is None:
-            com = skeleton.getCOM()  # (3,1)
-            return com.reshape(-1)
+            return skeleton.getCOM()
 
-        # if isinstance(link_ids, int):
-        #     link_ids = [link_ids]
-        #
-        # coms = []
-        # for link_id in link_ids:
-        #     body = skeleton.getBodyNode(link_id + 1)
-        #
-        #
-        return None  # TODO
+        if isinstance(link_ids, int):
+            return skeleton.getBodyNode(link_ids + 1).getCOM()
+
+        com = 0
+        for link_id in link_ids:
+            body = skeleton.getBodyNode(link_id + 1)
+            com += body.getCOM * body.getMass()
+        com /= skeleton.getMass()
+
+        return com
 
     def get_center_of_mass_velocity(self, body_id, link_ids=None):
         """
@@ -1056,10 +1150,17 @@ class Dart(Simulator):
         skeleton = self.world.getSkeleton(body_id)
 
         if link_ids is None:
-            com_vel = skeleton.getCOMLinearVelocity()  # (3,1)
-            return com_vel.reshape(-1)
+            return skeleton.getCOMLinearVelocity()
 
-        return None  # TODO
+        if isinstance(link_ids, int):
+            return skeleton.getBodyNode(link_ids + 1).getCOMLinearVelocity()
+
+        vel = 0
+        for link_id in link_ids:
+            body = skeleton.getBodyNode(link_id + 1)
+            vel += body.getCOMLinearVelocity() * body.getMass()
+        vel /= skeleton.getMass()
+        return vel
 
     def get_base_pose(self, body_id):
         """
@@ -1074,9 +1175,9 @@ class Dart(Simulator):
         """
         base = self.world.getSkeleton(body_id).getRootBodyNode()
         transform = base.getWorldTransform()
-        # position = transform[:-1, 3]
-        position = base.getCOM().reshape(-1)
-        orientation = get_quaternion_from_matrix(transform[:-1, :-1])
+        # position = self._get_pos_from_transform(transform)
+        position = base.getCOM()
+        orientation = self._get_quat_from_transform(transform)
         return position, orientation
 
     def get_base_position(self, body_id):
@@ -1090,8 +1191,8 @@ class Dart(Simulator):
             np.array[float[3]]: base position.
         """
         base = self.world.getSkeleton(body_id).getRootBodyNode()
-        # return base.getWorldTransform()[:-1, 3]
-        return base.getCOM().reshape(-1)
+        # return self._get_pos_from_transform(base.getWorldTransform())
+        return base.getCOM()
 
     def get_base_orientation(self, body_id):
         """
@@ -1105,7 +1206,7 @@ class Dart(Simulator):
         """
         base = self.world.getSkeleton(body_id).getRootBodyNode()
         transform = base.getWorldTransform()
-        return get_quaternion_from_matrix(transform[:-1, :-1])
+        return self._get_quat_from_transform(transform)
 
     def reset_base_pose(self, body_id, position, orientation):
         """
@@ -1116,7 +1217,13 @@ class Dart(Simulator):
             position (np.array[float[3]]): new base position.
             orientation (np.array[float[4]]): new base orientation (expressed as a quaternion [x,y,z,w])
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        rpy = get_rpy_from_quaternion(orientation)
+        for i in range(6):
+            if i < 3:
+                skeleton.setPosition(index=i, position=rpy[i])
+            else:
+                skeleton.setPosition(index=i, position=position[i - 3])
 
     def reset_base_position(self, body_id, position):
         """
@@ -1126,7 +1233,9 @@ class Dart(Simulator):
             body_id (int): unique object id.
             position (np.array[float[3]]): new base position.
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        for i in range(3):
+            skeleton.setPosition(index=i+3, position=position[i])
 
     def reset_base_orientation(self, body_id, orientation):
         """
@@ -1136,7 +1245,10 @@ class Dart(Simulator):
             body_id (int): unique object id.
             orientation (np.array[float[4]]): new base orientation (expressed as a quaternion [x,y,z,w])
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        rpy = get_rpy_from_quaternion(orientation)
+        for i in range(3):
+            skeleton.setPosition(index=i, position=rpy[i])
 
     def get_base_velocity(self, body_id):
         """
@@ -1191,7 +1303,9 @@ class Dart(Simulator):
             linear_velocity (np.array[float[3]]): new linear velocity of the base.
             angular_velocity (np.array[float[3]]): new angular velocity of the base.
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        for i in range(3):
+            skeleton.setVelocity(index=i + 3, velocity=linear_velocity[i])
 
     def reset_base_linear_velocity(self, body_id, linear_velocity):
         """
@@ -1201,7 +1315,9 @@ class Dart(Simulator):
             body_id (int): unique object id.
             linear_velocity (np.array[float[3]]): new linear velocity of the base
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        for i in range(3):
+            skeleton.setVelocity(index=i+3, velocity=linear_velocity[i])
 
     def reset_base_angular_velocity(self, body_id, angular_velocity):
         """
@@ -1211,7 +1327,9 @@ class Dart(Simulator):
             body_id (int): unique object id.
             angular_velocity (np.array[float[3]]): new angular velocity of the base
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        for i in range(3):
+            skeleton.setVelocity(index=i, velocity=angular_velocity[i])
 
     def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.), frame=1):
         """
@@ -1229,7 +1347,7 @@ class Dart(Simulator):
         link = self.world.getSkeleton(body_id).getBodyNode(link_id + 1)
         force = np.asarray(force).reshape(-1, 1)  # (3,1)
         offset = np.asarray(position).reshape(-1, 1)  # (3,1)
-        is_local = (frame == 1)
+        is_local = (frame == Simulator.LINK_FRAME)
         link.setExtForce(force=force, offset=offset, isForceLocal=is_local, isOffsetLocal=is_local)
 
     def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1):
@@ -1246,7 +1364,7 @@ class Dart(Simulator):
         """
         link = self.world.getSkeleton(body_id).getBodyNode(link_id + 1)
         torque = np.asarray(torque).reshape(-1, 1)  # (3,1)
-        is_local = (frame == 1)
+        is_local = (frame == Simulator.LINK_FRAME)
         link.setExtForce(torque=torque, isLocal=is_local)
 
     ###################
@@ -1331,7 +1449,40 @@ class Dart(Simulator):
             [15] np.array[float[4]]:  joint orientation in parent frame
             [16] int:       parent link index, -1 for base
         """
-        pass
+        skeleton = self.world.getSkeleton(body_id)
+        joint = skeleton.getJoint(joint_id + 1)
+
+        name = joint.getName()
+
+        if isinstance(joint, dart.dynamics.FreeJoint):
+            joint_type = Simulator.JOINT_FREE
+        elif isinstance(joint, dart.dynamics.WeldJoint):
+            joint_type = Simulator.JOINT_FIXED
+        elif isinstance(joint, dart.dynamics.RevoluteJoint):
+            joint_type = Simulator.JOINT_REVOLUTE
+        elif isinstance(joint, dart.dynamics.BallJoint):
+            joint_type = Simulator.JOINT_SPHERICAL
+        elif isinstance(joint, dart.dynamics.PrismaticJoint):
+            joint_type = Simulator.JOINT_PRISMATIC
+        else:
+            joint_type = -1
+
+        q_idx = joint.getIndexInTree(joint_id)  # or joint.getIndexInSkeleton(joint_id)
+        dq_idx = 0
+        flag = -1
+        damping = joint.getDampingCoefficient()
+        friction = joint.getCoulombFriction()
+        pos_limits = (joint.getPositionLowerLimit, joint.getPositionUpperLimit)
+        force_limits = (joint.getForceLowerLimit, joint.getForceUpperLimit)
+        vel_limits = (joint.getVelocityLowerLimit, joint.getVelocityUpperLimit)
+        link_name = joint.getChildBodyNode().getName()
+        axis = joint.getAxis() if hasattr(joint, 'getAxis') else np.zeros(3)
+
+        transform = joint.getTransformFromParentBodyNode()
+        pos, quat = self._get_pose_from_transform(transform)
+
+        return joint_id, name, joint_type, q_idx, dq_idx, flag, damping, friction, pos_limits, force_limits, \
+               vel_limits, link_name, axis, pos, quat
 
     def get_joint_state(self, body_id, joint_id):
         """
@@ -1351,9 +1502,10 @@ class Dart(Simulator):
                 is exactly what you provide, so there is no need to report it separately.
         """
         joint = self.world.getSkeleton(body_id).getJoint(joint_id + 1)
+        body = joint.getChildBodyNode()
         position = joint.getPosition(0)  # joints can have less or more than 1 DoF (like WeldJoint, FreeJoint)
         velocity = joint.getVelocity(0)
-        reaction_forces = np.zeros(6)    # TODO
+        reaction_forces = body.getExternalForceGlobal()
         torque = joint.getForce(0)
         return position, velocity, reaction_forces, torque
 
@@ -1379,7 +1531,7 @@ class Dart(Simulator):
             return self.get_joint_state(body_id, joint_ids)
         return [self.get_joint_state(body_id, joint_id) for joint_id in joint_ids]
 
-    def reset_joint_state(self, body_id, joint_id, position, velocity=0.):
+    def reset_joint_state(self, body_id, joint_id, position, velocity=None):
         """
         Reset the state of the joint. It is best only to do this at the start, while not running the simulation:
         `reset_joint_state` overrides all physics simulation.
@@ -1408,7 +1560,7 @@ class Dart(Simulator):
         """
         pass
 
-    def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None,
+    def set_joint_motor_control(self, body_id, joint_ids, control_mode=Simulator.POSITION_CONTROL, positions=None,
                                 velocities=None, forces=None, kp=None, kd=None, max_velocity=None):
         r"""
         Set the joint motor control.
@@ -2142,7 +2294,16 @@ class Dart(Simulator):
             if multiple joints:
                 np.array[float[N,6]]: joint reaction forces [N, Nm]
         """
-        pass
+        if isinstance(joint_ids, int):
+            joint = self.world.getSkeleton(body_id).getJoint(joint_ids + 1)
+            body = joint.getChildBodyNode()
+            return body.getExternalForceGlobal()
+        forces = []
+        for joint_id in joint_ids:
+            joint = self.world.getSkeleton(body_id).getJoint(joint_id + 1)
+            body = joint.getChildBodyNode()
+            forces.append(body.getExternalForceGlobal())
+        return np.array(forces)
 
     def get_joint_powers(self, body_id, joint_ids):
         """Return the applied power at the given joint(s). Power = torque * velocity.
@@ -2709,13 +2870,14 @@ class Dart(Simulator):
         damping = -1
         stiffness = -1
 
-        return [mass, friction, local_inertia_diag, position, orientation, restitution, rolling_friction,
-                spinning_friction, damping, stiffness]
+        return mass, friction, local_inertia_diag, position, orientation, restitution, rolling_friction, \
+               spinning_friction, damping, stiffness
 
     def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
                         rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
-                        local_inertia_diagonal=None, joint_damping=None):
+                        local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
+                        joint_damping=None, joint_friction=None):
         """
         Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
@@ -2726,21 +2888,25 @@ class Dart(Simulator):
             lateral_friction (float): lateral (linear) contact friction
             spinning_friction (float): torsional friction around the contact normal
             rolling_friction (float): torsional friction orthogonal to contact normal
-            restitution (float): bouncyness of contact. Keep it a bit less than 1.
+            restitution (float): bounciness of contact. Keep it a bit less than 1.
             linear_damping (float): linear damping of the link (0.04 by default)
             angular_damping (float): angular damping of the link (0.04 by default)
             contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
             contact_damping (float): damping of the contact constraints for this body/link. Used together with
-                `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
-                section.
+              `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
+              section.
             friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
-                default, unless set in the URDF contact section)
+              default, unless set in the URDF contact section)
             local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
-                and links are centered around the center of mass and aligned with the principal axes of inertia so
-                there are no off-diagonal elements in the inertia tensor.
+              and links are centered around the center of mass and aligned with the principal axes of inertia so
+              there are no off-diagonal elements in the inertia tensor.
+            inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
+            inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
+              with respect to the link frame.
             joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
-                joint damping field. Keep the value close to 0.
-                `joint_damping_force = -damping_coefficient * joint_velocity`.
+              joint damping field. Keep the value close to 0.
+              `joint_damping_force = -damping_coefficient * joint_velocity`.
+            joint_friction (float): joint friction coefficient.
         """
         skeleton = self.world.getSkeleton(body_id)
         body = skeleton.getBodyNode(link_id + 1)
@@ -2764,8 +2930,8 @@ class Dart(Simulator):
         if joint_damping is not None:
             joint.setDampingCoefficient(joint_damping)
 
-        # if joint_friction is not None:
-        #     joint.setCoulombFriction(joint_friction)
+        if joint_friction is not None:
+            joint.setCoulombFriction(joint_friction)
 
     def calculate_jacobian(self, body_id, link_id, local_position, q, dq=None, des_ddq=None):
         r"""
diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py
index 79d192e..8916115 100644
--- a/pyrobolearn/simulators/mujoco.py
+++ b/pyrobolearn/simulators/mujoco.py
@@ -77,6 +77,9 @@ 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, get_quaternion_from_matrix
+from pyrobolearn.utils.parsers.robots.data_structures import transform_child_joint_frame_to_parent_inertial_frame, \
+    transform_inertial_frame_to_child_inertial_frame, transform_inertial_frame_to_child_link_frame, \
+    transform_inertial_frame_to_joint_frame
 
 
 # check Python version
@@ -127,7 +130,7 @@ class Body(object):
 
         # check given body
         if body is None:
-            body = struct.MultiBody()
+            body = struct.MultiBody(name='prl_dummy_' + str(body_id), root=struct.Body(body_id=body_id))
         if not isinstance(body, struct.MultiBody):
             raise TypeError("Expecting the given 'body' to be an instance of `MultiBody` but got instead: "
                             "{}".format(type(body)))
@@ -148,6 +151,7 @@ class Body(object):
         self._b_idx0, self._b_idxf = 0, 0  # initial and final body indices
         self._j_idx0, self._j_idxf = 0, 0  # initial and final free joint indices
         self._v_idx0, self._v_idxf = 0, 0  # initial and final dq (velocity) indices
+        self._q_idx1, self._v_idx1 = 0, 0  # initial q and dq indices (which don't take into account virtual joints)
 
         # keep in memory the body
         # self.body = body
@@ -164,6 +168,9 @@ class Body(object):
                 idx += 1
         self.jnt_to_q = np.array(jnt_to_q)
 
+        # keep in memory the link ids
+
+
     @property
     def num_links(self):
         """Alias to `num_bodies`."""
@@ -182,6 +189,12 @@ class Body(object):
             raise ValueError("Error while setting the initial q index, this index has to be bigger than 0!")
         self._q_idx0 = q
         self._q_idxf = q + self.q_length  # set the final q index
+        self._q_idx1 = q if self.fixed else q + 7  # set the initial q index (doesn't take into account virtual joints)
+
+    @property
+    def q_idx1(self):
+        """Return the initial q index that does not take into account the virtual joints for the base."""
+        return self._q_idx1
 
     @property
     def q_idxf(self):
@@ -277,6 +290,12 @@ class Body(object):
             raise ValueError("Error while setting the initial velocity index, this index has to be bigger than 0!")
         self._v_idx0 = v
         self._v_idxf = v + self.num_dofs  # set the final velocity index
+        self._v_idx1 = v if self.fixed else v + 6  # set the initial q index (doesn't take into account virtual joints)
+
+    @property
+    def v_idx1(self):
+        """Return the initial velocity index that does not take into account the virtual joints for the base."""
+        return self._v_idx1
 
     @property
     def v_idxf(self):
@@ -312,25 +331,43 @@ class Body(object):
         if keep:  # keep fixed joints (-1)
             return q
         if isinstance(q, float):
-            if q!=-1:
+            if q != -1:
                 return q
             return []
-        return q[q!=-1]  # remove fixed joints
+        return q[q != -1]  # remove fixed joints
 
     def get_dq_idx(self, joint_id, keep=False):
         return self.get_q_idx(joint_id, keep)
 
-    def check_joint_id(self, joint_id):
-        if joint_id < 0 or joint_id > (self.num_joints - 1):
-            raise ValueError("joint_id should belong to [0, `num_joints-1`].")
-
-    def check_link_id(self, link_id):
-        if link_id < -1 or link_id > (self.num_bodies - 2):  # -1 is for the base
-            raise ValueError("link_id should belong to [-1, `num_links-2`].")
+    def get_joint(self, joint_id):
+        return self.joints[joint_id]
 
     def get_joint_type(self, joint_id):
         return self.joints[joint_id].dtype
 
+    def get_link(self, link_id):
+        return self.links[link_id]
+
+    def transform_inertial_frame_to_joint_frame(self, body_id):
+        """Return the homogeneous transform from the inertial frame to the joint frame."""
+        body = self.links[body_id]
+        return transform_inertial_frame_to_joint_frame(body)
+
+    def transform_child_joint_frame_to_parent_inertial_frame(self, child_body_id):
+        """Return the homogeneous transform from the child joint frame to the parent inertial frame."""
+        body = self.links[child_body_id]
+        return transform_child_joint_frame_to_parent_inertial_frame(body)
+
+    def transform_inertial_frame_to_child_link_frame(self, child_body_id):
+        """Return the homogeneous transform from the parent inertial frame to the child link/joint frame."""
+        body = self.links[child_body_id]
+        return transform_inertial_frame_to_child_link_frame(body)
+
+    def transform_inertial_frame_to_child_inertial_frame(self, child_body_id):
+        """Return the homogeneous transform from the parent inertial frame to the child inertial frame."""
+        body = self.links[child_body_id]
+        return transform_inertial_frame_to_child_inertial_frame(body)
+
 
 class Mujoco(Simulator):
     r"""Mujoco Simulator interface.
@@ -507,6 +544,8 @@ class Mujoco(Simulator):
               If None, it will take the root defined in the simulator.
             render (bool): if we should render or not.
         """
+        # self.render(enable=False)  # to delete the previous viewer instance if defined
+
         # create the model
         # self.model = mujoco.load_model_from_path(path)
         root = self._parser.get_string(pretty_format=False)
@@ -517,7 +556,6 @@ class Mujoco(Simulator):
 
         # if we need to render
         if render:
-            # self.render(enable=False)  # to delete the previous viewer instance if defined
             # self.render(enable=True)   # to instantiate the viewer
             if self.viewer is None:
                 self.render(enable=True)
@@ -525,6 +563,63 @@ class Mujoco(Simulator):
                 print("Update the viewer's sim")
                 self.viewer.update_sim(self.sim)
 
+    @staticmethod
+    def _check_joint_id(body, joint_id):
+        if not isinstance(joint_id, int):
+            raise TypeError("Expecting the given joint id to be an int, but got instead: {}".format(type(joint_id)))
+        if joint_id < 0 or joint_id > (body.num_joints - 1):
+            raise ValueError("joint_id should belong to [0, {}], but got: {}".format(body.num_joints - 1, joint_id))
+        return joint_id
+
+    @staticmethod
+    def _check_joint_ids(body, joint_ids):
+        joint_ids = np.asarray(joint_ids)
+        if np.any(joint_ids < 0) or np.any(joint_ids > (body.num_joints - 1)):
+            raise ValueError("joint_ids should belong to [0, {}], but got: {}".format(body.num_joints - 1, joint_ids))
+        if joint_ids.ndim == 0:
+            return int(joint_ids)
+        return joint_ids
+
+    @staticmethod
+    def _check_link_id(body, link_id):
+        if not isinstance(link_id, int):
+            raise TypeError("Expecting the given link id to be an int, but got instead: {}".format(type(link_id)))
+        if link_id < -1 or link_id > (body.num_bodies - 2):  # -1 is for the base
+            raise ValueError("link_id should belong to [-1, {}], but got: {}".format(body.num_bodies - 2, link_id))
+        return link_id + 1  # shift such that between [0, `body.num_bodies`[.
+
+    @staticmethod
+    def _check_link_ids(body, link_ids):
+        link_ids = np.asarray(link_ids)
+        if np.any(link_ids < -1) or np.any(link_ids > (body.num_bodies - 2)):  # -1 is for the base
+            raise ValueError("link_ids should belong to [-1, {}], but got: {}".format(body.num_bodies - 2, link_ids))
+        if link_ids.ndim == 0:
+            return int(link_ids) + 1
+        return link_ids + 1  # shift such that between [0, `body.num_bodies`[.
+
+    @staticmethod
+    def _get_joint_type_id(joint_type):
+        if joint_type == 'fixed':
+            return Simulator.JOINT_FIXED
+        if joint_type == 'revolute':
+            return Simulator.JOINT_REVOLUTE
+        if joint_type == 'prismatic':
+            return Simulator.JOINT_PRISMATIC
+        elif joint_type == 'ball':
+            return Simulator.JOINT_SPHERICAL
+        elif joint_type == 'floating':
+            return Simulator.JOINT_FREE
+        elif joint_type == 'gear':
+            return Simulator.JOINT_GEAR
+        else:
+            return -1
+
+    @staticmethod
+    def _process_name(name):
+        if name.startswith('prl_'):
+            return '_'.join(name.split('_')[1:-1])
+        return name
+
     #################
     # utils methods #
     #################
@@ -532,12 +627,14 @@ class Mujoco(Simulator):
     @staticmethod
     def _convert_wxyz_to_xyzw(q):
         """Convert a quaternion in the (w,x,y,z) format to (x,y,z,w)."""
-        return np.roll(q, shift=-1)
+        q = np.asarray(q)
+        return np.roll(q, shift=-1, axis=q.ndim - 1)
 
     @staticmethod
     def _convert_xyzw_to_wxyz(q):
         """Convert a quaternion in the (x,y,z,w) format to (w,x,y,z)."""
-        return np.roll(q, shift=1)
+        q = np.asarray(q)
+        return np.roll(q, shift=1, axis=q.ndim - 1)
 
     ##############
     # Simulators #
@@ -742,7 +839,7 @@ class Mujoco(Simulator):
             body.add_parent_joint(joint)
             tree.add_joint(joint, idx=0)
 
-        return self._create_body(tree, verbose=2)
+        return self._create_body(tree, verbose=1)
 
     def load_sdf(self, filename, scaling=1., *args, **kwargs):  # TODO
         """Load a SDF file in the simulator.
@@ -870,9 +967,10 @@ class Mujoco(Simulator):
             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
+            # self._model_changed = True
+            self._create_sim()
 
-    def _create_body(self, tree, body_id=None, verbose=2):
+    def _create_body(self, tree, body_id=None, 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
@@ -972,7 +1070,7 @@ class Mujoco(Simulator):
         if not static:
             tree.add_joint(joint)
 
-        return self._create_body(tree, body_id=self._body_cnt, verbose=2)
+        return self._create_body(tree, body_id=self._body_cnt, verbose=1)
 
     def remove_body(self, body_id):  # DONE
         """Remove a particular body in the simulator.
@@ -1538,7 +1636,7 @@ class Mujoco(Simulator):
         Returns:
             int: number of links with the associated body id.
         """
-        return self._bodies[body_id].num_links
+        return self._bodies[body_id].num_links - 1  # remove the base link
 
     def get_joint_info(self, body_id, joint_id):
         """
@@ -1576,33 +1674,67 @@ class Mujoco(Simulator):
         """
         body = self._bodies[body_id]
 
-        if joint_id < 0 or joint_id > (body.num_joints - 1):
-            raise ValueError("joint_id should belong to [0, `num_joints-1`].")
+        # check joint id
+        joint_id = self._check_joint_id(body, joint_id)
+        joint = body.get_joint(joint_id)
 
-        dtype = body.get_joint_type(joint_id)
-        if dtype == 'fixed':  # special care for fixed joints (as they are usually not specified in Mujoco models)
-            pass
-        idx = body.j_idx0 + joint_id
+        name = self._process_name(joint.name)
+        dtype = self._get_joint_type_id(joint.dtype)
 
-        name = self.model.joint_id2name()
-        dtype = self.model.jnt_type  # ['free', 'ball', 'slide', 'hinge']
-        q_idx = body.get_q_idx(joint_id)
-        dq_idx = body.get_dq_idx(joint_id)
-        flag = -1
-        damping = self.model.dof_damping
-        friction = self.model.dof_frictionloss
-        limited = self.model.limited
-        limits = self.model.jnt_range
-        axis = self.model.jnt_axis
-        pos = self.model.jnt_pos
-        orientation = 0
+        # get index
+        q = body.get_q_idx(joint_id, keep=True)  # -1 for fixed joint
+        flag = 1  # as in Bullet
+        if joint_id == body.num_joints - 1:
+            flag = 0
 
-        axis_pos = self.sim.data.xaxis
-        # xanchor
+        if q == -1:  # fixed joint
+            q_idx = -1
+            dq_idx = -1
+            damping = 0.
+            friction = 0.
+            axis = np.zeros(3)
+            lower, upper = 0., -1.
+            max_vel = 0.
+            max_force = 0.
 
-        # stiffness
+        else:
+            joint_idx = body.j_idx0 + q
+            q_idx = q
+            dq_idx = q
+            # if not body.fixed:  # compatible with Bullet
+            q_idx += 7
+            dq_idx += 6
+            dof_addr = self.model.jnt_dofadr[joint_idx]
+            damping = self.model.dof_damping[dof_addr]
+            friction = self.model.dof_frictionloss[dof_addr]
+            axis = self.model.jnt_axis[joint_idx]
+            limited = self.model.jnt_limited[joint_idx]
+            if limited:
+                lower, upper = self.model.jnt_range[joint_idx]
+            else:
+                lower, upper = -np.infty, np.infty
+            max_vel = joint.velocity
+            max_force = joint.effort
 
-        return joint_id
+        link = joint.child
+        link_name = link.name
+        parent_idx = joint.parent.id - 1  # the base is -1 by convention in Bullet
+
+        # position = joint.position  # self.model.jnt_pos[body.j_idx0 + q]
+        # orientation = joint.quaternion
+        homogeneous = transform_inertial_frame_to_child_link_frame(link)  # Bullet express joint from inertial
+        position = homogeneous[:3, 3]
+        orientation = get_quaternion_from_matrix(homogeneous[:3, :3])
+
+        if position is None:
+            position = np.zeros(3)
+        if orientation is None:
+            orientation = np.zeros(3)
+
+        # xanchor, stiffness, etc
+
+        return joint_id, name, dtype, q_idx, dq_idx, flag, damping, friction, lower, upper, max_force, max_vel, \
+               link_name, axis, position, orientation, parent_idx
 
     def get_joint_state(self, body_id, joint_id):
         """
@@ -1627,19 +1759,24 @@ class Mujoco(Simulator):
         if joint_id < 0 or joint_id > (body.num_joints - 1):
             raise ValueError("joint_id should belong to [0, `num_joints-1`].")
         q = body.get_q_idx(joint_id, keep=True)
-        if q == -1:
+        if q == -1:  # fixed joint
             return 0, 0, np.zeros(6), 0
-        qpos_idx = body.q_idx0 + q
-        qvel_idx = body.v_idx0 + q
-        if not body.fixed:
-            qpos_idx += 7
-            qvel_idx += 6
 
-        pos = self.sim.data.qpos[qpos_idx]
-        vel = self.sim.data.qvel[qvel_idx]
-        reaction_forces = np.zeros(6)
-        torque = self.sim.data.qfrc_applied[qvel_idx]
-        return pos, vel, reaction_forces, torque
+        # compute joint position and velocity
+        pos = self.sim.data.qpos[body.q_idx1 + q]
+        vel = self.sim.data.qvel[body.v_idx1 + q]
+
+        # compute reaction force
+        force_parent = self.sim.data.cfrc_int[body.b_idx0 + joint_id]  # com-based interaction force with parent
+        force_ext = self.sim.data.cfrc_ext[body.b_idx0 + joint_id]  # com-based external force on body
+        force = force_ext - force_parent  # TODO: is it + instead of -?
+        np.roll(force, shift=3, axis=force.ndim - 1)  # [torque, force] --> [force, torque]
+        # TODO: express it in the joint frame
+
+        # compute the applied torque
+        torque = self.sim.data.qfrc_applied[body.v_idx1 + q]
+
+        return pos, vel, force, torque
 
     def get_joint_states(self, body_id, joint_ids):
         """
@@ -1661,7 +1798,7 @@ class Mujoco(Simulator):
         """
         return [self.get_joint_state(body_id, joint_id) for joint_id in joint_ids]
 
-    def reset_joint_state(self, body_id, joint_id, position, velocity=0.):
+    def reset_joint_state(self, body_id, joint_id, position, velocity=None):
         """
         Reset the state of the joint. It is best only to do this at the start, while not running the simulation:
         `reset_joint_state` overrides all physics simulation.
@@ -1678,16 +1815,9 @@ class Mujoco(Simulator):
         if joint_id < 0 or joint_id > (body.num_joints - 1):
             raise ValueError("joint_id should belong to [0, `num_joints-1`].")
         q = body.get_q_idx(joint_id, keep=True)
-        if q == -1:
-            return
-        qpos_idx = body.q_idx0 + q
-        qvel_idx = body.v_idx0 + q
-        if not body.fixed:
-            qpos_idx += 7
-            qvel_idx += 6
-
-        self.sim.data.qpos[qpos_idx] = position
-        self.sim.data.qvel[qvel_idx] = velocity
+        if q != -1:
+            self.sim.data.qpos[body.q_idx1 + q] = position
+            self.sim.data.qvel[body.v_idx1 + q] = velocity
 
     def enable_joint_force_torque_sensor(self, body_id, joint_ids, enable=True):
         """
@@ -1698,9 +1828,12 @@ class Mujoco(Simulator):
             joint_ids (int, int[N]): joint index in range [0..num_joints(body_id)], or list of joint ids.
             enable (bool): True to enable, False to disable the force/torque sensor
         """
-        pass  # TODO
+        # attach a force sensor to the specified body (site)
+        # attach a torque sensor to the specfied body (site)
+        # or check cfrc_int and cfrc_ext
+        pass
 
-    def set_joint_motor_control(self, body_id, joint_ids, control_mode=2, positions=None,
+    def set_joint_motor_control(self, body_id, joint_ids, control_mode=Simulator.POSITION_CONTROL, positions=None,
                                 velocities=None, forces=None, kp=None, kd=None, max_velocity=None):
         r"""
         Set the joint motor control.
@@ -1757,22 +1890,28 @@ class Mujoco(Simulator):
             [7] np.array[float[3]]: Cartesian world angular velocity. Only returned if `compute_velocity` is True.
         """
         body = self._bodies[body_id]
-        if link_id < -1 or link_id > (body.num_bodies - 2):  # -1 is for the base
-            raise ValueError("link_id should belong to [-1, `num_links-2`].")
-        idx = body.b_idx0 + 1 + link_id
+        link_id = self._check_link_id(body, link_id)
+        idx = body.b_idx0 + link_id
 
-        pos = self.sim.data.body_xpos[idx]  # Cartesian position of body frame (same as xipos)
+        pos = self.sim.data.xipos[idx]  # Cartesian position of body CoM
         quat = self._convert_wxyz_to_xyzw(self.sim.data.body_xquat[idx])  # Cartesian orientation of body frame
         # Note that in Mujoco the body frame is defined at the CoM
 
-        # link frame in URDF is the joint frame in Mujoco
-        # TODO: read from the Tree data structure and perform the correct transformations
+        link = body.get_link(link_id)
+        inertial = link.inertial
+        ipos = inertial.position if inertial is not None else np.zeros(3)
+        iquat = inertial.quaternion if inertial is not None else np.array([0., 0., 0., 1.])
+
+        lpos = self.sim.data.body_xpos[idx]  # Cartesian position of body frame
+        lquat = quat
 
         if compute_velocity:
             vel = self.sim.data.cvel[idx]  # com-based velocity [3D rot; 3D tran]
-            return pos, quat, vel[3:], vel[:3]
+            lin_vel, ang_vel = vel[3:], vel[:3]
+        else:
+            lin_vel, ang_vel = np.zeros(3), np.zeros(3)
 
-        return pos, quat
+        return pos, quat, ipos, iquat, lpos, lquat, lin_vel, ang_vel
 
     def get_link_states(self, body_id, link_ids, compute_velocity=False, compute_forward_kinematics=False):
         """
@@ -1801,7 +1940,7 @@ class Mujoco(Simulator):
         return [self.get_link_state(body_id, link_id, compute_velocity, compute_forward_kinematics)
                 for link_id in link_ids]
 
-    def get_link_names(self, body_id, link_ids):
+    def get_link_names(self, body_id, link_ids=None):
         """
         Return the name of the given link(s).
 
@@ -1815,29 +1954,36 @@ class Mujoco(Simulator):
             if multiple links:
                 str[N]: link names
         """
-        pass
+        body = self._bodies[body_id]
+        one_link = isinstance(link_ids, int)
+        if link_ids is None:
+            link_ids = range(0, body.num_bodies-1)  # do not return the name of the base link
+        elif one_link:
+            link_ids = [link_ids]
+        names = []
+        for link_id in link_ids:
+            link_id = self._check_link_id(body, link_id)
+            name = self._process_name(body.get_link(link_id).name)
+            names.append(name)
+        if one_link:
+            return names[0]
+        return names
 
     def _get_link_result(self, body_id, link_ids, mujoco_data, fct=None, slice=None):
         body = self._bodies[body_id]  # TODO: maybe use the tree data structure instead...
-        one_link = isinstance(link_ids, int)
-        if one_link:
-            link_ids = [link_ids]
-        results = []
-        for link_id in link_ids:
-            if link_id < -1 or link_id > (body.num_bodies - 2):  # -1 is for the base
-                raise ValueError("link_id should belong to [-1, `num_links-2`].")
-            idx = body.b_idx0 + 1 + link_id
+        if link_ids is None:
+            data = mujoco_data[body.b_idx0+1:body.b_idxf]  # +1 to not account for the base as in PyBullet
+        else:
+            link_ids = self._check_link_ids(body, link_ids)
+            idx = body.b_idx0 + link_ids
             data = mujoco_data[idx]
-            if slice is not None:
-                data = data[slice]
-            if fct is not None:
-                data = fct(data)
-            results.append(data)
-        if one_link and len(results) > 0:
-            return results[0]
-        return results
+        if slice is not None:
+            data = data[slice]
+        if fct is not None:
+            data = fct(data)
+        return data
 
-    def get_link_masses(self, body_id, link_ids):
+    def get_link_masses(self, body_id, link_ids=None):
         """
         Return the mass of the given link(s).
 
@@ -1851,10 +1997,9 @@ class Mujoco(Simulator):
             else:
                 float[N]: mass of each link
         """
-        # TODO: maybe use the tree data structure instead...
         return self._get_link_result(body_id, link_ids, self.sim.model.body_mass)
 
-    def get_link_frames(self, body_id, link_ids):
+    def get_link_frames(self, body_id, link_ids=None):
         r"""
         Return the link world frame position(s) and orientation(s).
 
@@ -1870,9 +2015,11 @@ class Mujoco(Simulator):
                 np.array[float[N,3]]: link frame position of each link in world space
                 np.array[float[N,4]]: orientation of each link frame [x,y,z,w]
         """
-        pass
+        pos = self._get_link_result(body_id, link_ids, self.sim.data.body_xpos)
+        quat = self._get_link_result(body_id, link_ids, self.sim.data.body_xquat, fct=self._convert_wxyz_to_xyzw)
+        return pos, quat
 
-    def get_link_world_positions(self, body_id, link_ids):
+    def get_link_world_positions(self, body_id, link_ids=None):
         """
         Return the CoM position (in the Cartesian world space coordinates) of the given link(s).
 
@@ -1886,8 +2033,7 @@ class Mujoco(Simulator):
             if multiple links:
                 np.array[float[N,3]]: CoM position of each link in world space
         """
-        # Cartesian position of body frame (same as xipos)
-        return np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.body_xpos))
+        return self._get_link_result(body_id, link_ids, self.sim.data.xipos)
 
     def get_link_positions(self, body_id, link_ids):
         pass
@@ -1906,9 +2052,7 @@ class Mujoco(Simulator):
             if multiple links:
                 np.array[float[N,4]]: CoM orientation of each link (x,y,z,w)
         """
-        # Cartesian orientation of body frame
-        return np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.body_xquat,
-                                                fct=self._convert_wxyz_to_xyzw))
+        return self._get_link_result(body_id, link_ids, self.sim.data.body_xquat, fct=self._convert_wxyz_to_xyzw)
 
     def get_link_orientations(self, body_id, link_ids):
         pass
@@ -1927,7 +2071,7 @@ class Mujoco(Simulator):
             if multiple links:
                 np.array[float[N,3]]: linear velocity of each link
         """
-        return np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.cvel, slice=slice(3, 6)))
+        return self._get_link_result(body_id, link_ids, self.sim.data.cvel, slice=slice(3, 6))
 
     def get_link_world_angular_velocities(self, body_id, link_ids):
         """
@@ -1961,9 +2105,7 @@ class Mujoco(Simulator):
                 np.array[float[N,6]]: linear and angular velocity of each link
         """
         velocities = np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.cvel))
-        if velocities.ndim == 1:
-            return np.roll(velocities, shift=3)
-        return np.roll(velocities, shift=3, axis=1)
+        return np.roll(velocities, shift=3, axis=velocities.ndim-1)
 
     def get_link_velocities(self, body_id, link_ids):
         pass
@@ -2018,9 +2160,7 @@ class Mujoco(Simulator):
                 np.array[float[N,6]]: linear and angular acceleration of each link
         """
         accelerations = np.asarray(self._get_link_result(body_id, link_ids, self.sim.data.cacc))
-        if accelerations.ndim == 1:
-            return np.roll(accelerations, shift=3)
-        return np.roll(accelerations, shift=3, axis=1)
+        return np.roll(accelerations, shift=3, axis=accelerations.ndim - 1)
 
     def get_q_indices(self, body_id, joint_ids):
         """
@@ -2094,7 +2234,17 @@ class Mujoco(Simulator):
                 int: joint type id.
             if multiple joints: list of above
         """
-        pass
+        body = self._bodies[body_id]
+        one_joint = isinstance(joint_ids, int)
+        if one_joint:
+            joint_ids = [joint_ids]
+        types = []
+        for joint_id in joint_ids:
+            joint = body.get_joint(joint_id)
+            types.append(self._get_joint_type_id(joint.dtype))
+        if one_joint and len(types) > 1:
+            return types[0]
+        return types
 
     def get_joint_type_names(self, body_id, joint_ids):
         """
@@ -2224,15 +2374,32 @@ class Mujoco(Simulator):
             kds (None, float, np.array[float[N]]): velocity gain(s)
             forces (None, float, np.array[float[N]]): maximum motor force(s)/torque(s) used to reach the target values.
         """
-        pass
+        # TODO: use the other arguments
 
-    def get_joint_positions(self, body_id, joint_ids):
+        body = self._bodies[body_id]
+
+        if joint_ids is None:
+            self.sim.data.qpos[body.q_idx1:body.q_idxf] = positions
+
+        # check if valid joints
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, set its torque
+        if isinstance(joint_ids, int):
+            self.sim.data.qpos[body.q_idx1 + joint_ids] = positions
+
+        # if multiple joints, set their torques
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        self.sim.data.qpos[body.q_idx1 + q[q != -1]] = positions
+
+    def get_joint_positions(self, body_id, joint_ids=None):
         """
         Get the position of the given joint(s).
 
         Args:
             body_id (int): unique body id.
-            joint_ids (int, list[int]): joint id, or list of joint ids.
+            joint_ids (int, list[int], None): joint id, or list of joint ids. If None, it will take all the actuated
+              joints.
 
         Returns:
             if 1 joint:
@@ -2241,7 +2408,22 @@ class Mujoco(Simulator):
                 np.array[float[N]]: joint positions [rad]
         """
         body = self._bodies[body_id]
-        pass
+
+        if joint_ids is None:
+            return self.sim.data.qpos[body.q_idx1:body.q_idxf]
+
+        # check if valid joint
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, return its position
+        if isinstance(joint_ids, int):
+            return self.sim.data.qpos[body.q_idx1 + joint_ids]
+
+        # if multiple joints, return their positions
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        qpos = np.zeros(len(joint_ids))
+        qpos[q != -1] = self.sim.data.qpos[body.q_idx1 + q[q != -1]]
+        return qpos
 
     def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
         """
@@ -2253,9 +2435,25 @@ class Mujoco(Simulator):
             velocities (float, np.array[float[N]]): desired velocity, or list of desired velocities [rad/s]
             max_force (None, float, np.array[float[N]]): maximum motor forces/torques
         """
-        pass
+        # TODO: use the other arguments
 
-    def get_joint_velocities(self, body_id, joint_ids):
+        body = self._bodies[body_id]
+
+        if joint_ids is None:
+            self.sim.data.qvel[body.v_idx1:body.v_idxf] = velocities
+
+        # check if valid joints
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, set its torque
+        if isinstance(joint_ids, int):
+            self.sim.data.qvel[body.v_idx1 + joint_ids] = velocities
+
+        # if multiple joints, set their torques
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        self.sim.data.qvel[body.v_idx1 + q[q != -1]] = velocities
+
+    def get_joint_velocities(self, body_id, joint_ids=None):
         """
         Get the velocity of the given joint(s).
 
@@ -2269,7 +2467,23 @@ class Mujoco(Simulator):
             if multiple joints:
                 np.array[float[N]]: joint velocities [rad/s]
         """
-        pass
+        body = self._bodies[body_id]
+
+        if joint_ids is None:
+            return self.sim.data.qvel[body.v_idx1:body.v_idxf]
+
+        # check if valid joint
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, return its velocity
+        if isinstance(joint_ids, int):
+            return self.sim.data.qvel[body.v_idx1 + joint_ids]
+
+        # if multiple joints, return their velocities
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        qvel = np.zeros(len(joint_ids))
+        qvel[q != -1] = self.sim.data.qvel[body.v_idx1 + q[q != -1]]
+        return qvel
 
     def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None):
         """
@@ -2280,11 +2494,11 @@ class Mujoco(Simulator):
             body_id (int): unique body id.
             joint_ids (int, list[int]): joint id, or list of joint ids.
             accelerations (float, np.array[float[N]]): desired joint acceleration, or list of desired joint
-                accelerations [rad/s^2]
+              accelerations [rad/s^2]
         """
         pass
 
-    def get_joint_accelerations(self, body_id, joint_ids):  # , q=None, dq=None):
+    def get_joint_accelerations(self, body_id, joint_ids=None):  # , q=None, dq=None):
         """
         Get the acceleration of the specified joint(s). This is only valid if the simulator `supports_acceleration`.
 
@@ -2298,7 +2512,23 @@ class Mujoco(Simulator):
             if multiple joints:
                 np.array[float[N]]: joint accelerations [rad/s^2]
         """
-        pass
+        body = self._bodies[body_id]
+
+        if joint_ids is None:
+            return self.sim.data.qacc[body.v_idx1:body.v_idxf]
+
+        # check if valid joint
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, return its velocity
+        if isinstance(joint_ids, int):
+            return self.sim.data.qacc[body.v_idx1 + joint_ids]
+
+        # if multiple joints, return their velocities
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        qacc = np.zeros(len(joint_ids))
+        qacc[q != -1] = self.sim.data.qacc[body.v_idx1 + q[q != -1]]
+        return qacc
 
     def set_joint_torques(self, body_id, joint_ids, torques):
         """
@@ -2309,9 +2539,23 @@ class Mujoco(Simulator):
             joint_ids (int, list[int]): joint id, or list of joint ids.
             torques (float, list[float], np.array[float]): desired torque(s) to apply to the joint(s) [N].
         """
-        pass
+        body = self._bodies[body_id]
 
-    def get_joint_torques(self, body_id, joint_ids):
+        if joint_ids is None:
+            self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf] = torques
+
+        # check if valid joints
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, set its torque
+        if isinstance(joint_ids, int):
+            self.sim.data.qfrc_applied[body.v_idx1 + joint_ids] = torques
+
+        # if multiple joints, set their torques
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques
+
+    def get_joint_torques(self, body_id, joint_ids=None):
         """
         Get the applied torque(s) on the given joint(s).
 
@@ -2325,9 +2569,25 @@ class Mujoco(Simulator):
             if multiple joints:
                 np.array[float[N]]: torques associated to the given joints [Nm]
         """
-        pass
+        body = self._bodies[body_id]
 
-    def get_joint_reaction_forces(self, body_id, joint_ids):
+        if joint_ids is None:
+            return self.sim.data.qfrc_applied[body.v_idx1:body.v_idxf]
+
+        # check if valid joint
+        self._check_joint_ids(body, joint_ids)
+
+        # if one joint, return its velocity
+        if isinstance(joint_ids, int):
+            return self.sim.data.qfrc_applied[body.v_idx1 + joint_ids]
+
+        # if multiple joints, return their velocities
+        q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+        torques = np.zeros(len(joint_ids))
+        torques[q != -1] = self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]]
+        return torques
+
+    def get_joint_reaction_forces(self, body_id, joint_ids=None):
         """Return the joint reaction forces at the given joint. Note that the torque sensor must be enabled, otherwise
         it will always return [0,0,0,0,0,0].
 
@@ -2341,7 +2601,32 @@ class Mujoco(Simulator):
             if multiple joints:
                 np.array[float[N,6]]: joint reaction forces [N, Nm]
         """
-        pass
+        body = self._bodies[body_id]
+
+        if joint_ids is None:
+            # com-based interaction force with parent [torque, force]
+            force_parent = self.sim.data.cfrc_int[body.b_idx0:body.b_idxf]
+            # com-based external force on body [torque, force]
+            force_ext = self.sim.data.cfrc_ext[body.b_idx0:body.b_idxf]
+            force = force_ext - force_parent  # TODO: is it + instead of -?
+            np.roll(force, shift=3, axis=force.ndim - 1)  # [torque, force] --> [force, torque]
+            # TODO: express that force in the joint frame
+            return force
+
+        # check if valid link id
+        self._check_link_ids(body, joint_ids)
+
+        # com-based interaction force with parent [torque, force]
+        force_parent = self.sim.data.cfrc_int[body.b_idx0 + joint_ids]
+        # com-based external force on body [torque, force]
+        force_ext = self.sim.data.cfrc_ext[body.b_idx0 + joint_ids]
+
+        force = force_ext - force_parent  # TODO: is it + instead of -?
+
+        np.roll(force, shift=3, axis=force.ndim-1)  # [torque, force] --> [force, torque]
+
+        # TODO: express that force in the joint frame
+        return force
 
     def get_joint_powers(self, body_id, joint_ids):
         """Return the applied power at the given joint(s). Power = torque * velocity.
@@ -2356,7 +2641,9 @@ class Mujoco(Simulator):
             if multiple joints:
                 np.array[float[N]]: power at each joint [W]
         """
-        pass
+        torque = self.get_joint_torques(body_id, joint_ids)
+        velocity = self.get_joint_velocities(body_id, joint_ids)
+        return torque * velocity
 
     #################
     # Visualization #
@@ -2702,15 +2989,21 @@ class Mujoco(Simulator):
             to_position (np.array[float[3]]): end of the ray in world coordinates
 
         Returns:
-            list:
-                int: object unique id of the hit object
-                int: link index of the hit object, or -1 if none/parent
-                float: hit fraction along the ray in range [0,1] along the ray.
-                np.array[float[3]]: hit position in Cartesian world coordinates
-                np.array[float[3]]: hit normal in Cartesian world coordinates
+            [0] int: object unique id of the hit object
+            [1] int: link index of the hit object, or -1 if none/parent
+            [2] float: hit fraction along the ray in range [0,1] along the ray.
+            [3] np.array[float[3]]: hit position in Cartesian world coordinates
+            [4] np.array[float[3]]: hit normal in Cartesian world coordinates
         """
         vec = to_position - from_position
-        return self.sim.ray(pnt=from_position, vec=vec)  # this return the distance and id of the geom
+        distance, geom_id = self.sim.ray(pnt=from_position, vec=vec)  # this return the distance and id of the geom
+        norm = np.linalg.norm(vec)
+        fraction = distance / norm
+        unit_vec = vec / norm
+        position = distance * unit_vec
+        normal = -unit_vec  # TODO: this is not correct...
+        # TODO: get body_id and link_id from geom_id
+        return geom_id, geom_id, fraction, position, normal
 
     ###########################
     # Kinematics and Dynamics #
@@ -2772,7 +3065,8 @@ class Mujoco(Simulator):
     def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
                         rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
-                        local_inertia_diagonal=None, joint_damping=None):
+                        local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
+                        joint_damping=None, joint_friction=None):
         """
         Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
@@ -2783,7 +3077,7 @@ class Mujoco(Simulator):
             lateral_friction (float): lateral (linear) contact friction
             spinning_friction (float): torsional friction around the contact normal
             rolling_friction (float): torsional friction orthogonal to contact normal
-            restitution (float): bouncyness of contact. Keep it a bit less than 1.
+            restitution (float): bounciness of contact. Keep it a bit less than 1.
             linear_damping (float): linear damping of the link (0.04 by default)
             angular_damping (float): angular damping of the link (0.04 by default)
             contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
@@ -2795,13 +3089,17 @@ class Mujoco(Simulator):
             local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
                 and links are centered around the center of mass and aligned with the principal axes of inertia so
                 there are no off-diagonal elements in the inertia tensor.
+            inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
+            inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
+              with respect to the link frame.
             joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
                 joint damping field. Keep the value close to 0.
                 `joint_damping_force = -damping_coefficient * joint_velocity`.
+            joint_friction (float): joint friction coefficient.
+
         """
         body = self._bodies[body_id]
-        if link_id < -1 or link_id > (body.num_bodies - 2):  # -1 is for the base
-            raise ValueError("link_id should belong to [-1, `num_links-2`].")
+        self._check_link_id(body, link_id)
 
         idx = body.b_idx0 + 1 + link_id
         if mass is not None:
@@ -3092,6 +3390,53 @@ class Mujoco(Simulator):
         qacc = dest.qacc[body.v_idx0:body.v_idxf]
         return qacc
 
+    #########
+    # Debug #
+    #########
+
+    # TODO
+
+    ############################
+    # Events (mouse, keyboard) #
+    ############################
+
+    def get_keyboard_events(self):
+        """Get the key events.
+
+        Returns:
+            dict: {keyId: keyState}
+                * `keyID` is an integer (ascii code) representing the key. Some special keys like shift, arrows,
+                and others are are defined in pybullet such as `B3G_SHIFT`, `B3G_LEFT_ARROW`, `B3G_UP_ARROW`,...
+                * `keyState` is an integer. 3 if the button has been pressed, 1 if the key is down, 2 if the key has
+                been triggered.
+        """
+        pass
+
+    def get_mouse_events(self):
+        """Get the mouse events.
+
+        Returns:
+            list of mouse events:
+                eventType (int): 1 if the mouse is moving, 2 if a button has been pressed or released
+                mousePosX (float): x-coordinates of the mouse pointer
+                mousePosY (float): y-coordinates of the mouse pointer
+                buttonIdx (int): button index for left/middle/right mouse button. It is -1 if nothing,
+                                 0 if left button, 1 if scroll wheel (pressed), 2 if right button
+                buttonState (int): 0 if nothing, 3 if the button has been pressed, 4 is the button has been released,
+                                   1 if the key is down (never observed), 2 if the key has been triggered (never
+                                   observed).
+        """
+        pass
+
+    def get_mouse_and_keyboard_events(self):
+        """Get the mouse and key events.
+
+        Returns:
+            list: list of mouse events
+            dict: dictionary of key events
+        """
+        pass
+
 
 # Test
 if __name__ == '__main__':
diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py
index ed06381..7110fa3 100644
--- a/pyrobolearn/simulators/simulator.py
+++ b/pyrobolearn/simulators/simulator.py
@@ -135,6 +135,7 @@ class Simulator(object):
     JOINT_PRISMATIC = 1
     JOINT_REVOLUTE = 0
     JOINT_SPHERICAL = 2
+    JOINT_FREE = 7      # NEW
 
     KEY_IS_DOWN = 1
     KEY_WAS_RELEASED = 4
@@ -2361,7 +2362,8 @@ class Simulator(object):
     def change_dynamics(self, body_id, link_id=-1, mass=None, lateral_friction=None, spinning_friction=None,
                         rolling_friction=None, restitution=None, linear_damping=None, angular_damping=None,
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
-                        local_inertia_diagonal=None, joint_damping=None):
+                        local_inertia_diagonal=None, inertia_position=None, inertia_orientation=None,
+                        joint_damping=None, joint_friction=None):
         """
         Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
@@ -2372,21 +2374,25 @@ class Simulator(object):
             lateral_friction (float): lateral (linear) contact friction
             spinning_friction (float): torsional friction around the contact normal
             rolling_friction (float): torsional friction orthogonal to contact normal
-            restitution (float): bouncyness of contact. Keep it a bit less than 1.
+            restitution (float): bounciness of contact. Keep it a bit less than 1.
             linear_damping (float): linear damping of the link (0.04 by default)
             angular_damping (float): angular damping of the link (0.04 by default)
             contact_stiffness (float): stiffness of the contact constraints, used together with `contact_damping`
             contact_damping (float): damping of the contact constraints for this body/link. Used together with
-                `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
-                section.
+              `contact_stiffness`. This overrides the value if it was specified in the URDF file in the contact
+              section.
             friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
-                default, unless set in the URDF contact section)
+              default, unless set in the URDF contact section)
             local_inertia_diagonal (np.array[float[3]]): diagonal elements of the inertia tensor. Note that the base
-                and links are centered around the center of mass and aligned with the principal axes of inertia so
-                there are no off-diagonal elements in the inertia tensor.
+              and links are centered around the center of mass and aligned with the principal axes of inertia so
+              there are no off-diagonal elements in the inertia tensor.
+            inertia_position (np.array[float[3]]): new inertia position with respect to the link frame.
+            inertia_orientation (np.array[float[4]]): new inertia orientation (expressed as a quaternion [x,y,z,w]
+              with respect to the link frame.
             joint_damping (float): joint damping coefficient applied at each joint. This coefficient is read from URDF
-                joint damping field. Keep the value close to 0.
-                `joint_damping_force = -damping_coefficient * joint_velocity`.
+              joint damping field. Keep the value close to 0.
+              `joint_damping_force = -damping_coefficient * joint_velocity`.
+            joint_friction (float): joint friction coefficient.
         """
         pass
 
diff --git a/pyrobolearn/utils/parsers/robots/data_structures.py b/pyrobolearn/utils/parsers/robots/data_structures.py
index c86b92f..9d85d07 100644
--- a/pyrobolearn/utils/parsers/robots/data_structures.py
+++ b/pyrobolearn/utils/parsers/robots/data_structures.py
@@ -8,7 +8,7 @@ import trimesh
 from collections import OrderedDict, Iterable
 
 from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy, get_matrix_from_rpy, \
-    get_rpy_from_matrix, get_matrix_from_axis_angle
+    get_rpy_from_matrix, get_matrix_from_axis_angle, get_inverse_homogeneous
 from pyrobolearn.utils.inertia import get_inertia_of_box, get_inertia_of_capsule, get_inertia_of_cylinder, \
     get_inertia_of_ellipsoid, get_inertia_of_mesh, get_inertia_of_sphere, combine_inertias
 
@@ -142,7 +142,41 @@ class PhysicsEngine(object):
 
 
 class Frame(object):
-    r"""Reference Frame"""
+    r"""Reference Frame.
+
+    This is used to expressed the position and orientation of frames that are used for bodies/links (inertials,
+    visuals, collisions) and joints.
+
+    Note that we follow the convention described in URDFs [1, 2] to describe the various frames. Notably,
+    - the link frame is the same as the joint frame and is at the base of a body/link.
+    - the inertial frame is expressed with respect to the link/joint frame.
+    - the visual frame is expressed with respect to the link/joint frame.
+    - the collision frame is expressed with respect to the link/joint frame.
+    - the child link/joint frame is expressed with respect to the parent link/joint frame.
+
+    This sometimes can conflict with other conventions such as the one followed in MuJoCo [3], where:
+    - In MuJoCo, the positions and orientations of all elements can be expressed in global or local coordinates in
+      the XML file. However, once compiled everything will be expressed in local coordinates. The local coordinates
+      are different from the ones defined in URDFs.
+    - the body/link frame is defined at the CoM of the body, thus at the inertial frame.
+    - the inertial, visual, and collision (geoms/sites) frames are expressed with respect to the body/link frame they
+      belong to.
+    - the child body frame is expressed with respect to the parent body frame.
+    - the joint frame that connects the parent and child body is expressed with respect to the child body frame.
+
+    Another one where there can be a conflict is with Bullet [4], where all the elements are expressed in local
+    coordinates and the convention is pretty similar to URDF, except:
+    - the link and joint frames are decoupled; the joint frame is the same as the link/joint frame in URDF but the
+      link frame is the same as Mujoco (i.e. it is at the inertial frame of the body).
+    - the next joint frame is expressed with respect to the inertial frame.
+
+    References:
+        - [1] http://wiki.ros.org/urdf/XML/link
+        - [2] http://wiki.ros.org/urdf/XML/joint
+        - [3] http://mujoco.org/book/modeling.html#CFrame
+        - [4] Pybullet Quickstart Guide:
+          https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.e27vav9dy7v6
+    """
 
     def __init__(self, position=None, orientation=None, dtype=None, right_handed=True):
         """
@@ -788,6 +822,34 @@ class MultiBody(object):
 
     The multi-body / tree data structure starts with a root element (=base link) and contains each bodies / joints.
     Each tree represents a multi-body in the world. Its position / orientation is expressed in the world frame.
+
+    Note that we follow the convention described in URDFs [1, 2] to describe the various frames. Notably,
+    - the link frame is the same as the joint frame and is at the base of a body/link.
+    - the inertial, visual, and collision frames are expressed with respect to the link/joint frame.
+    - the child link/joint frame is expressed with respect to the parent link/joint frame.
+
+    This sometimes can conflict with other conventions such as the one followed in MuJoCo [3], where:
+    - In MuJoCo, the positions and orientations of all elements can be expressed in global or local coordinates in
+      the XML file. However, once compiled everything will be expressed in local coordinates. The local coordinates
+      are different from the ones defined in URDFs.
+    - the body/link frame is defined at the CoM of the body, thus at the inertial frame.
+    - the inertial, visual, and collision (geoms/sites) frames are expressed with respect to the body/link frame they
+      belong to.
+    - the child body frame is expressed with respect to the parent body frame.
+    - the joint frame that connects the parent and child body is expressed with respect to the child body frame.
+
+    Another one where there can be a conflict is with Bullet [4], where all the elements are expressed in local
+    coordinates and the convention is pretty similar to URDF, except:
+    - the link and joint frames are decoupled; the joint frame is the same as the link/joint frame in URDF but the
+      link frame is the same as Mujoco (i.e. it is at the inertial frame of the body).
+    - the next joint frame is expressed with respect to the inertial frame.
+
+    References:
+        - [1] http://wiki.ros.org/urdf/XML/link
+        - [2] http://wiki.ros.org/urdf/XML/joint
+        - [3] http://mujoco.org/book/modeling.html#CFrame
+        - [4] Pybullet Quickstart Guide:
+          https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#heading=h.e27vav9dy7v6
     """
 
     def __init__(self, name=None, root=None, position=None, orientation=None):
@@ -801,9 +863,9 @@ class MultiBody(object):
             orientation (list/tuple/np.array[float[3/4/9]], np.array[float[3,3]], str): frame orientation in the world.
         """
         self.name = name
-        self.root = root
         self.bodies = OrderedDict()  # {name: Body}
         self.joints = OrderedDict()  # {name: Joint}
+        self.root = root
         self.materials = {}
         self.frame = Frame(position, orientation, dtype='world')
 
@@ -858,9 +920,12 @@ class MultiBody(object):
     @root.setter
     def root(self, root):
         """Set the root body element."""
-        if root is not None and not isinstance(root, Body):
-            raise TypeError("Expecting the given 'body' to be an instance of `Body`, but instead got: "
-                            "{}".format(type(root)))
+        if root is not None:
+            if not isinstance(root, Body):
+                raise TypeError("Expecting the given 'body' to be an instance of `Body`, but instead got: "
+                                "{}".format(type(root)))
+            if len(self.bodies) == 0:  # only if it is empty
+                self.bodies[root.name] = root
         self._root = root
 
     @property
@@ -992,6 +1057,43 @@ class MultiBody(object):
 Tree = MultiBody
 
 
+def transform_inertial_frame_to_joint_frame(body):
+    """Return the homogeneous transform from the inertial frame to the joint frame."""
+    # the inertial frame is expressed wrt to the joint frame by default
+    inertial = body.inertial
+    if inertial is not None:
+        return get_inverse_homogeneous(inertial.homogeneous)
+
+
+def transform_child_joint_frame_to_parent_inertial_frame(child_body):
+    """Return the homogeneous transform from the child joint frame to the parent inertial frame."""
+    parent_joint = child_body.parent_joint
+    parent = child_body.parent_body
+    if parent_joint is not None and parent.inertial is not None:
+        h_p_c = parent_joint.homogeneous  # from parent to child link/joint frame
+        h_c_p = get_inverse_homogeneous(h_p_c)  # from child to parent link/joint frame
+        h_p_pi = parent.inertial.homogeneous  # from parent link/joint frame to inertial frame
+        h_c_pi = h_c_p.dot(h_p_pi)  # from child link/joint frame to parent inertial frame
+        return h_c_pi
+
+
+def transform_inertial_frame_to_child_link_frame(child_body):
+    """Return the homogeneous transform from the parent inertial frame to the child link/joint frame."""
+    # from child link/joint frame to parent inertial frame
+    h_c_pi = transform_child_joint_frame_to_parent_inertial_frame(child_body)
+    if h_c_pi is not None:
+        return get_inverse_homogeneous(h_c_pi)
+
+
+def transform_inertial_frame_to_child_inertial_frame(child_body):
+    """Return the homogeneous transform from the parent inertial frame to the child inertial frame."""
+    if child_body.inertial is not None:
+        h_c_ci = child_body.inertial.homogeneous
+        h_pi_c = transform_inertial_frame_to_child_link_frame(child_body)
+        if h_pi_c is not None:
+            return h_pi_c.dot(h_c_ci)  # from parent parent inertial frame to child inertial frame
+
+
 class Body(object):
     r"""Body / Link data structure."""
 
@@ -1022,7 +1124,7 @@ class Body(object):
         self.name = name
 
         self.joints = OrderedDict()             # child joints
-        self.parent_joints = OrderedDict()      # parent joints
+        self.parent_joints = OrderedDict()      # parent joints: Warning each body should only have one parent joint!!
 
         # set body properties
         self.inertials = inertials
@@ -1264,6 +1366,25 @@ class Body(object):
         """Set the given homogeneous matrix."""
         self.frame.homogeneous = matrix
 
+    @property
+    def parent_body(self):
+        """Return the parent body."""
+        if self.parent_joints:
+            joint = self.parent_joints[next(iter(self.parent_joints))]
+            return joint.parent  # this can be None (like for the root)
+
+    @property
+    def child_bodies(self):
+        """Return the child bodies."""
+        if self.joints:
+            return [joint.child for joint in self.joints if joint.child is not None]
+
+    @property
+    def parent_joint(self):
+        """Return the parent joint."""
+        if self.parent_joints:
+            return self.parent_joints[next(iter(self.parent_joints))]
+
     def add_collision(self, collision):
         """
         Add a collision shape to the list of collision shapes.
@@ -1349,14 +1470,14 @@ class Joint(object):
     Joint types: fixed, floating/free, prismatic, revolute/hinge, continuous, gearbox, revolute2, ball, screw,
     universal, and planar.
 
-    - fixed: no motions is allowed; both links are rigidly attached to each other.
+    - fixed/weld: no motions is allowed; both links are rigidly attached to each other.
     - floating/free: allows motion for all 6 degrees of motion.
     - prismatic: allows motion along 1 translational DoF.
     - revolute/hinge: allows rotational motion around one axis (1 DoF).
     - continuous: a revolute/hinge joint that doesn't have lower or upper limits.
-    - gearbox: geared revolute joint.
+    - gearbox/gear: geared revolute joint.
     - revolute2: two revolute joints connected in series
-    - ball: a ball and socket joint which allows rotational motions around the 3 axis (3 DoFs).
+    - ball (=spherical): a ball and socket joint which allows rotational motions around the 3 axis (3 DoFs).
     - screw: a single DoF joint wich coupled sliding and rotational motion
     - universal: like a ball joint, but constrains one DoF
     - planar: allows motion in a plane perpendicular to the axis.
@@ -1365,6 +1486,7 @@ class Joint(object):
     - SDF: ball, fixed, gearbox, prismatic, revolute, revolute2, screw, universal
     - Dart: ball, free (=floating), euler, prismatic, weld (=fixed), revolute, universal
     - MuJoCo: ball, free (=floating), hinge (=revolute), slide (=prismatic)
+    - Bullet: fixed, gear, planar, point2point, prismatic, revolute, spherical (=ball)
 
     By default, we follow the convention expressed in URDF to describe the frames. That is, the child joint frame is
     described with respect to the parent joint/link frame.
@@ -1450,7 +1572,8 @@ class Joint(object):
             elif dtype in {'free', 'floating'}:
                 dtype = 'floating'
                 self.num_dofs = 6
-            elif dtype == 'ball':
+            elif dtype in {'ball', 'spherical'}:
+                dtype = 'ball'
                 self.num_dofs = 3
             elif dtype == 'continuous':
                 self.num_dofs = 1
diff --git a/pyrobolearn/utils/parsers/robots/mujoco_parser.py b/pyrobolearn/utils/parsers/robots/mujoco_parser.py
index d5b927f..1aa555a 100644
--- a/pyrobolearn/utils/parsers/robots/mujoco_parser.py
+++ b/pyrobolearn/utils/parsers/robots/mujoco_parser.py
@@ -1209,6 +1209,9 @@ class MuJoCoParser(WorldParser):
         # the element for geoms, joints, sites, cameras and lights", and a joint defined in a body connects that body
         # with its parent body.
 
+        # copy Tree just in case to not rewrite anything
+        tree = copy.deepcopy(tree)
+
         h_bodies, h_joints, h_visuals, h_collisions, h_inertials = {}, {}, {}, {}, {}
         for i, body in enumerate(tree.bodies.values()):
             # print(body.name, body.homogeneous)
diff --git a/pyrobolearn/utils/parsers/robots/urdf_parser.py b/pyrobolearn/utils/parsers/robots/urdf_parser.py
index 1ba4e81..c61e246 100644
--- a/pyrobolearn/utils/parsers/robots/urdf_parser.py
+++ b/pyrobolearn/utils/parsers/robots/urdf_parser.py
@@ -283,7 +283,7 @@ class URDFParser(RobotParser):
             joint.friction = dynamics_tag.attrib.get('friction')
 
         # limits
-        limits_tag = joint_tag.find('limits')
+        limits_tag = joint_tag.find('limit')
         if limits_tag is not None:
             joint.effort = limits_tag.attrib.get('effort')
             joint.velocity = limits_tag.attrib.get('velocity')
diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py
index e054576..59255ac 100644
--- a/pyrobolearn/worlds/world.py
+++ b/pyrobolearn/worlds/world.py
@@ -1195,7 +1195,8 @@ class World(object):
             int: unique id of the floor in the world
         """
         # self.floor_id = self.sim.load_urdf('plane100.urdf', use_fixed_base=True, scale=scaling)
-        self.floor_id = self.sim.load_urdf('plane.urdf', position=[0., 0., 0.], use_fixed_base=True, scale=scaling)
+        # self.floor_id = self.sim.load_urdf('plane.urdf', position=[0., 0., 0.], use_fixed_base=True, scale=scaling)
+        self.floor_id = self.sim.load_floor(dimension=scaling * 20)
         # distance = self.camera.distance
         # self.camera.reset(distance=scaling * distance)
         return self.floor_id