diff --git a/pyrobolearn/simulators/mujoco.py b/pyrobolearn/simulators/mujoco.py
index 8466b41..48fc2f9 100644
--- a/pyrobolearn/simulators/mujoco.py
+++ b/pyrobolearn/simulators/mujoco.py
@@ -191,7 +191,7 @@ class Body(object):
         # keep in memory the link ids
 
         # define variables related to actuators and control
-        self.ctrl_mode = struct.ControlMode.NULL  # remember the last control mode
+        self.ctrl_modes = np.array([struct.ControlMode.NULL] * self.num_actuated_joints)  # one for each joint
         self.gains = None   # original gains
         self.biases = None  # original biases
         self.ctrl_limited = None  # original binary vector (nu,) to specify if the control inputs are limited
@@ -416,7 +416,14 @@ class Body(object):
         return self.tag.attrib.get("name")
 
     def get_q_idx(self, joint_id, keep=False):
-        """Return the q index(ices) associated with the given joint id(s).
+        """
+        Return the q index(ices) associated with the given joint id(s). The indices are between -1 and the number of
+        actuated joints. If the provided `keep` argument is True, then it will return also the q indices for the
+        fixed joints. By default, for the fixed joints, the corresponding q index is set to -1.
+
+        For instance, if we have a body with 5 joints such that in the order we have 1 revolute, 2 fixed, and 2
+        revolute joints, the method `get_q_idx` (with `keep=True` and specifying all the joint ids even the fixed
+        ones) will return [0, -1, -1, 1, 2]. If `keep=False`, then it will return [0, 1, 2].
 
         Args:
             joint_id (np.array[int], int): joint id(s) (each joint id should be between [0, num_joints[).
@@ -1376,12 +1383,22 @@ class Mujoco(Simulator):
             print(self._parser.get_string(pretty_format=True))
 
         # save default gain and bias parameters, control inputs range, and force range
-        # body.gains = self.model.actuator_gainprm[body.u_idx0:body.u_idxf]
-        # body.biases = self.model.actuator_biasprm[body.u_idx0:body.u_idxf]
-        # body.ctrl_limited = self.model.actuator_ctrllimited[body.u_idx0:body.u_idxf]
-        # body.force_limited = self.model.actuator_forcelimited[body.u_idx0:body.u_idxf]
-        # body.ctrl_range = self.model.actuator_ctrlrange[body.u_idx0:body.u_idxf]
-        # body.force_range = self.model.actuator_forcerange[body.u_idx0:body.u_idxf]
+        body.gains = self.model.actuator_gainprm[body.u_idx0:body.u_idxf]
+        body.biases = self.model.actuator_biasprm[body.u_idx0:body.u_idxf]
+        body.ctrl_limited = self.model.actuator_ctrllimited[body.u_idx0:body.u_idxf]
+        body.force_limited = self.model.actuator_forcelimited[body.u_idx0:body.u_idxf]
+        body.ctrl_range = self.model.actuator_ctrlrange[body.u_idx0:body.u_idxf]
+        body.force_range = self.model.actuator_forcerange[body.u_idx0:body.u_idxf]
+
+        # print("Creating body with the following gains: ")
+        # print("Position gain = {}, bias = {}".format(self.model.actuator_gainprm[body.u_p_indices],
+        #                                              self.model.actuator_biasprm[body.u_p_indices]))
+        # print("Velocity gain = {}, bias = {}".format(self.model.actuator_gainprm[body.u_v_indices],
+        #                                              self.model.actuator_biasprm[body.u_v_indices]))
+        # print("Effort gain = {}, bias = {}".format(self.model.actuator_gainprm[body.u_e_indices],
+        #                                            self.model.actuator_biasprm[body.u_e_indices]))
+        # print("Force limited?: ", self.model.actuator_forcelimited[body.u_p_indices])
+        # print("Force range = ", self.model.actuator_forcerange[body.u_p_indices])
 
         # return body id
         return body_id
@@ -2841,66 +2858,74 @@ class Mujoco(Simulator):
         #         # self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = tau + c_q_dq  # DEPRECATED
         #         self.sim.data.qfrc_actuator[body.v_idx1 + q[q != -1]] = tau + c_q_dq
 
-        # TODO: use the given kps and kds
+        # get body and q indices
         body = self._bodies[body_id]
-
-        # check the current control mode, deactivate the other motors by setting their gains and biases to zero
-        if velocities is None and body.ctrl_mode != struct.ControlMode.POSITION:
-            # Switch to position control mode
-            body_p_indices = body.u_p_indices - body.u_idx0
-            self.model.actuator_gainprm[body.u_p_indices, 0] = body.gains[body_p_indices, 0]
-            self.model.actuator_biasprm[body.u_p_indices, 1] = -body.biases[body_p_indices, 1]
-            self.model.actuator_gainprm[body.u_v_indices] = 0
-            self.model.actuator_biasprm[body.u_v_indices] = 0
-            self.model.actuator_gainprm[body.u_e_indices] = 0
-            self.model.actuator_biasprm[body.u_e_indices] = 0
-            body.ctrl_mode = struct.ControlMode.POSITION
-        elif velocities is not None and body.ctrl_mode != struct.ControlMode.PD:
-            # Switch to PD control mode
-            body_p_indices = body.u_p_indices - body.u_idx0
-            body_v_indices = body_p_indices + 1
-            self.model.actuator_gainprm[body.u_p_indices, 0] = body.gains[body_p_indices, 0]
-            self.model.actuator_biasprm[body.u_p_indices, 1] = -body.biases[body_p_indices, 1]
-            self.model.actuator_gainprm[body.u_v_indices, 0] = body.gains[body_v_indices, 0]
-            self.model.actuator_biasprm[body.u_v_indices, 2] = -body.gains[body_v_indices, 2]
-            self.model.actuator_gainprm[body.u_e_indices] = 0
-            self.model.actuator_biasprm[body.u_e_indices] = 0
-            body.ctrl_mode = struct.ControlMode.PD
-
-        if forces is not None:
-            self.model.actuator_forcelimited[body.u_p_indices] = 1
-            self.model.actuator_forcerange[body.u_p_indices, 0] = -forces
-            self.model.actuator_forcerange[body.u_p_indices, 1] = forces
-        # TODO: reset force range when None
-
-        if velocities is not None:
-            self.model.actuator_ctrllimited[body.u_v_indices] = 1
-            self.model.actuator_ctrlrange[body.u_v_indices, 0] = -velocities
-            self.model.actuator_ctrlrange[body.u_v_indices, 1] = velocities
-        # TODO: reset velocity range when None
-
         if joint_ids is None:
-            self.sim.data.ctrl[body.u_p_indices] = positions
-            if velocities is not None:
-                self.sim.data.ctrl[body.u_v_indices] = velocities
-
+            q_idx = np.array(range(body.num_actuated_joints))
         else:
             # check if valid joints
             self._check_joint_ids(body, joint_ids)
 
-            # if one joint, set its position
-            if isinstance(joint_ids, int):
-                self.sim.data.ctrl[body.u_p_indices[joint_ids]] = positions
-                if velocities is not None:
-                    self.sim.data.ctrl[body.u_v_indices[joint_ids]] = velocities
+            q_idx = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+            q_idx = q_idx[q_idx != -1]
 
-            # if multiple joints, set their positions
-            else:
-                q = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
-                q = q[q != -1]
-                self.sim.data.ctrl[body.u_p_indices + q] = positions
-                if velocities is not None:
-                    self.sim.data.ctrl[body.u_v_indices + q] = velocities
+        if len(q_idx) == 0:
+            raise ValueError("No actuated joints to set the positions to...")
+
+        # set gains
+        if kps is not None:
+            self.model.actuator_gainprm[body.u_p_indices[q_idx], 0] = kps  # for desired positions
+            self.model.actuator_biasprm[body.u_p_indices[q_idx], 1] = -kps  # for current positions
+        if kds is not None:
+            if velocities is not None:
+                self.model.actuator_gainprm[body.u_v_indices[q_idx], 0] = kds  # for desired velocities
+            self.model.actuator_biasprm[body.u_v_indices[q_idx], 2] = -kds  # for current velocities
+
+        # check the current control mode, deactivate the other motors by setting their gains and biases to zero
+        if velocities is None and np.any(body.ctrl_modes[q_idx] != struct.ControlMode.POSITION):
+            # Switch to position control mode
+            body.ctrl_modes[q_idx] = struct.ControlMode.POSITION
+
+            body_p_indices = body.u_p_indices - body.u_idx0
+            if kps is None:  # if gains were not provided, set back to the default ones
+                self.model.actuator_gainprm[body.u_p_indices[q_idx], 0] = body.gains[body_p_indices[q_idx], 0]
+                self.model.actuator_biasprm[body.u_p_indices[q_idx], 1] = body.biases[body_p_indices[q_idx], 1]
+            self.model.actuator_gainprm[body.u_v_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_v_indices[q_idx]] = 0
+            self.model.actuator_gainprm[body.u_e_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
+        elif velocities is not None and np.any(body.ctrl_mode[q_idx] != struct.ControlMode.PD):
+            # Switch to PD control mode
+            body.ctrl_modes[q_idx] = struct.ControlMode.PD
+
+            body_p_indices = body.u_p_indices - body.u_idx0
+            body_v_indices = body_p_indices + 1
+            if kps is None:  # if gains were not provided, set back to the default ones
+                self.model.actuator_gainprm[body.u_p_indices[q_idx], 0] = body.gains[body_p_indices[q_idx], 0]
+                self.model.actuator_biasprm[body.u_p_indices[q_idx], 1] = body.biases[body_p_indices[q_idx], 1]
+            if kds is None:  # if gains were not provided, set back to the default ones
+                self.model.actuator_gainprm[body.u_v_indices[q_idx], 0] = body.gains[body_v_indices[q_idx], 0]
+                self.model.actuator_biasprm[body.u_v_indices[q_idx], 2] = body.gains[body_v_indices[q_idx], 2]
+            self.model.actuator_gainprm[body.u_e_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
+
+        # set max forces
+        if forces is None:  # TODO: improve this part
+            body_p_indices = body.u_p_indices - body.u_idx0
+            self.model.actuator_forcelimited[body.u_p_indices[q_idx]] = body.force_limited[body_p_indices[q_idx]]
+            self.model.actuator_forcerange[body.u_p_indices[q_idx], 0] = -body.force_range[body_p_indices[q_idx], 0]
+            self.model.actuator_forcerange[body.u_p_indices[q_idx], 1] = body.force_range[body_p_indices[q_idx], 1]
+        else:
+            self.model.actuator_forcelimited[body.u_p_indices[q_idx]] = 1
+            self.model.actuator_forcerange[body.u_p_indices[q_idx], 0] = -forces
+            self.model.actuator_forcerange[body.u_p_indices[q_idx], 1] = forces
+
+        # set joint positions
+        self.sim.data.ctrl[body.u_p_indices[q_idx]] = positions
+
+        # set joint velocities if specified as well
+        if velocities is not None:
+            self.sim.data.ctrl[body.u_v_indices[q_idx]] = velocities
 
     def get_joint_positions(self, body_id, joint_ids=None):
         """
@@ -2930,9 +2955,9 @@ class Mujoco(Simulator):
             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)
+        q_idx = 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]]
+        qpos[q_idx != -1] = self.sim.data.qpos[body.q_idx1 + q_idx[q_idx != -1]]
         return qpos
 
     def set_joint_velocities(self, body_id, joint_ids, velocities, max_force=None):
@@ -2962,41 +2987,40 @@ class Mujoco(Simulator):
         #         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
 
+        # get body and q indices
         body = self._bodies[body_id]
-
-        # check the current control mode, deactivate the other motors by setting their gains and biases to zero
-        if body.ctrl_mode != struct.ControlMode.VELOCITY:
-            # Switch to velocity control mode
-            body.ctrl_mode = struct.ControlMode.VELOCITY
-            body_v_indices = body.u_v_indices - body.u_idx0
-            self.model.actuator_gainprm[body.u_p_indices] = 0
-            self.model.actuator_biasprm[body.u_p_indices] = 0
-            self.model.actuator_gainprm[body.u_v_indices, 0] = body.gains[body_v_indices, 0]
-            self.model.actuator_biasprm[body.u_v_indices, 2] = body.biases[body_v_indices, 2]
-            self.model.actuator_gainprm[body.u_e_indices] = 0
-            self.model.actuator_biasprm[body.u_e_indices] = 0
-
-        if max_force is not None:
-            self.model.actuator_forcelimited[body.u_v_indices] = 1
-            self.model.actuator_forcerange[body.u_v_indices, 0] = -max_force
-            self.model.actuator_forcerange[body.u_v_indices, 1] = max_force
-        # TODO: reset force range when None
-
         if joint_ids is None:
-            self.sim.data.ctrl[body.u_v_indices] = velocities
-
+            q_idx = np.array(range(body.num_actuated_joints))
         else:
             # check if valid joints
             self._check_joint_ids(body, joint_ids)
 
-            # if one joint, set its velocities
-            if isinstance(joint_ids, int):
-                self.sim.data.ctrl[body.u_v_indices[joint_ids]] = velocities
+            q_idx = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+            q_idx = q_idx[q_idx != -1]
 
-            # if multiple joints, set their velocities
-            else:
-                q_idx = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
-                self.sim.data.ctrl[body.u_v_indices + q_idx[q_idx != -1]] = velocities
+        if len(q_idx) == 0:
+            raise ValueError("No actuated joints to set the velocities to...")
+
+        # check the current control mode, deactivate the other motors by setting their gains and biases to zero
+        if np.any(body.ctrl_modes[q_idx] != struct.ControlMode.VELOCITY):
+            # Switch to velocity control mode
+            body.ctrl_modes[q_idx] = struct.ControlMode.VELOCITY
+            body_v_indices = body.u_v_indices - body.u_idx0
+            self.model.actuator_gainprm[body.u_p_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_p_indices[q_idx]] = 0
+            self.model.actuator_gainprm[body.u_v_indices[q_idx], 0] = body.gains[body_v_indices[q_idx], 0]
+            self.model.actuator_biasprm[body.u_v_indices[q_idx], 2] = body.biases[body_v_indices[q_idx], 2]
+            self.model.actuator_gainprm[body.u_e_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
+
+        if max_force is not None:
+            self.model.actuator_forcelimited[body.u_v_indices[q_idx]] = 1
+            self.model.actuator_forcerange[body.u_v_indices[q_idx], 0] = -max_force
+            self.model.actuator_forcerange[body.u_v_indices[q_idx], 1] = max_force
+        # TODO: reset force range when None
+
+        # set joint velocities
+        self.sim.data.ctrl[body.u_v_indices[q_idx]] = velocities
 
     def get_joint_velocities(self, body_id, joint_ids=None):
         """
@@ -3025,9 +3049,9 @@ class Mujoco(Simulator):
             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)
+        q_idx = 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]]
+        qvel[q_idx != -1] = self.sim.data.qvel[body.v_idx1 + q_idx[q_idx != -1]]
         return qvel
 
     def set_joint_accelerations(self, body_id, joint_ids, accelerations, q=None, dq=None):
@@ -3102,46 +3126,40 @@ class Mujoco(Simulator):
         #         self.sim.data.qfrc_applied[body.v_idx1 + q[q != -1]] = torques
 
         body = self._bodies[body_id]
-
-        # check the current control mode, deactivate the other motors by setting their gains and biases to zero
-        if body.ctrl_mode != struct.ControlMode.EFFORT:
-            # Switch to effort control mode
-            body.ctrl_mode = struct.ControlMode.EFFORT
-            body_e_indices = body.u_e_indices - body.u_idx0
-            self.model.actuator_gainprm[body.u_p_indices] = 0
-            self.model.actuator_biasprm[body.u_p_indices] = 0
-            self.model.actuator_gainprm[body.u_v_indices] = 0
-            self.model.actuator_biasprm[body.u_v_indices] = 0
-            self.model.actuator_gainprm[body.u_e_indices, 0] = body.gains[body_e_indices, 0]
-            self.model.actuator_biasprm[body.u_e_indices] = 0
-
-            # reset to original control and force ranges
-            self.model.actuator_ctrllimited[body.u_e_indices] = body.ctrl_limited[body_e_indices]
-            self.model.actuator_ctrlrange[body.u_e_indices, 0] = body.ctrl_range[body_e_indices, 0]
-            self.model.actuator_ctrlrange[body.u_e_indices, 1] = body.ctrl_range[body_e_indices, 1]
-            self.model.actuator_forcelimited[body.u_e_indices] = body.force_limited[body_e_indices]
-            self.model.actuator_forcerange[body.u_e_indices, 0] = body.force_range[body_e_indices, 0]
-            self.model.actuator_forcerange[body.u_e_indices, 1] = body.force_range[body_e_indices, 1]
-
         if joint_ids is None:
-            self.sim.data.ctrl[body.u_e_indices] = torques
-
+            q_idx = np.array(range(body.num_actuated_joints))
         else:
             # check if valid joints
             self._check_joint_ids(body, joint_ids)
 
-            # get q indices e.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
-            q_idx = body.get_q_idx(joint_ids, keep=True)
+            q_idx = body.get_q_idx(joint_ids, keep=True)  # E.g. [0, -1, 1, -1, 2, 3]  (-1 are for fixed joints)
+            q_idx = q_idx[q_idx != -1]
 
-            # if one joint, set its efforts (torques/forces)
-            if isinstance(q_idx, int):
-                if q_idx == -1:
-                    return
-                self.sim.data.ctrl[body.u_e_indices[q_idx]] = torques
+        if len(q_idx) == 0:
+            raise ValueError("No actuated joints to set the torques to...")
 
-            # if multiple joints, set their efforts (torques/forces)
-            else:
-                self.sim.data.ctrl[body.u_e_indices + q_idx[q_idx != -1]] = torques
+        # check the current control mode, deactivate the other motors by setting their gains and biases to zero
+        if np.any(body.ctrl_modes != struct.ControlMode.EFFORT):
+            # Switch to effort control mode
+            body.ctrl_modes[q_idx] = struct.ControlMode.EFFORT
+            body_e_indices = body.u_e_indices - body.u_idx0
+            self.model.actuator_gainprm[body.u_p_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_p_indices[q_idx]] = 0
+            self.model.actuator_gainprm[body.u_v_indices[q_idx]] = 0
+            self.model.actuator_biasprm[body.u_v_indices[q_idx]] = 0
+            self.model.actuator_gainprm[body.u_e_indices[q_idx], 0] = body.gains[body_e_indices[q_idx], 0]
+            self.model.actuator_biasprm[body.u_e_indices[q_idx]] = 0
+
+            # reset to original control and force ranges
+            self.model.actuator_ctrllimited[body.u_e_indices[q_idx]] = body.ctrl_limited[body_e_indices[q_idx]]
+            self.model.actuator_ctrlrange[body.u_e_indices[q_idx], 0] = body.ctrl_range[body_e_indices[q_idx], 0]
+            self.model.actuator_ctrlrange[body.u_e_indices[q_idx], 1] = body.ctrl_range[body_e_indices[q_idx], 1]
+            self.model.actuator_forcelimited[body.u_e_indices[q_idx]] = body.force_limited[body_e_indices[q_idx]]
+            self.model.actuator_forcerange[body.u_e_indices[q_idx], 0] = body.force_range[body_e_indices[q_idx], 0]
+            self.model.actuator_forcerange[body.u_e_indices[q_idx], 1] = body.force_range[body_e_indices[q_idx], 1]
+
+        # set joint efforts (torques/forces)
+        self.sim.data.ctrl[body.u_e_indices[q_idx]] = torques
 
     def get_joint_torques(self, body_id, joint_ids=None):
         """
@@ -3170,9 +3188,9 @@ class Mujoco(Simulator):
             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)
+        q_idx = 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]]
+        torques[q_idx != -1] = self.sim.data.qfrc_applied[body.v_idx1 + q_idx[q_idx != -1]]
         return torques
 
     def get_joint_reaction_forces(self, body_id, joint_ids=None):
diff --git a/pyrobolearn/simulators/test_mujoco.py b/pyrobolearn/simulators/test_mujoco.py
new file mode 100644
index 0000000..6bcac26
--- /dev/null
+++ b/pyrobolearn/simulators/test_mujoco.py
@@ -0,0 +1,214 @@
+# -*- coding: utf-8 -*-
+#!/usr/bin/env python
+"""Test with MuJoCo.
+"""
+
+import os
+import time
+import numpy as np
+from itertools import count
+# from pyrobolearn.simulators.bullet import Bullet
+from pyrobolearn.simulators.mujoco import Mujoco
+import mujoco_py as mujoco
+
+
+# sim = Bullet(render=True)
+sim = Mujoco(render=True, update_dynamically=True)
+print("Gravity: {}".format(sim.get_gravity()))
+print("Timestep: {}".format(sim.get_time_step()))
+# sim.set_gravity(np.zeros(3))
+
+# load floor
+# floor = sim.load_floor(dimension=20)
+
+print("qpos (before loading): ", sim.sim.data.qpos)
+
+# 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=[0.5, 0., 1.], mass=0, radius=0.05,
+#                                      rgba_color=(1, 0, 0, 0.5))
+# 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))
+
+print("qpos (after loading sphere): ", sim.sim.data.qpos)
+
+# print("Sphere id: ", sphere)
+# print("Num bodies before loading robot: ", sim.num_bodies())
+
+# load robot
+path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/rrbot/pendulum.urdf'
+# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/rrbot/rrbot.urdf'
+path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/franka/franka.urdf'
+# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/kuka/kuka_iiwa/iiwa14.urdf'
+path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/hyq2max/hyq2max.urdf'
+# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/anymal/anymal.urdf'
+# path = os.path.dirname(os.path.abspath(__file__)) + '/../robots/urdfs/centauro/centauro_stick.urdf'
+robot_name = path.split('/')[-1].split('.')[0]
+robot = sim.load_urdf(path, position=(0, 0, 0.8), use_fixed_base=True)
+
+print("qpos (after loading robot): ", sim.sim.data.qpos)
+
+print("Base position: ", sim.get_base_position(robot))
+
+print("Num bodies after loading robot: ", sim.num_bodies())
+
+# print("Robot")
+# print("base name: ", sim.get_body_info(robot))
+# print("mass: ", sim.get_base_mass(body_id=robot))
+
+# sim.remove_body(sphere)
+print(sim.sim.data.qpos)
+
+# sim.step()
+model = sim.model
+mjc_sim = sim.sim
+data = mjc_sim.data
+
+# The ones that appear in the following are because of the floor
+print("nbody", model.nbody - 1)  # total number of links
+# print("nuser_body", model.nuser_body)
+print("njnt", model.njnt)    # total number of joints
+print("nq", model.nq)            # total number of generalized coordinates (=num_actuated_joints); for free joints 7
+print("nv", model.nv)            # generalized velocities (nq - 1)
+print("na", model.na)
+print("nu", model.nu)
+print("qpos", data.qpos)
+print("qvel", data.qvel)
+print("act", data.act)
+# print("qpos", data.qpos, len(data.qpos))  # nqx1
+print("body_dofnum: ", model.body_dofnum)
+print("body_mass: ", model.body_mass)
+print("body_subtreemass", model.body_subtreemass)
+print("subtree_com", data.subtree_com)
+print("body_xpos: ", data.body_xpos)
+print("body pos: ", model.body_pos)
+print("body_xquat: ", data.body_xquat)
+# print("get_xpos: ", data.get_body_xpos(sim._bodies[sphere].tag_name))
+print("xfrc_applied: ", data.xfrc_applied)
+
+# joints
+# print("jnt_type: ", [["free", "ball", "slide", "hinge"][idx] for idx in model.jnt_type])
+# print("jnt_qposadr: ", model.jnt_qposadr)
+
+data.body_xpos[1] = np.array(range(3))
+
+num_joints = sim.num_joints(robot)
+num_actuated_joints = sim.num_actuated_joints(robot)
+num_links = sim.num_links(robot)
+
+joint_ids = sim.get_joint_type_ids(robot, list(range(num_joints)))
+joint_ids = np.array([i for i in range(num_joints) if joint_ids[i] != sim.JOINT_FIXED])
+
+# define amplitude and angular velocity when moving the sphere
+w = 0.01/2
+r = 0.2
+
+print("\nncam: ", model.ncam)
+print("cam_xpos: ", data.cam_xpos)
+print("cam_xmat: ", data.cam_xmat)
+print("cam_fovy: ", model.cam_fovy)
+print("Masses: ", sim.get_link_masses(robot))
+print("Names: ", sim.get_link_names(robot))
+print("Num links: ", num_links)
+print("Num joints: ", num_joints)
+print("Num actuated joints: ", num_actuated_joints)
+print("Contacts: ", data.contact)
+print("Sim state: ", mjc_sim.get_state())
+
+print("time: ", data.time)
+
+for i in range(num_joints):
+    print(sim.get_joint_info(robot, i))
+
+for i in range(num_links):
+    print(sim.get_link_state(robot, i))
+
+print("Jacobian: ", sim.calculate_jacobian(robot, num_actuated_joints))
+
+data.qpos[:] = np.zeros(model.nq)
+
+viewer = sim.viewer
+
+print(viewer.cam)
+print(dir(viewer.cam))
+
+# sim.reset_joint_states(robot, positions=[8.84305270e-05, 7.11378917e-02, -1.68059886e-04, -9.71690439e-01,
+#                                          1.68308810e-05, 3.71467111e-01, 5.62890805e-05])
+
+print(sim.print_xml())
+
+# TODO: the angles are reversed when setting qpos0
+# TODO: the robots
+if robot_name == 'franka':
+    positions = np.array([0.0277854, -0.97229678, -0.028778385, -2.427800237, -0.086976557, 1.442695354, -0.711514286,
+                          0., 0.])
+    sim.reset_joint_states(robot, joint_ids=joint_ids, positions=positions)
+elif robot_name == 'iiwa14':
+    sim.reset_joint_state(robot, joint_id=3, position=-np.pi/2)
+elif robot_name == 'pendulum':
+    sim.reset_joint_state(robot, joint_id=1, position=np.pi/8)
+
+# perform step
+for t in count():
+    # print("nbody", model.nbody)
+    # print("njnt", model.njnt)
+    # print("nq", model.nq)
+    # print("nv", model.nv)
+    # print("na", model.na)
+    # print("nu", model.nu)
+    # print("qpos", mjc_sim.data.qpos)  # nqx1
+    # print("body_dofnum: ", model.body_dofnum)
+    # print("body_mass: ", model.body_mass)
+    # print("body_subtreemass", model.body_subtreemass)
+    # if (t % 200) == 0:
+    #     print("Resetting position")
+    #     model.body_pos[1] = range(3)
+    #     # data.qpos[:3] = range(3)
+    #     print(model.body_pos)
+    #     print(mjc_sim.data.body_xpos[1])
+
+    # if t % 200 == 0:
+    #     # print(mjc_sim.data.subtree_com)
+    #     pos = np.zeros(3)
+    #     jacp = np.zeros(3 * model.nv)
+    #     jacr = np.zeros(3 * model.nv)
+    #     mujoco.functions.mj_jac(model, data, jacp, jacr, pos, 4)
+    #     print(jacp)
+    #     print(jacr)
+    #     # model.body_pos[1] = range(3)
+    #     # sim.reset_base_position(sphere, [2, -1, 3])
+
+    # position = np.array([0.5, r * np.cos(w * t + np.pi / 2), (1. - r) + r * np.sin(w * t + np.pi / 2)])
+    # sim.reset_base_position(sphere, position)
+    # data.qpos[:] = np.zeros(model.nq)
+
+    # print joint positions
+    # print(sim.get_joint_positions(robot))
+    # sim.set_joint_positions(robot, joint_ids, 0 * np.ones(num_actuated_joints))
+    # sim.reset_joint_states(robot, joint_ids=joint_ids, positions=positions)
+    # sim.set_joint_positions(robot, joint_ids, positions)
+    # sim.set_joint_positions(robot, joint_ids, [0., 0., 0., np.pi/2, 0., 0., 0.])
+
+    sim.set_joint_positions(robot, joint_ids, np.zeros(num_actuated_joints), kps=50, kds=1)
+    # sim.set_joint_positions(robot, joint_ids, np.pi/2 * np.ones(num_actuated_joints), kps=100, kds=10)
+    # sim.set_joint_positions(robot, joint_ids=1, positions=np.pi/2, kps=100, kds=10)
+    # sim.set_joint_velocities(robot, joint_ids, np.zeros(num_actuated_joints))
+    # sim.set_joint_velocities(robot, joint_ids, velocities=5 * np.ones(num_actuated_joints))
+    # sim.set_joint_torques(robot, joint_ids, torques=np.zeros(num_actuated_joints))
+    # sim.set_joint_torques(robot, joint_ids, torques=5 * np.ones(num_actuated_joints))
+
+    # if t == 500:
+    #     cylinder = sim.create_primitive_object(sim.GEOM_CYLINDER, position=(0, 2, 2), mass=1)
+
+    # if t == 2000:
+    #     sim.remove_body(cylinder)
+
+    # qpos = data.qpos
+    # print("time: ", data.time)
+    # # print(qpos)
+    # data.qpos[:] = np.zeros(len(qpos))
+    # print(data.xfrc_applied.shape)
+    # print(data.mocap_quat)
+
+    # print(mjc_sim.data.contact)
+    sim.step(sleep_time=sim.dt)