diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py
index a082ab9..635e5e1 100644
--- a/pyrobolearn/actions/action.py
+++ b/pyrobolearn/actions/action.py
@@ -916,13 +916,13 @@ class Action(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         actions = [copy.deepcopy(action, memo) for action in self.actions]
         data = copy.deepcopy(self._data)
         space = copy.deepcopy(self._space)
         action = self.__class__(actions=actions, data=data, space=space, name=self.name, ticks=self.ticks)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = action
-
         return action
diff --git a/pyrobolearn/actions/basic_actions.py b/pyrobolearn/actions/basic_actions.py
index 6db82cf..079349c 100644
--- a/pyrobolearn/actions/basic_actions.py
+++ b/pyrobolearn/actions/basic_actions.py
@@ -41,6 +41,8 @@ class FixedAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         data = copy.deepcopy(self._data)
         action = self.__class__(value=data)
         memo[self] = action
@@ -70,6 +72,8 @@ class FunctionalAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         function = copy.deepcopy(self.function)
         data = copy.deepcopy(self._data)
         action = self.__class__(function=function, initial_data=data)
diff --git a/pyrobolearn/actions/gym_actions.py b/pyrobolearn/actions/gym_actions.py
index d65a643..c7d53bf 100644
--- a/pyrobolearn/actions/gym_actions.py
+++ b/pyrobolearn/actions/gym_actions.py
@@ -59,6 +59,8 @@ class GymAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         env = copy.deepcopy(self.env)
         action = self.__class__(gym_env=env)
         memo[self] = action
diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py
index 552a425..d7060f4 100644
--- a/pyrobolearn/actions/robot_actions/joint_actions.py
+++ b/pyrobolearn/actions/robot_actions/joint_actions.py
@@ -60,6 +60,8 @@ class JointAction(RobotAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         action = self.__class__(robot=robot, joint_ids=joints)
@@ -94,6 +96,8 @@ class JointPositionAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         kp = copy.deepcopy(self.kp)
@@ -153,6 +157,8 @@ class JointPositionAndVelocityAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         kp = copy.deepcopy(self.kp)
@@ -201,6 +207,8 @@ class JointForceAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         f_min = copy.deepcopy(self.f_min)
@@ -241,6 +249,8 @@ class JointAccelerationAction(JointAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
         a_min = copy.deepcopy(self.a_min)
diff --git a/pyrobolearn/actions/robot_actions/link_actions.py b/pyrobolearn/actions/robot_actions/link_actions.py
index c64e5eb..cbf832e 100644
--- a/pyrobolearn/actions/robot_actions/link_actions.py
+++ b/pyrobolearn/actions/robot_actions/link_actions.py
@@ -50,6 +50,8 @@ class LinkAction(RobotAction):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         links = copy.deepcopy(self.links)
         action = self.__class__(robot, links)
diff --git a/pyrobolearn/actions/robot_actions/robot_actions.py b/pyrobolearn/actions/robot_actions/robot_actions.py
index 7200367..dd3fb57 100644
--- a/pyrobolearn/actions/robot_actions/robot_actions.py
+++ b/pyrobolearn/actions/robot_actions/robot_actions.py
@@ -59,6 +59,8 @@ class RobotAction(Action):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         robot = copy.deepcopy(self.robot, memo)
         action = self.__class__(robot=robot)
         memo[self] = action
diff --git a/pyrobolearn/actorcritics/actorcritic.py b/pyrobolearn/actorcritics/actorcritic.py
index 243f219..aeaf138 100644
--- a/pyrobolearn/actorcritics/actorcritic.py
+++ b/pyrobolearn/actorcritics/actorcritic.py
@@ -162,15 +162,15 @@ class ActorCritic(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         policy = copy.deepcopy(self.actor, memo) if isinstance(self.actor, Policy) else copy.deepcopy(self.actor)
         value = copy.deepcopy(self.critic, memo) if isinstance(self.critic, ValueApproximator) \
             else copy.deepcopy(self.critic)
         actor_critic = self.__class__(policy=policy, value=value)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = actor_critic
-
         return actor_critic
 
 
diff --git a/pyrobolearn/approximators/approximator.py b/pyrobolearn/approximators/approximator.py
index 87221d3..9311358 100644
--- a/pyrobolearn/approximators/approximator.py
+++ b/pyrobolearn/approximators/approximator.py
@@ -526,6 +526,9 @@ class Approximator(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         def get_inputs_outputs(items):
             if isinstance(items, list):
                 elements = []
@@ -548,10 +551,7 @@ class Approximator(object):
         approximator = self.__class__(inputs=inputs, outputs=outputs, model=model,
                                       preprocessors=preprocessors, postprocessors=postprocessors)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = approximator
-
         return approximator
 
 
diff --git a/pyrobolearn/approximators/nn_approximator.py b/pyrobolearn/approximators/nn_approximator.py
index 5dfd3d0..7644a33 100644
--- a/pyrobolearn/approximators/nn_approximator.py
+++ b/pyrobolearn/approximators/nn_approximator.py
@@ -82,11 +82,11 @@ class MLPApproximator(NNApproximator):
         """
 
         # check that the inputs and ouputs are 1D
-        # if not self._check1D(inputs):
+        # if not self._check_1d(inputs):
         #     raise ValueError("Length of input shape should be 1! Instead, got {}".format(inputs.shape))
         # print(outputs)
         # print(outputs.shape)
-        # if not self._check1D(outputs):
+        # if not self._check_1d(outputs):
         #     raise ValueError("Length of output shape should be 1! Instead, got {}".format(outputs.shape))
 
         input_size = self._size(inputs)
@@ -100,7 +100,8 @@ class MLPApproximator(NNApproximator):
         super(MLPApproximator, self).__init__(inputs, outputs, model, preprocessors=preprocessors,
                                               postprocessors=postprocessors)
 
-    def _check1D(self, arg):
+    @staticmethod
+    def _check_1d(arg):
         """Check that the given argument is a 1D vector, or simple array"""
         # if isinstance(arg, np.ndarray):
         shapes = arg.shape
diff --git a/pyrobolearn/dynamics/dynamic.py b/pyrobolearn/dynamics/dynamic.py
index d98995c..56e310e 100644
--- a/pyrobolearn/dynamics/dynamic.py
+++ b/pyrobolearn/dynamics/dynamic.py
@@ -327,6 +327,9 @@ class DynamicModel(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         state = copy.deepcopy(self.state, memo)
         action = copy.deepcopy(self.action, memo)
         next_state = copy.deepcopy(self.next_state, memo)
@@ -334,6 +337,7 @@ class DynamicModel(object):
         postprocessors = [copy.deepcopy(postprocessor, memo) for postprocessor in self.postprocessors]
         dynamic = self.__class__(state=state, action=action, next_state=next_state, preprocessors=preprocessors,
                                  postprocessors=postprocessors)
+
         memo[self] = dynamic
         return dynamic
 
@@ -540,6 +544,9 @@ class ParametrizedDynamicModel(DynamicModel):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         state = copy.deepcopy(self.state, memo)
         action = copy.deepcopy(self.action, memo)
         model = copy.deepcopy(self.model, memo)
@@ -550,6 +557,7 @@ class ParametrizedDynamicModel(DynamicModel):
         dynamic = self.__class__(state=state, action=action, model=model, next_state=next_state,
                                  distributions=distributions, preprocessors=preprocessors,
                                  postprocessors=postprocessors)
+
         memo[self] = dynamic
         return dynamic
 
diff --git a/pyrobolearn/envs/env.py b/pyrobolearn/envs/env.py
index bca2739..d34cc73 100644
--- a/pyrobolearn/envs/env.py
+++ b/pyrobolearn/envs/env.py
@@ -407,6 +407,8 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
         world = copy.deepcopy(self.world, memo)
         states = [copy.deepcopy(state, memo) for state in self.states]
         rewards = None if self.rewards is None else copy.deepcopy(self.rewards, memo)
@@ -415,9 +417,11 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         physics_randomizers = [copy.deepcopy(randomizer, memo) for randomizer in self.physics_randomizers]
         extra_info = copy.deepcopy(self.extra_info)
         actions = None if self.actions is None else [copy.deepcopy(action, memo) for action in self.actions]
-        return self.__class__(world=world, states=states, rewards=rewards, terminal_conditions=terminal_conditions,
-                              initial_state_generators=state_generators, physics_randomizers=physics_randomizers,
-                              extra_info=extra_info, actions=actions)
+        env = self.__class__(world=world, states=states, rewards=rewards, terminal_conditions=terminal_conditions,
+                             initial_state_generators=state_generators, physics_randomizers=physics_randomizers,
+                             extra_info=extra_info, actions=actions)
+        memo[self] = env
+        return env
 
 
 class BasicEnv(Env):
diff --git a/pyrobolearn/envs/gym_wrapper.py b/pyrobolearn/envs/gym_wrapper.py
index 9092e0c..371b3de 100644
--- a/pyrobolearn/envs/gym_wrapper.py
+++ b/pyrobolearn/envs/gym_wrapper.py
@@ -295,9 +295,9 @@ class GymEnvWrapper(gym.Env):
             attribute = functools.partial(attribute)
         return attribute
 
-    def __repr__(self):
-        """Return a representing object."""
-        return self.env.__repr__()
+    # def __repr__(self):
+    #     """Return a representing object."""
+    #     return self.env.__repr__()
 
     def __str__(self):
         """Return a string describing the class."""
diff --git a/pyrobolearn/envs/locomotion_env.py b/pyrobolearn/envs/locomotion_env.py
index 41ddebf..9b932ef 100644
--- a/pyrobolearn/envs/locomotion_env.py
+++ b/pyrobolearn/envs/locomotion_env.py
@@ -4,6 +4,7 @@
 Define the environment to perform a locomotion task; it mainly defines the reward function.
 """
 
+from pyrobolearn.simulators.simulator import Simulator
 from pyrobolearn.envs.env import Env
 from pyrobolearn.worlds import BasicWorld
 from pyrobolearn.states import State
@@ -20,7 +21,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class LocomotionEnv(Env):
+class LocomotionEnv(Env):  # TODO
     r"""Locomotion environment
 
     Define a simple environment for a locomotion task.
@@ -63,5 +64,5 @@ class LocomotionEnv(Env):
         terminal_condition = None
 
         super(LocomotionEnv, self).__init__(world, states, rewards=rewards,
-                                            terminal_condition=terminal_condition, extra_info=None)
+                                            terminal_conditions=terminal_condition, extra_info=None)
 
diff --git a/pyrobolearn/policies/policy.py b/pyrobolearn/policies/policy.py
index e3a8112..91277c2 100644
--- a/pyrobolearn/policies/policy.py
+++ b/pyrobolearn/policies/policy.py
@@ -718,6 +718,9 @@ class Policy(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         states = copy.deepcopy(self.states, memo)
         actions = copy.deepcopy(self.actions, memo)
         model = copy.deepcopy(self.model, memo)
@@ -727,8 +730,5 @@ class Policy(object):
         policy = self.__class__(states=states, actions=actions, model=model, rate=rate,
                                 preprocessors=preprocessors, postprocessors=postprocessors)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = policy
-
         return policy
diff --git a/pyrobolearn/rewards/reward.py b/pyrobolearn/rewards/reward.py
index d39f8ff..447954e 100644
--- a/pyrobolearn/rewards/reward.py
+++ b/pyrobolearn/rewards/reward.py
@@ -301,17 +301,16 @@ class Reward(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         state = copy.deepcopy(self.state, memo)
         action = copy.deepcopy(self.action, memo)
         rewards = [copy.deepcopy(reward, memo) for reward in self.rewards]
         range = copy.deepcopy(self.range)
         reward = self.__class__(state=state, action=action, rewards=rewards, range=range)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = reward
-
-        # return the copy
         return reward
 
     # for unary and binary operators, see `__init__()` method.
diff --git a/pyrobolearn/robots/base.py b/pyrobolearn/robots/base.py
index f34008b..651207f 100644
--- a/pyrobolearn/robots/base.py
+++ b/pyrobolearn/robots/base.py
@@ -7,8 +7,6 @@ Dependencies:
 """
 
 import copy
-import numpy as np
-# import quaternion
 
 from pyrobolearn.simulators import Simulator
 from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_matrix_from_quaternion
@@ -70,8 +68,11 @@ class Body(object):
     @id.setter
     def id(self, body_id):
         """Set the unique body id."""
-        # if not isinstance(body_id, int):
-        #     raise TypeError("Expecting the given simulator to be an integer, instead got: {}".format(type(body_id)))
+        if not isinstance(body_id, (int, long)):
+            raise TypeError("Expecting the given 'body_id' to be an integer, instead got: {}".format(type(body_id)))
+        if body_id < 0:
+            raise ValueError("The given 'body_id' is not a valid one; it should be positive, unique, and returned by "
+                             "the simulator.")
         self._id = body_id
 
     @property
@@ -90,6 +91,17 @@ class Body(object):
             raise TypeError("Expecting the given name to be a string, instead got: {}".format(type(name)))
         self._name = name
 
+    @property
+    def is_only_visual(self):
+        """Return True if the body doesn't have any collision shapes (i.e. it is only a visual body in the simulator)"""
+        return len(self.sim.get_collision_shape_data(self.id)) == 0
+
+    @property
+    def is_movable(self):
+        """Return True if the body is not fixed in the world. A body is fixed if it has a base mass of 0 and has at
+        least one collision shape."""
+        return not self.is_only_visual and self.base_mass != 0.
+
     @property
     def base_link_id(self):
         """Return the base link id."""
@@ -120,11 +132,21 @@ class Body(object):
         """Return the body position."""
         return self.sim.get_base_position(self.id)
 
+    @position.setter
+    def position(self, position):
+        """Set the body position. This is only valid in the simulator."""
+        self.sim.reset_base_position(self.id, position)
+
     @property
     def orientation(self):
         """Return the body orientation as a quaternion [x,y,z,w]."""
         return self.sim.get_base_orientation(self.id)
 
+    @orientation.setter
+    def orientation(self, quaternion):
+        """Set the body orientation given the quaternion [x,y,z,w]. This is only valid in the simulator."""
+        self.sim.reset_base_orientation(self.id, quaternion)
+
     # alias
     quaternion = orientation
 
@@ -170,8 +192,14 @@ class Body(object):
 
     @property
     def color(self):
+        """Return the color of the object."""
         return self.sim.get_visual_shape_data(self.id)[0][-1]
 
+    @color.setter
+    def color(self, color):
+        """Set the RGBA color of the object. This is only valid in the simulator."""
+        self.sim.change_visual_shape(object_id=self.id, link_id=-1, rgba_color=color)
+
     @property
     def mass(self):
         """Return the total mass of the body."""
@@ -179,10 +207,25 @@ class Body(object):
             self._mass = self.sim.get_mass(self.id)
         return self._mass
 
+    @mass.setter
+    def mass(self, mass):
+        """Set the mass of the body (its base). This is only valid in the simulator."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, mass=mass)
+
+    @property
+    def local_inertia_diagonal(self):
+        """Return the local inertia diagonal."""
+        return self.sim.get_dynamics_info(self.id, link_id=-1)[2]
+
+    @local_inertia_diagonal.setter
+    def local_inertia_diagonal(self, inertia):
+        """Set the local inertia diagonal."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, local_inertia_diagonal=inertia)
+
     @property
     def dimensions(self):
         """Return the dimensions of the body. Warnings: this should not be trusted too much..."""
-        return np.array(self.sim.get_visual_shape_data(self.id)[0][3])
+        return self.sim.get_visual_shape_data(self.id)[0][3]
 
     @property
     def num_joints(self):
@@ -199,6 +242,351 @@ class Body(object):
         """Return the center of mass."""
         return self.sim.get_center_of_mass_position(self.id)
 
+    @property
+    def lateral_friction(self):
+        """Return the floor lateral friction coefficient."""
+        return self.sim.get_dynamics_info(self.id, link_id=-1)[1]
+
+    @lateral_friction.setter
+    def lateral_friction(self, coefficient):
+        """Set the floor lateral friction coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, lateral_friction=coefficient)
+
+    @property
+    def rolling_friction(self):
+        """Return the floor rolling friction coefficient."""
+        return self.sim.get_dynamics_info(self.id, -1)[6]
+
+    @rolling_friction.setter
+    def rolling_friction(self, coefficient):
+        """Set the floor rolling friction coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, rolling_friction=coefficient)
+
+    @property
+    def spinning_friction(self):
+        """Return the floor spinning friction coefficient."""
+        return self.sim.get_dynamics_info(self.id, -1)[7]
+
+    @spinning_friction.setter
+    def spinning_friction(self, coefficient):
+        """Set the spinning friction coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, spinning_friction=coefficient)
+
+    @property
+    def restitution(self):
+        """Return the floor restitution (bounciness) coefficient."""
+        return self.sim.get_dynamics_info(self.id, -1)[5]
+
+    @restitution.setter
+    def restitution(self, coefficient):
+        """Set the floor restitution (bounciness) coefficient."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, restitution=coefficient)
+
+    @property
+    def contact_damping(self):
+        """Return the floor contact damping."""
+        return self.sim.get_dynamics_info(self.id, -1)[8]
+
+    @contact_damping.setter
+    def contact_damping(self, value):
+        """Set the floor contact damping value."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, contact_damping=value)
+
+    @property
+    def contact_stiffness(self):
+        """Return the floor contact stiffness."""
+        return self.sim.get_dynamics_info(self.id, -1)[9]
+
+    @contact_stiffness.setter
+    def contact_stiffness(self, value):
+        """Set the floor contact stiffness value."""
+        self.sim.change_dynamics(body_id=self.id, link_id=-1, contact_stiffness=value)
+
+    # just create setter
+    def _set_force(self, force):
+        """Set the given force (expressed in the world cartesian frame) on the center of mass of the body."""
+        self.apply_force(link_id=-1, force=force, position=None, frame=Simulator.WORLD_FRAME)
+
+    force = property(fset=_set_force)
+
+    ###########
+    # Methods #
+    ###########
+
+    def set_color(self, color, link_id=-1):
+        """Set the given RGBA color to the specified link. This is only valid in the simulator.
+
+        Args:
+            color (tuple of 4 float): RGBA color where each channel is between 0 and 1.
+            link_id (int): link id. By default, it is the base (-1).
+        """
+        self.sim.change_visual_shape(object_id=self.id, link_id=link_id, rgba_color=color)
+
+    def apply_force(self, link_id=-1, force=(0., 0., 0.), position=None, frame=Simulator.LINK_FRAME):
+        """
+        Apply the given force on the specified link of the current body.
+
+        Warnings:
+            - after each simulation step, the external forces are cleared to 0.
+            - this does not work when using `sim.setRealTimeSimulation(1)`.
+
+        Args:
+            link_id (int): link id to apply the force, if -1 it will apply the force on the base
+            force (np.array[3]): Cartesian forces to be applied on the body
+            position (np.array[3], None): position on the link where the force is applied (expressed in the given
+                cartesian frame, see next attribute :attr:`frame`). If None, it is the center of mass of the body
+                (or the link if specified).
+            frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local
+                link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the world frame.
+        """
+        self.sim.apply_external_force(self.id, link_id, force, position, frame)
+
+    def apply_external_torque(self, link_id=-1, torque=(0., 0., 0.), frame=Simulator.LINK_FRAME):
+        """
+        Apply an external torque on the body, or a link of the body. Note that after each simulation step, the external
+        torques are cleared to 0.
+
+        Warnings: This does not work when using `sim.setRealTimeSimulation(1)`.
+
+        Args:
+            link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
+            torque (float[3]): Cartesian torques to be applied on the body
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+                Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
+        """
+        self.sim.apply_external_torque(self.id, link_id=link_id, torque=torque, frame=frame)
+
+    def get_dynamics(self, link_id=-1):
+        """
+        Get dynamic information such as the mass, center of mass, friction and other properties of the specified link.
+
+        Args:
+            link_id (int): link/joint index or -1 for the base.
+
+        Returns:
+            float: mass in kg
+            float: lateral friction coefficient
+            np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
+                aligned with the principal axes of inertia.
+            np.float[3]: position of inertial frame in local coordinates of the joint frame
+            np.float[4]: orientation of inertial frame in local coordinates of joint frame
+            float: coefficient of restitution
+            float: rolling friction coefficient orthogonal to contact normal
+            float: spinning friction coefficient around contact normal
+            float: damping of contact constraints. -1 if not available.
+            float: stiffness of contact constraints. -1 if not available.
+        """
+        return self.sim.get_dynamics_info(self.id, link_id=link_id)
+
+    def change_dynamics(self, 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):
+        """
+        Change dynamic properties of the current body (or link) such as mass, friction and restitution coefficients,
+        etc.
+
+        Args:
+            link_id (int): link index or -1 for the base.
+            mass (float): change the mass of the link (or base for link index -1)
+            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.
+            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.
+            friction_anchor (int): enable or disable a friction anchor: positional friction correction (disabled by
+                default, unless set in the URDF contact section)
+            local_inertia_diagonal (np.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.
+            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`.
+        """
+        self.sim.change_dynamics(body_id=self.id, link_id=link_id, mass=mass, lateral_friction=lateral_friction,
+                                 spinning_friction=spinning_friction, rolling_friction=rolling_friction,
+                                 restitution=restitution, linear_damping=linear_damping,
+                                 angular_damping=angular_damping, contact_stiffness=contact_stiffness,
+                                 contact_damping=contact_damping, friction_anchor=friction_anchor,
+                                 local_inertia_diagonal=local_inertia_diagonal, joint_damping=joint_damping)
+
+    def get_collision_shape_data(self, link_id=-1):
+        """
+        Get the collision shape data associated with the specified link of the current body.
+
+        Args:
+            link_id (int): link index or -1 for the base.
+
+        Returns:
+            if not has_collision_shape_data:
+                tuple: empty tuple
+            else:
+                int: object unique id.
+                int: link id.
+                int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
+                np.float[3]: depends on geometry type:
+                    for GEOM_BOX: extents,
+                    for GEOM_SPHERE: dimensions[0] = radius,
+                    for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
+                    For GEOM_MESH: dimensions is the scaling factor.
+                str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
+                np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
+                np.float[4]: Local orientation of the collision frame with respect to the inertial frame
+        """
+        return self.sim.get_collision_shape_data(self.id, link_id=link_id)
+
+    def get_visual_shape_data(self, flags=-1):
+        """
+        Get the visual shape data associated with the current body. It will output a list of visual shape data.
+
+        Args:
+            flags (int, None): VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) will also provide `texture_unique_id`.
+
+        Returns:
+            list:
+                int: object unique id.
+                int: link index or -1 for the base
+                int: visual geometry type (TBD)
+                np.float[3]: dimensions (size, local scale) of the geometry
+                str: path to the triangle mesh, if any. Typically relative to the URDF, SDF or MJCF file location, but
+                    could be absolute
+                np.float[3]: position of local visual frame, relative to link/joint frame
+                np.float[4]: orientation of local visual frame relative to link/joint frame
+                list of 4 floats: URDF color (if any specified) in Red / Green / Blue / Alpha
+                int: texture unique id of the shape or -1 if None. This field only exists if using
+                    VISUAL_SHAPE_DATA_TEXTURE_UNIQUE_IDS (=1) flag.
+        """
+        return self.sim.get_visual_shape_data(self.id, flags=flags)
+
+    def apply_texture(self, texture, link_id=-1):
+        """
+        Apply the texture on the specified link of the current body.
+
+        Args:
+            texture (str): path to the texture.
+            link_id (int): link id. If -1, it will be the base.
+        """
+        texture = self.sim.load_texture(texture)
+        self.sim.change_visual_shape(self.id, link_id=link_id, texture_id=texture)
+
+    def get_contacts(self):
+        """
+        Return all the contacts made by the robot.
+
+        Warnings: note that in reality, you can't know if your link(s) is/are in contact with an object unless there
+        is a sensor attached to it. However, this can be useful in simulation to optimize, for instance, trajectories.
+
+        Returns:
+            list: list of contact points where each contact point has:
+                int: contact flag
+                int: unique id of body A (this should be the robot id)
+                int: unique id of body B
+                int: link index of body A (-1 for base, this should be the same as the given link)
+                int: link index of body B (-1 for base)
+                float[3]: contact position on A (in Cartesian world coordinates)
+                float[3]: contact position on B (in Cartesian world coordinates)
+                float[3]: contact normal on B pointing towards A
+                float: contact distance (positive for separation and negative for penetration)
+                float: normal force applied during the last simulation step
+        """
+        return self.sim.get_contact_points(body1=self.id)
+
+    def get_link_states(self, link_ids, compute_link_velocity=True, compute_forward_kinematics=True):
+        """
+        Return the state of the given link(s).
+
+        Warning: note that we do not convert the data here.
+
+        Args:
+            link_ids (int, list of int): link id, or list of desired link ids.
+            compute_link_velocity (bool): if True, the Cartesian world velocity will be computed and returned.
+            compute_forward_kinematics (bool): if True, the Cartesian world position/orientation will be recomputed
+                using forward kinematics.
+
+        Returns:
+            if 1 link:
+                [0] np.array[3]: Cartesian position of center of mass
+                [1] np.array[4]: Cartesian orientation of center of mass
+                [2] np.array[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame
+                [3] np.array[4]: local orientation (quat. [x,y,z,w]) offset of the inertial frame expressed in URDF
+                    link frame
+                [4] np.array[3]: world position of the URDF link frame
+                [5] np.array[4]: world orientation of the URDF link frame
+                [6] np.array[3]: Cartesian world linear velocity
+                [7] np.array[3]: Cartesian world angular velocity
+            if multiple links: list of above
+        """
+        if isinstance(link_ids, int):  # one link
+            return self.sim.get_link_state(self.id, link_ids, compute_velocity=compute_link_velocity,
+                                           compute_forward_kinematics=compute_forward_kinematics)
+        # multiple links
+        return self.sim.get_link_states(self.id, link_ids, compute_velocity=compute_link_velocity,
+                                        compute_forward_kinematics=compute_forward_kinematics)
+
+    def get_joint_states(self, joint_ids):
+        """
+        Get the state of the given joint(s).
+
+        Args:
+            joint_ids (int, list of int): id of the joint, or list of joint ids.
+
+        Returns:
+            for 1 joint:
+                float: joint position [rad]
+                float: joint velocity [rad/s]
+                np.array[6]: joint reaction forces [fx,fy,fz,mx,my,mz]
+                float: applied joint motor torque (during the last step)
+            for multiple joints: list of each joint state
+        """
+        if isinstance(joint_ids, int):
+            return self.sim.get_joint_state(self.id, joint_ids)
+        return self.sim.get_joint_states(self.id, joint_ids)
+
+    def get_joint_info(self, joint_ids):
+        """
+        Get information about the given joint(s).
+
+        Note that this method returns a lot of information, so specific methods have been implemented that return
+        only the desired information. Also, note that we do not convert the data here.
+
+        Args:
+            joint_ids (int, list of int): joint id, or list of joint ids.
+
+        Returns:
+            if 1 joint:
+                [0] int:        the same joint id as the input parameter
+                [1] str:        name of the joint (as specified in the URDF/SDF/etc file)
+                [2] int:        type of the joint which implie the number of position and velocity variables.
+                                The types include JOINT_REVOLUTE (=0), JOINT_PRISMATIC (=1), JOINT_SPHERICAL (=2),
+                                JOINT_PLANAR (=3), and JOINT_FIXED (=4).
+                [3] int:        q index - the first position index in the positional state variables for this body
+                [4] int:        dq index - the first velocity index in the velocity state variables for this body
+                [5] int:        flags (reserved)
+                [6] float:      the joint damping value (as specified in the URDF file)
+                [7] float:      the joint friction value (as specified in the URDF file)
+                [8] float:      the positional lower limit for slider and revolute joints
+                [9] float:      the positional upper limit for slider and revolute joints
+                [10] float:     maximum force specified in URDF. Note that this value is not automatically used.
+                                You can use maxForce in 'setJointMotorControl2'.
+                [11] float:     maximum velocity specified in URDF. Note that this value is not used in actual
+                                motor control commands at the moment.
+                [12] str:       name of the link (as specified in the URDF/SDF/etc file)
+                [13] np.array[3]:  joint axis in local frame (ignored for JOINT_FIXED)
+                [14] np.array[3]:  joint position in parent frame
+                [15] np.array[4]:  joint orientation in parent frame (x, y, z, w)
+                [16] int:       parent link index, -1 for base
+
+            if multiple joints: list of joint information (i.e. list of above)
+        """
+        if isinstance(joint_ids, int):
+            return self.sim.get_joint_info(self.id, joint_ids)
+        return [self.sim.get_joint_info(self.id, joint_id) for joint_id in joint_ids]
+
     #############
     # Operators #
     #############
@@ -209,7 +597,7 @@ class Body(object):
 
     def __str__(self):
         """Return a readable string about the class."""
-        return self.__class__.__name__
+        return self.__class__.__name__ + '(' + self.name + ')'
 
     def __copy__(self):
         """Return a shallow copy of the body. This can be overridden in the child class."""
@@ -221,14 +609,13 @@ class Body(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         simulator = copy.deepcopy(self.simulator, memo)
         body = self.__class__(simulator=simulator, body_id=self.id, name=self.name)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = body
-
-        # return the copy
         return body
 
 
@@ -240,6 +627,9 @@ class MovableBody(Body):
 
     def __init__(self, simulator, object_id=0, name=None):
         super(MovableBody, self).__init__(simulator, object_id, name=name)
+        # # check that the body is movable
+        # if not self.is_movable:
+        #     raise ValueError("The given id does not correspond to a movable body.")
 
     # def move(self, position=None, orientation=None):
     #     pass
diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py
index aa56cf4..c7d321d 100644
--- a/pyrobolearn/robots/robot.py
+++ b/pyrobolearn/robots/robot.py
@@ -45,8 +45,8 @@ class Robot(ControllableBody):
         Args:
             simulator: reference to the simulator such that the robot can access it.
             urdf (str): path to the URDF/MJCF file.
-            position (np.float[3]): initial position.
-            orientation (np.float[4]): initial orientation represented as a quaternion (x,y,z,w).
+            position (np.array[3]): initial position.
+            orientation (np.array[4]): initial orientation represented as a quaternion (x,y,z,w).
             fixed_base (bool, None): if True, the base of the robot will be fixed.
             scale (float): scaling factor.
         """
@@ -159,6 +159,9 @@ class Robot(ControllableBody):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         simulator = copy.deepcopy(self.simulator, memo)
         urdf = copy.deepcopy(self.urdf)
         position = copy.deepcopy(self.position)
@@ -166,11 +169,9 @@ class Robot(ControllableBody):
         robot = self.__class__(simulator=simulator, urdf=urdf, position=position, orientation=orientation,
                                fixed_base=self.fixed_base, scale=self.scale)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
-        memo[self] = robot
+        # TODO: copy sensors and actuators
 
-        # return the copy
+        memo[self] = robot
         return robot
 
     ##############
@@ -191,8 +192,8 @@ class Robot(ControllableBody):
         Get base position and orientation with respect to the world frame.
 
         Returns:
-            float[3]: position
-            np.float[4]: orientation (x, y, z, w)
+            np.array[3]: position
+            np.array[4]: orientation (x, y, z, w)
         """
         return self.sim.get_base_pose(self.id)
 
@@ -201,7 +202,7 @@ class Robot(ControllableBody):
         Return the base position.
 
         Returns:
-            float[3]: base position.
+            np.array[3]: base position.
         """
         return self.sim.get_base_position(self.id)
 
@@ -210,7 +211,7 @@ class Robot(ControllableBody):
         Get the base orientation in the form of a quaternion (x, y, z, w).
 
         Returns:
-            quaternion (np.float[4]): base orientation in the form of a quaternion (x, y, z, w).
+            quaternion (np.array[4]): base orientation in the form of a quaternion (x, y, z, w).
         """
         return self.sim.get_base_orientation(self.id)
 
@@ -219,7 +220,7 @@ class Robot(ControllableBody):
         Return the base linear and angular velocities.
 
         Returns:
-            np.float[6]: linear and angular velocities of the base
+            np.array[6]: linear and angular velocities of the base
         """
         lin_vel, ang_vel = self.sim.get_base_velocity(self.id)
         if concatenate:
@@ -231,7 +232,7 @@ class Robot(ControllableBody):
         Return the linear velocity of the base.
 
         Returns:
-            float[3]: linear velocity of the base
+            np.array[3]: linear velocity of the base
         """
         return self.sim.get_base_linear_velocity(self.id)
 
@@ -240,7 +241,7 @@ class Robot(ControllableBody):
         Return the angular velocity of the base.
 
         Returns:
-            float[3]: angular velocity of the base
+            np.array[3]: angular velocity of the base
         """
         return self.sim.get_base_angular_velocity(self.id)
 
@@ -296,7 +297,7 @@ class Robot(ControllableBody):
         Return the center of mass position.
 
         Returns:
-            np.float[3]: center of mass position
+            np.array[3]: center of mass position
         """
         self.com = self.sim.get_center_of_mass_position(self.id)
         return self.com
@@ -306,7 +307,7 @@ class Robot(ControllableBody):
         Return the center of mass velocity.
 
         Returns:
-            float[3]: center of mass velocity
+            np.array[3]: center of mass velocity
         """
         return self.sim.get_center_of_mass_velocity(self.id)
 
@@ -343,9 +344,9 @@ class Robot(ControllableBody):
     #     consists of its net linear momentum as well as its net angular momentum about its center of mass (CoM)" [1]
     #
     #     Args:
-    #         q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
+    #         q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
     #             get the current joint positions (but note that this could lead to a decrease of performance).
-    #         dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
+    #         dq (np.array[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
     #             get the current joint velocities (but note that this could lead to a decrease of performance).
     #
     #     Returns:
@@ -429,9 +430,9 @@ class Robot(ControllableBody):
                 [11] float:     maximum velocity specified in URDF. Note that this value is not used in actual
                                 motor control commands at the moment.
                 [12] str:       name of the link (as specified in the URDF/SDF/etc file)
-                [13] float[3]:  joint axis in local frame (ignored for JOINT_FIXED)
-                [14] float[3]:  joint position in parent frame
-                [15] float[4]:  joint orientation in parent frame (x, y, z, w)
+                [13] np.array[3]:  joint axis in local frame (ignored for JOINT_FIXED)
+                [14] np.array[3]:  joint position in parent frame
+                [15] np.array[4]:  joint orientation in parent frame (x, y, z, w)
                 [16] int:       parent link index, -1 for base
 
             if multiple joints: list of joint information (i.e. list of above)
@@ -455,9 +456,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 joint:
-                np.float[3]: joint axis
+                np.array[3]: joint axis
             if multiple joint:
-                [np.float[3]]: list of joint axis
+                [np.array[3]]: list of joint axis
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -511,9 +512,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 joint:
-                np.float[2]: lower and upper limit
+                np.array[2]: lower and upper limit
             if multiple joints:
-                np.float[N,2]: lower and upper limit for each specified joint
+                np.array[N,2]: lower and upper limit for each specified joint
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -531,7 +532,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: damping coefficient of the given joint
             if multiple joints:
-                float[N]: damping coefficient for each specified joint
+                np.array[N]: damping coefficient for each specified joint
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -549,7 +550,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: friction coefficient of the given joint
             if multiple joints:
-                np.float[N]: friction coefficient for each specified joint
+                np.array[N]: friction coefficient for each specified joint
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -569,7 +570,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: maximum force [N]
             if multiple joints:
-                np.float[N]: maximum force for each specified joint [N]
+                np.array[N]: maximum force for each specified joint [N]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -589,7 +590,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: maximum velocity [rad/s]
             if multiple joints:
-                float[N]: maximum velocities for each specified joint [rad/s]
+                np.array[N]: maximum velocities for each specified joint [rad/s]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -624,7 +625,7 @@ class Robot(ControllableBody):
             for 1 joint:
                 float: joint position [rad]
                 float: joint velocity [rad/s]
-                np.float[6]: joint reaction forces [fx,fy,fz,mx,my,mz]
+                np.array[6]: joint reaction forces [fx,fy,fz,mx,my,mz]
                 float: applied joint motor torque (during the last step)
             for multiple joints: list of each joint state
         """
@@ -646,7 +647,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: joint position [rad]
             if multiple joints:
-                np.float[N]: joint positions [rad]
+                np.array[N]: joint positions [rad]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -664,7 +665,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: joint velocity [rad/s]
             if multiple joints:
-                np.float[N]: joint velocities [rad/s]
+                np.array[N]: joint velocities [rad/s]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -683,7 +684,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: joint acceleration [rad/s^2]
             if multiple joints:
-                np.float[N]: joint accelerations [rad/s^2]
+                np.array[N]: joint accelerations [rad/s^2]
         """
         # check joint id
         if joint_ids is None:
@@ -710,9 +711,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 joint:
-                np.float[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm]
+                np.array[6]: joint reaction force (fx,fy,fz,mx,my,mz) [N,Nm]
             if multiple joints:
-                np.float[N,6]: joint reaction forces [N, Nm]
+                np.array[N,6]: joint reaction forces [N, Nm]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -730,7 +731,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: torque [Nm]
             if multiple joints:
-                np.float[N]: torques associated to the given joints [Nm]
+                np.array[N]: torques associated to the given joints [Nm]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -748,7 +749,7 @@ class Robot(ControllableBody):
             if 1 joint:
                 float: joint power [W]
             if multiple joints:
-                np.float[N]: power at each joint [W]
+                np.array[N]: power at each joint [W]
         """
         if joint_ids is None:
             joint_ids = self.joints
@@ -761,11 +762,11 @@ class Robot(ControllableBody):
 
         Args:
             joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints.
-            positions (float, np.float[N]): desired position, or list of desired positions [rad]
-            velocities (float, np.float[N], None): desired velocity, or list of desired velocities [rad/s]
-            kp (float, np.float[N], None): position gain(s)
-            kd (float, np.float[N], None): velocity gain(s)
-            forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the
+            positions (float, np.array[N]): desired position, or list of desired positions [rad]
+            velocities (float, np.array[N], None): desired velocity, or list of desired velocities [rad/s]
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            forces (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
                 default maximum force values.
         """
         if joint_ids is None:
@@ -773,51 +774,6 @@ class Robot(ControllableBody):
         self.sim.set_joint_positions(self.id, joint_ids, positions, velocities=velocities, kps=kp, kds=kd,
                                      forces=forces)
 
-        # if isinstance(joint_ids, int):
-        #     kwargs = {}
-        #     if kp is not None:
-        #         kwargs['positionGain'] = kp
-        #     if kd is not None:
-        #         kwargs['velocityGain'] = kd
-        #     if velocities is not None:
-        #         kwargs['targetVelocity'] = velocities
-        #     if forces is not None:
-        #         kwargs['force'] = forces
-        #     self.sim.setJointMotorControl2(self.id, joint_ids, self.sim.POSITION_CONTROL, position=positions,
-        #                                    **kwargs)
-        # else:
-        #     if joint_ids is None:
-        #         joint_ids = self.joints
-        #     kwargs = {}
-        #     if kp is not None:
-        #         if isinstance(kp, (float, int)):
-        #             kp = kp * np.ones(len(joint_ids))
-        #         kwargs['positionGains'] = kp
-        #     if kd is not None:
-        #         if isinstance(kd, (float, int)):
-        #             kd = kd * np.ones(len(joint_ids))
-        #         kwargs['velocityGains'] = kd
-        #     # qIdx = self.get_q_indices(jointId)
-        #     # print("pos: ", position)
-        #     # print(self.joint_limits[qIdx, 0], self.joint_limits[qIdx, 1])
-        #     # TODO: the following clip causes an error... Check Minitaur...
-        #     # position = np.clip(position, self.joint_limits[qIdx, 0], self.joint_limits[qIdx, 1])
-        #     # kp = kp.tolist()
-        #     # kd = kd.tolist()
-        #     # print("pos: ", position)
-        #     # print("kp: ", kp)
-        #     # print("kd: ", kd)
-        #     if velocities is not None:
-        #         if isinstance(velocities, (float, int)):
-        #             velocities = velocities * np.ones(len(joint_ids))
-        #         kwargs['targetVelocities'] = velocities
-        #     if forces is not None:
-        #         if isinstance(forces, (float, int)):
-        #             forces = forces * np.ones(len(joint_ids))
-        #         kwargs['forces'] = forces
-        #     self.sim.setJointMotorControlArray(self.id, joint_ids, self.sim.POSITION_CONTROL, positions=positions,
-        #                                        **kwargs)
-
     # TODO: max_velocities and forces
     def set_joint_velocities(self, velocities, joint_ids=None, forces=None, max_velocity=None):
         """
@@ -825,8 +781,8 @@ class Robot(ControllableBody):
 
         Args:
             joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints.
-            velocities (float, float[N]): desired velocity, or list of desired velocities [rad/s]
-            forces (float, np.float[N], None, bool): maximum motor torques / forces. If True, it will apply the
+            velocities (float, np.array[N]): desired velocity, or list of desired velocities [rad/s]
+            forces (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
                 default maximum force values.
             max_velocity (float, bool, None): if True, it will make sure that the given velocity(ies) are below their
                 authorized maximum value(s) (inferred from the URDF, or set previously by the user). If you already
@@ -843,7 +799,8 @@ class Robot(ControllableBody):
         dynamic which given the joint accelerations compute the joint torques to be applied.
 
         Args:
-            accelerations (float, float[N]): desired joint acceleration, or list of desired joint accelerations [rad/s^2]
+            accelerations (float, np.array[N]): desired joint acceleration, or list of desired joint accelerations
+                [rad/s^2]
             joint_ids (int, int[N], None): joint id, or list of joint ids. If None, get all the actuated joints.
             max_acceleration (bool, float, None): if True, it will make sure that the given acceleration(s) are below
                 their authorized maximum value(s). If you already did the check outside the method or if you don't want
@@ -885,10 +842,10 @@ class Robot(ControllableBody):
         Set the torque to the given joint(s) (using force/torque control).
 
         Args:
+            torque (float, np.array[N], None): desired torque(s) to apply to the joint(s) [N]. If None, it will apply
+                a torque of 0 to the given joint(s).
             joint_ids (int, int[N], None): joint id, or list of joint ids. If None, it will set the joint torques to
                 all (actuated) joints.
-            torque (float, float[N], None): desired torque(s) to apply to the joint(s) [N]. If None, it will apply
-                a torque of 0 to the given joint(s).
         """
         if isinstance(joint_ids, int):
             if torque is None:
@@ -922,14 +879,14 @@ class Robot(ControllableBody):
             joint_ids (int, int[N]): joint id, or list of joint ids
             control_mode (int): sim.VELOCITY_CONTROL (=0), sim.TORQUE_CONTROL (=1), sim.POSITION_CONTROL (=2)
             kwargs:
-                positions (float, float[N]) (optional): target position of the joint (in position control) [rad]
-                velocities (float, float[N]) (optional): target velocity of the joint (in position/velocity
+                positions (float, np.array[N]) (optional): target position of the joint (in position control) [rad]
+                velocities (float, np.array[N]) (optional): target velocity of the joint (in position/velocity
                     control) [rad/s]
-                forces (float, float[N]) (optional): in position/velocity control, this is the maximum force used
+                forces (float, np.array[N]) (optional): in position/velocity control, this is the maximum force used
                     to reach the target value. In torque control, this is the force/torque to be applied.
-                kp (float, float[N]) (optional): position gain :math:`Kp`
-                kd (float, float[N]) (optional): velocity gain :math:`Kd`
-                maxVelocity (float, float[N]) (optional): in position control, this limits the velocity to a maximum.
+                kp (float, np.array[N]) (optional): position gain :math:`Kp`
+                kd (float, np.array[N]) (optional): velocity gain :math:`Kd`
+                maxVelocity (float, np.array[N]) (optional): in position control, this limits the velocity to a maximum.
         """
         self.sim.set_joint_motor_control(self.id, joint_ids, control_mode, **kwargs)
 
@@ -1118,15 +1075,15 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                [0] np.float[3]: Cartesian position of center of mass
-                [1] np.float[4]: Cartesian orientation of center of mass
-                [2] np.float[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame
-                [3] np.float[4]: local orientation (quat. [x,y,z,w]) offset of the inertial frame expressed in URDF
+                [0] np.array[3]: Cartesian position of center of mass
+                [1] np.array[4]: Cartesian orientation of center of mass
+                [2] np.array[3]: local position offset of inertial frame (CoM) expressed in the URDF link frame
+                [3] np.array[4]: local orientation (quat. [x,y,z,w]) offset of the inertial frame expressed in URDF
                     link frame
-                [4] np.float[3]: world position of the URDF link frame
-                [5] np.float[4]: world orientation of the URDF link frame
-                [6] np.float[3]: Cartesian world linear velocity
-                [7] np.float[3]: Cartesian world angular velocity
+                [4] np.array[3]: world position of the URDF link frame
+                [5] np.array[4]: world orientation of the URDF link frame
+                [6] np.array[3]: Cartesian world linear velocity
+                [7] np.array[3]: Cartesian world angular velocity
             if multiple links: list of above
         """
         if isinstance(link_ids, int):  # one link
@@ -1169,7 +1126,7 @@ class Robot(ControllableBody):
             if 1 link:
                 float: mass of the given link
             else:
-                np.float[N]: mass of each link
+                np.array[N]: mass of each link
         """
         if isinstance(link_ids, int):
             return self.sim.get_dynamics_info(self.id, link_ids)[0]
@@ -1188,11 +1145,11 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: the link frame position in the world space
-                np.float[4]: Cartesian orientation of the link frame [x,y,z,w]
+                np.array[3]: the link frame position in the world space
+                np.array[4]: Cartesian orientation of the link frame [x,y,z,w]
             if multiple links:
-                np.float[Nx3], np.float[N,3]: link frame position of each link in world space
-                np.float[Nx4], np.float[N,4]: orientation of each link frame [x,y,z,w]
+                np.array[Nx3], np.array[N,3]: link frame position of each link in world space
+                np.array[Nx4], np.array[N,4]: orientation of each link frame [x,y,z,w]
 
         """
         return self.get_link_frame_world_positions(link_ids, flatten), self.get_link_frame_world_orientations(link_ids,
@@ -1209,9 +1166,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: the link frame position in the world space
+                np.array[3]: the link frame position in the world space
             if multiple links:
-                np.float[Nx3], np.float[N,3]: link frame position of each link in world space
+                np.array[Nx3], np.array[N,3]: link frame position of each link in world space
         """
         if isinstance(link_ids, int):
             return np.array(self.sim.get_link_state(self.id, link_ids)[4])
@@ -1233,9 +1190,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[4]: Cartesian orientation of the link frame [x,y,z,w]
+                np.array[4]: Cartesian orientation of the link frame [x,y,z,w]
             if multiple links:
-                np.float[Nx4], np.float[N,4]: orientation of each link frame [x,y,z,w]
+                np.array[Nx4], np.array[N,4]: orientation of each link frame [x,y,z,w]
         """
         if isinstance(link_ids, int):
             return self.sim.get_link_state(self.id, link_ids)[5]
@@ -1257,9 +1214,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: the link CoM position in the world space
+                np.array[3]: the link CoM position in the world space
             if multiple links:
-                np.float[Nx3], np.float[N,3]: CoM position of each link in world space
+                np.array[Nx3], np.array[N,3]: CoM position of each link in world space
         """
         if isinstance(link_ids, int):
             if link_ids == -1:
@@ -1284,9 +1241,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: the link CoM position
+                np.array[3]: the link CoM position
             if multiple links:
-                np.float[Nx3], np.float[N,3]: CoM position of each link
+                np.array[Nx3], np.array[N,3]: CoM position of each link
         """
         p1 = self.get_link_world_positions(link_ids, flatten=False)
         p0 = self.get_base_position() if wrt_link_id is None or wrt_link_id == -1 \
@@ -1307,9 +1264,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[4]: Cartesian orientation of the link CoM [x,y,z,w]
+                np.array[4]: Cartesian orientation of the link CoM [x,y,z,w]
             if multiple links:
-                float[Nx4], np.float[N,4]: CoM orientation of each link [x,y,z,w]
+                np.array[Nx4], np.array[N,4]: CoM orientation of each link [x,y,z,w]
         """
         if isinstance(link_ids, int):
             return self.sim.get_link_state(self.id, link_ids)[1]
@@ -1332,9 +1289,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[4]: Cartesian orientation of the link CoM [x,y,z,w]
+                np.array[4]: Cartesian orientation of the link CoM [x,y,z,w]
             if multiple links:
-                float[Nx4], np.float[N,4]: CoM orientation of each link [x,y,z,w]
+                np.array[Nx4], np.array[N,4]: CoM orientation of each link [x,y,z,w]
         """
         q1 = self.get_link_world_orientations(link_ids)
         if wrt_link_id is None or wrt_link_id == -1:
@@ -1361,9 +1318,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: linear velocity of the link in the Cartesian world space
+                np.array[3]: linear velocity of the link in the Cartesian world space
             if multiple links:
-                np.float[Nx3], np.float[N,3]: linear velocity of each link
+                np.array[Nx3], np.array[N,3]: linear velocity of each link
         """
         if isinstance(link_ids, int):
             return np.array(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6])
@@ -1385,9 +1342,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: angular velocity of the link in the Cartesian world space
+                np.array[3]: angular velocity of the link in the Cartesian world space
             if multiple links:
-                np.float[Nx3], np.float[N,3]: angular velocity of each link
+                np.array[Nx3], np.array[N,3]: angular velocity of each link
         """
         if isinstance(link_ids, int):
             return np.array(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[7])
@@ -1410,9 +1367,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                float[6]: linear and angular velocity of the link in the Cartesian world space
+                np.array[6]: linear and angular velocity of the link in the Cartesian world space
             if multiple links:
-                float[Nx6], float[N,6]: linear and angular velocity of each link
+                np.array[Nx6], np.array[N,6]: linear and angular velocity of each link
         """
         if isinstance(link_ids, int):
             lin_vel, ang_vel = self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6:8]
@@ -1440,9 +1397,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: the linear velocity of the given link wrt to the other link
+                np.array[3]: the linear velocity of the given link wrt to the other link
             if multiple links:
-                np.float[Nx3], np.float[N,3]: linear velocity of each link wrt to the other link(s)
+                np.array[Nx3], np.array[N,3]: linear velocity of each link wrt to the other link(s)
         """
         v1 = self.get_link_world_linear_velocities(link_ids, flatten=False)
         v0 = self.get_base_linear_velocity() if wrt_link_id is None or wrt_link_id == -1 \
@@ -1465,9 +1422,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[3]: the angular velocity of the given link wrt to the other link
+                np.array[3]: the angular velocity of the given link wrt to the other link
             if multiple links:
-                np.float[Nx3], np.float[N,3]: angular velocity of each link wrt to the other link(s)
+                np.array[Nx3], np.array[N,3]: angular velocity of each link wrt to the other link(s)
         """
         w1 = self.get_link_world_angular_velocities(link_ids, flatten=False)
         w0 = self.get_base_angular_velocity() if wrt_link_id is None or wrt_link_id == -1 \
@@ -1490,9 +1447,9 @@ class Robot(ControllableBody):
 
         Returns:
             if 1 link:
-                np.float[6]: the linear and angular velocity of the given link wrt to the other link
+                np.array[6]: the linear and angular velocity of the given link wrt to the other link
             if multiple links:
-                np.float[Nx6], np.float[N,6]: linear and angular velocity of each link wrt to the other link(s)
+                np.array[Nx6], np.array[N,6]: linear and angular velocity of each link wrt to the other link(s)
         """
         v1 = self.get_link_world_velocities(link_ids, flatten=False)
         v0 = self.get_base_velocity() if wrt_link_id is None or wrt_link_id == -1 \
@@ -1530,9 +1487,9 @@ class Robot(ControllableBody):
                     int: unique id of body B
                     int: link index of body A (-1 for base, this should be the same as the given link)
                     int: link index of body B (-1 for base)
-                    np.float[3]: contact position on A (in Cartesian world coordinates)
-                    np.float[3]: contact position on B (in Cartesian world coordinates)
-                    np.float[3]: contact normal on B pointing towards A
+                    np.array[3]: contact position on A (in Cartesian world coordinates)
+                    np.array[3]: contact position on B (in Cartesian world coordinates)
+                    np.array[3]: contact normal on B pointing towards A
                     float: contact distance (positive for separation and negative for penetration)
                     float: normal force applied during the last simulation step
             if multiple links: list of above
@@ -1550,8 +1507,8 @@ class Robot(ControllableBody):
 
         Args:
             link_ids (int, int[N]): link id, or list of desired link ids.
-            position (np.float[3], [float[3]], float[N,3]):
-            orientation (np.float[4], [float[4]], float[N,4]):
+            position (np.array[3], [np.array[3]], np.array[N,3]):
+            orientation (np.array[4], [np.array[4]], np.array[N,4]):
         """
         pass
 
@@ -1638,27 +1595,14 @@ class Robot(ControllableBody):
         Return the Homogeneous transform matrix given the position vector and the orientation.
 
         Args:
-            position (np.float[3]): position vector
-            orientation (np.float[4], np.float[3,3], np.float[3]): orientation
+            position (np.array[3]): position vector
+            orientation (np.array[4], np.array[3,3], np.array[3]): orientation (expressed as a quaternion [x,y,z,w],
+                3x3 rotation matrix, or roll-pitch-yaw angles).
 
         Returns:
-            np.float[4,4]: homogeneous matrix
+            np.array[4,4]: homogeneous matrix
         """
-        if isinstance(orientation, quaternion.quaternion):
-            R = quaternion.as_rotation_matrix(orientation)
-        else:
-            orientation = np.array(orientation)
-            if orientation.shape == (3,):  # RPY Euler angles
-                R = get_matrix_from_rpy(orientation)
-            elif orientation.shape == (4,):  # quaternion in the form [x,y,z,w]
-                R = get_matrix_from_quaternion(orientation)
-            elif orientation.shape == (3, 3):  # Rotation matrix
-                R = orientation
-            else:
-                raise ValueError("Expecting a quaternion, RPY Euler angles, or rotation matrix")
-
-        H = np.vstack((np.hstack((R, position.reshape(-1, 1))), np.array([[0, 0, 0, 1]])))
-        return H
+        return get_homogeneous_transform(position, orientation)
 
     ##############
     # Kinematics #
@@ -1678,14 +1622,14 @@ class Robot(ControllableBody):
 
         Args:
             link_id (int): link id.
-            q (np.float[N], None): joint positions of size N, where N is the number of DoFs. If None, it will compute q
+            q (np.array[N], None): joint positions of size N, where N is the number of DoFs. If None, it will compute q
                 based on the current joint positions.
             local_position (None, np.array[3]): the point on the specified link to compute the Jacobian (in link local
                 coordinates around its center of mass). If None, it will use the CoM position (in the link frame).
 
         Returns:
-            np.float[6,N], np.float[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of columns
-                depends if the base is fixed or floating.
+            np.array[6,N], np.array[6,(6+N)]: full geometric (linear and angular) Jacobian matrix. The number of
+                columns depends if the base is fixed or floating.
         """
         if q is None:
             q = self.get_joint_positions()
@@ -1694,8 +1638,6 @@ class Robot(ControllableBody):
                 raise ValueError("The length of q ({}) is different from the number of DoFs"
                                  " ({}).".format(len(q), len(self.joints)))
 
-        if isinstance(q, np.ndarray):
-            q = q.tolist()  # Note that q has to be a list; it doesn't work if numpy array in Pybullet
         dq = [0]*len(self.joints)
 
         # specify point on the link
@@ -1718,13 +1660,13 @@ class Robot(ControllableBody):
 
         Args:
             link_id (int): link id
-            q (np.float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based
+            q (np.array[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based
                 on the current joint positions.
             local_position: the point on the specified link to compute the Jacobian (in link local coordinates around
                 its center of mass). If None, it will use the CoM position (in the link frame).
 
         Returns:
-            np.float[3,N], np.float[3,(6+N)]: full linear geometric Jacobian matrix. The number of columns depends if
+            np.array[3,N], np.array[3,(6+N)]: full linear geometric Jacobian matrix. The number of columns depends if
                 the base is fixed or floating.
         """
         return self.get_jacobian(link_id, q, local_position)[:3]
@@ -1742,13 +1684,13 @@ class Robot(ControllableBody):
 
         Args:
             link_id (int): link id
-            q (np.float[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based
+            q (np.array[N]): joint positions of size N, where N is the number of DoFs. If None, it will compute q based
                 on the current joint positions.
             local_position: the point on the specified link to compute the Jacobian (in link local coordinates around
                 its center of mass). If None, it will use the CoM position (in the link frame).
 
         Returns:
-            np.float[3,N], np.float[3,(6+N)]: full angular geometric Jacobian matrix. The number of columns depends if
+            np.array[3,N], np.array[3,(6+N)]: full angular geometric Jacobian matrix. The number of columns depends if
                 the base is fixed or floating.
         """
         return self.get_jacobian(link_id, q, local_position)[3:]
@@ -1761,10 +1703,10 @@ class Robot(ControllableBody):
         Warnings: :math:`T` is singular when the pitch angle :math:`\theta_p = \pm \frac{\pi}{2}`
 
         Args:
-            rpy_angle (np.float[3]): RPY Euler angles [rad]
+            rpy_angle (np.array[3]): RPY Euler angles [rad]
 
         Returns:
-            np.float[3,3]: Jacobian matrix that maps RPY angle rates to angular velocities.
+            np.array[3,3]: Jacobian matrix that maps RPY angle rates to angular velocities.
         """
         r, p, y = rpy_angle
         T = np.array([[1., 0., np.sin(p)],
@@ -1773,19 +1715,19 @@ class Robot(ControllableBody):
         return T
 
     @staticmethod
-    def get_jacobian_derivative_zyz_to_angular_velocity(zyzAngle):
+    def get_jacobian_derivative_zyz_to_angular_velocity(zyz_angle):
         r"""
         Return the Jacobian that maps ZYZ angle rates to angular velocities, i.e. :math:`\omega = T(\phi) \dot{\phi}`.
 
         Warnings: :math:`T` is singular when the angle associated with `Y` is :math:`0` or :math:`\pi`.
 
         Args:
-            rpyAngle (np.float[3]): ZYZ Euler angles [rad]
+            zyz_angle (np.array[3]): ZYZ Euler angles [rad]
 
         Returns:
-            np.float[3,3]: Jacobian matrix that maps ZYZ angle rates to angular velocities.
+            np.array[3,3]: Jacobian matrix that maps ZYZ angle rates to angular velocities.
         """
-        z, y = zyzAngle[:2]
+        z, y = zyz_angle[:2]
         T = np.array([[0., -np.sin(z), np.cos(z) * np.sin(y)],
                       [0., np.cos(z), np.sin(z) * np.sin(y)],
                       [1., 0., np.cos(y)]])
@@ -1818,11 +1760,11 @@ class Robot(ControllableBody):
                 Euler angles then T is singular when the pitch angle :math:`\theta_p = \pm \frac{\pi}{2}.
 
         Args:
-            jacobian (np.float[6,N], np.float[6,6+N]): full geometric Jacobian.
-            rpy_angle (np.float[3]): RPY Euler angles
+            jacobian (np.array[6,N], np.array[6,6+N]): full geometric Jacobian.
+            rpy_angle (np.array[3]): RPY Euler angles
 
         Returns:
-            np.float[6,N], np.foat[6,(6+N)]: the full analytical Jacobian. The number of columns depends if the base
+            np.array[6,N], np.foat[6,(6+N)]: the full analytical Jacobian. The number of columns depends if the base
                 is fixed or floating.
         """
         T = self.get_jacobian_derivative_rpy_to_angular_velocity(rpy_angle)
@@ -1851,11 +1793,11 @@ class Robot(ControllableBody):
         Note that :math:`T` is singular when the pitch angle :math:`\theta_p = \pm \frac{\pi}{2}`.
 
         Args:
-            rpy_angle (np.float[3]): RPY Euler angles [rad]
-            dRPY (np.float[3]): time derivative of RPY Euler angles [rad/s]
+            rpy_angle (np.array[3]): RPY Euler angles [rad]
+            dRPY (np.array[3]): time derivative of RPY Euler angles [rad/s]
 
         Returns:
-            np.float[3]: angular velocities [rad/s]
+            np.array[3]: angular velocities [rad/s]
         """
         T = self.get_jacobian_derivative_rpy_to_angular_velocity(rpy_angle)
         return T.dot(dRPY)
@@ -1870,11 +1812,11 @@ class Robot(ControllableBody):
         corresponding angular velocities :math:`\omega` are not defined.
 
         Args:
-            rpy_angle (np.float[3]): RPY Euler angles [rad]
-            angular_velocity (np.float[3]): angular velocities [rad/s]
+            rpy_angle (np.array[3]): RPY Euler angles [rad]
+            angular_velocity (np.array[3]): angular velocities [rad/s]
 
         Returns:
-            np.float[3]: time derivative of RPY Euler angles [rad/s]
+            np.array[3]: time derivative of RPY Euler angles [rad/s]
 
         Raises:
             LinAlgError: if singular configuration.
@@ -1889,10 +1831,10 @@ class Robot(ControllableBody):
         Given the Jacobian, it returns :math:`JJ^T`. This relation is used in many places in robotics.
 
         Args:
-            jacobian (np.float[D,N]): Jacobian matrix
+            jacobian (np.array[D,N]): Jacobian matrix
 
         Returns:
-            np.float[D,D]: :math:`JJ^T`
+            np.array[D,D]: :math:`JJ^T`
         """
         return jacobian.dot(jacobian.T)
 
@@ -1907,14 +1849,14 @@ class Robot(ControllableBody):
         :math:`\dot{q} = \hat{J} v`.
 
         Args:
-            jacobian (np.float[D,N]): Jacobian matrix
+            jacobian (np.array[D,N]): Jacobian matrix
             damping_factor (float): damping factor
 
         Returns:
-            np.float[N,D]: DLS inverse matrix
+            np.array[N,D]: DLS inverse matrix
         """
         J, k = jacobian, damping_factor
-        return (J.T).dot(np.linalg.inv(J.dot(J.T) + k**2 * np.identity(J.shape[0])))
+        return J.T.dot(np.linalg.inv(J.dot(J.T) + k**2 * np.identity(J.shape[0])))
 
     # alias
     getDLSInverse = get_damped_least_squares_inverse
@@ -1925,10 +1867,10 @@ class Robot(ControllableBody):
         Return the right pseudo-inverse of the jacobian, i.e. :math:`J^\dagger = J^T(JJ^T)^{-1}`.
 
         Args:
-            jacobian (np.float[D,N]): Jacobian matrix
+            jacobian (np.array[D,N]): Jacobian matrix
 
         Returns:
-            np.float[N,N]: right pseudo-inverse of the Jacobian
+            np.array[N,N]: right pseudo-inverse of the Jacobian
         """
         return np.linalg.pinv(jacobian)
 
@@ -1940,10 +1882,10 @@ class Robot(ControllableBody):
         :math:`\dot{q} = J^\dagger v + P \dot{q}_0` with :math:`\dot{q}_0` representing arbitrary joint velocities.
 
         Args:
-            jacobian (np.float[D,N]): Jacobian matrix
+            jacobian (np.array[D,N]): Jacobian matrix
 
         Returns:
-            np.float[N,N]: null space projector matrix
+            np.array[N,N]: null space projector matrix
         """
         J = jacobian
         JJT = self.get_JJT(jacobian)
@@ -1957,7 +1899,7 @@ class Robot(ControllableBody):
         configurations (see [1]).
 
         Args:
-            jacobian (np.float[D,N]): Jacobian matrix
+            jacobian (np.array[D,N]): Jacobian matrix
 
         Returns:
             float: manipulability measure :math:`w(q)`
@@ -1978,7 +1920,7 @@ class Robot(ControllableBody):
         - around them, small velocities in the task/operational space may cause large velocities in the joint space
 
         Args:
-            jacobian (np.float[D,N]): Jacobian matrix
+            jacobian (np.array[D,N]): Jacobian matrix
 
         Returns:
             bool: True if in a singular configuration
@@ -2001,11 +1943,11 @@ class Robot(ControllableBody):
         where :math:`J^\dagger` is the right pseudo-inverse of J, i.e. :math:`J^\dagger = J^T(JJ^T)^{-1}`.
 
         Args:
-            jacobain (np.float[3,N], np.float[6,N]): Jacobian matrix
-            velocity (np.float[3], np.float[6]): linear and/or angular velocities
+            jacobian (np.array[3,N], np.array[6,N]): Jacobian matrix
+            velocity (np.array[3], np.array[6]): linear and/or angular velocities
 
         Returns:
-            np.float[N]: joint velocities
+            np.array[N]: joint velocities
         """
         Jpinv = self.get_pinv_jacobian(jacobian)
         return Jpinv.dot(velocity)
@@ -2019,7 +1961,7 @@ class Robot(ControllableBody):
         .. math:: v = J(q) \dot{q}
 
         Returns:
-            np.float[6]: Cartesian linear and angular velocities
+            np.array[6]: Cartesian linear and angular velocities
         """
         return jacobian.dot(dq)
 
@@ -2032,20 +1974,20 @@ class Robot(ControllableBody):
 
         Args:
             link_id (int): end effector link index.
-            position (np.float[3]): target position of the end effector (its link coordinate, not center of mass
+            position (np.array[3]): target position of the end effector (its link coordinate, not center of mass
                 coordinate!). By default this is in Cartesian world space, unless you provide `q_curr` joint angles.
-            orientation (np.float[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not
+            orientation (np.array[4]): target orientation in Cartesian world space, quaternion [x,y,w,z]. If not
                 specified, pure position IK will be used.
-            lower_limits (np.float[N], list of N floats): lower joint limits. Optional null-space IK.
-            upper_limits (np.float[N], list of N floats): upper joint limits. Optional null-space IK.
-            joint_ranges (np.float[N], list of N floats): range of value of each joint.
-            rest_poses (np.float[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest
+            lower_limits (np.array[N], list of N floats): lower joint limits. Optional null-space IK.
+            upper_limits (np.array[N], list of N floats): upper joint limits. Optional null-space IK.
+            joint_ranges (np.array[N], list of N floats): range of value of each joint.
+            rest_poses (np.array[N], list of N floats): joint rest poses. Favor an IK solution closer to a given rest
                 pose.
-            joint_dampings (np.float[N], list of N floats): joint damping factors. Allow to tune the IK solution using
+            joint_dampings (np.array[N], list of N floats): joint damping factors. Allow to tune the IK solution using
                 joint damping factors.
             solver (int): p.IK_DLS (=0) or p.IK_SDLS (=1), Damped Least Squares or Selective Damped Least Squares, as
                 described in the paper by Samuel Buss "Selectively Damped Least Squares for Inverse Kinematics".
-            q_curr (np.float[N]): list of joint positions. By default PyBullet uses the joint positions of the body.
+            q_curr (np.array[N]): list of joint positions. By default PyBullet uses the joint positions of the body.
                 If provided, the target_position and targetOrientation is in local space!
             max_iters (int): maximum number of iterations. Refine the IK solution until the distance between target
                 and actual end effector position is below this threshold, or the `max_iters` is reached.
@@ -2053,7 +1995,7 @@ class Robot(ControllableBody):
                 end effector position is below this threshold, or the `max_iters` is reached.
 
         Returns:
-            np.float[M]: joint positions (for each actuated joint).
+            np.array[M]: joint positions (for each actuated joint).
         """
         # calculate joint positions solving IK and return them
         return self.sim.calculate_inverse_kinematics(self.id, link_id, position=position, orientation=orientation,
@@ -2111,12 +2053,12 @@ class Robot(ControllableBody):
         of motion in task/operational space (instead of joint space), check the references [1-4].
 
         Args:
-            q (np.float[M]): joint positions
-            dq (np.float[M]): joint velocities
-            des_ddq (np.float[M]): desired joint accelerations
+            q (np.array[M]): joint positions
+            dq (np.array[M]): joint velocities
+            des_ddq (np.array[M]): desired joint accelerations
 
         Returns:
-            np.float[M]: joint torques computed using the rigid-body equation of motion
+            np.array[M]: joint torques computed using the rigid-body equation of motion
 
         References:
             [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
@@ -2165,12 +2107,12 @@ class Robot(ControllableBody):
         of motion in task/operational space (instead of joint space), check the references [1-4].
 
         Args:
-            q (np.float[M]): joint positions
-            dq (np.float[M]): joint velocities
-            torques (np.float[M]): desired joint torques
+            q (np.array[M]): joint positions
+            dq (np.array[M]): joint velocities
+            torques (np.array[M]): desired joint torques
 
         Returns:
-            np.float[M]: joint accelerations computed using the rigid-body equation of motion
+            np.array[M]: joint accelerations computed using the rigid-body equation of motion
 
         References:
             [1] "Rigid Body Dynamics Algorithms", Featherstone, 2008, chap1.1
@@ -2200,12 +2142,12 @@ class Robot(ControllableBody):
             joints. If the base is fixed, it will return a [N,N] inertia matrix
 
         Args:
-            q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
+            q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
                 get the current joint positions (but note that this could lead to a decrease of performance).
             q_idx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N).
 
         Returns:
-            float[N,N], float[6+N,6+N], float[M,M]: inertia matrix
+            np.array[N,N], np.array[6+N,6+N], np.array[M,M]: inertia matrix
         """
         if q is None:
             q = self.get_joint_positions()
@@ -2218,14 +2160,14 @@ class Robot(ControllableBody):
         # make sure that we have all the joints even the fixed ones
         q_aug = np.zeros(self.num_joints)
         q_aug[self.joints] = q
-        q_aug = q_aug.tolist()  # Note that pybullet doesn't accept numpy arrays here
 
         if q_idx is None:
             return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))
         return np.array(self.sim.calculate_mass_matrix(self.id, q_aug))[q_idx, q_idx]
 
-    def get_cartesian_inertia_matrix(self, H=None, Ja=None):
-        """
+    @staticmethod
+    def get_cartesian_inertia_matrix(H=None, Ja=None):
+        r"""
         Return the cartesian inertia matrix.
 
         .. math:: H_{x}(q) = J_{a}^{-T}(q) H(q) J_{a}^{-1}(q)
@@ -2236,27 +2178,27 @@ class Robot(ControllableBody):
         :math:`v = [\dot{p} \omega]^T = J(q) \dot{q}`, where :math:`\omega` are the angular velocities.
 
         Args:
-            H (float[N,N], None): Joint inertia matrix. If None, it will be computed here (the q's then need to be
+            H (np.array[N,N], None): Joint inertia matrix. If None, it will be computed here (the q's then need to be
                 provided).
-            Ja (float[6,N], None): Analytical Jacobian. If None, it will be computed here (the q's then need to be
+            Ja (np.array[6,N], None): Analytical Jacobian. If None, it will be computed here (the q's then need to be
                 provided and the link_id
 
         Returns:
-            float[6,6]: Cartesian inertia matrix
+            np.array[6,6]: Cartesian inertia matrix
         """
         Ja_inv = np.linalg.inv(Ja)
         return Ja_inv.T.dot(H).dot(Ja_inv)
 
     def get_kinetic_energy(self, q=None, dq=None, q_idx=None):
-        """
+        r"""
         Return the kinetic energy due to the movement of the specified joint(s).
 
         .. math:: T(q,\dot{q}) = \frac{1}{2} \dot{q}^T H(q) \dot{q}
 
         Args:
-            q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
+            q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
                 get the current joint positions (but note that this could lead to a decrease of performance).
-            dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
+            dq (np.array[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
                 get the current joint velocities (but note that this could lead to a decrease of performance).
             q_idx (slice, None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N),
                 and the joint velocities vector.
@@ -2272,7 +2214,7 @@ class Robot(ControllableBody):
         return 1./2 * dq.dot(H.dot(dq))
 
     def get_gravity_potential_energy(self, q=None, q_idx=None, g=np.array((0., 0., -9.81))):
-        """
+        r"""
         Return the potential energy due to gravity.
 
         .. math:: V(q) = - \sum_{i=1}^N m_{l_i} g^T p_{l_i}
@@ -2281,11 +2223,11 @@ class Robot(ControllableBody):
         vector, and :math:`p_l` is the position of the link.
 
         Args:
-            q (float[N], None): joint positions of size N, where N is the total number of DoFs. THIS IS CURRENTLY
+            q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. THIS IS CURRENTLY
                 NOT USED, as we can get the link positions from the simulator (instead of using forward kinematics).
             q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N),
                 and the joint velocities vector.
-            g (np.float[3]): gravity vector.
+            g (np.array[3]): gravity vector.
 
         Returns:
             float: potential energy due to gravity
@@ -2299,7 +2241,7 @@ class Robot(ControllableBody):
         return np.sum((p.T * m).T * g)
 
     def get_potential_energy(self, q=None, dq=None, q_idx=None):
-        """
+        r"""
         Return the potential energy of the system.
 
         WARNING: Note that we currently assume rigid body systems (thus rigid links). With this assumption, the
@@ -2307,9 +2249,9 @@ class Robot(ControllableBody):
         `get_gravity_potential_energy`.
 
         Args:
-            q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
+            q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
                 get the current joint positions (but note that this could lead to a decrease of performance).
-            dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
+            dq (np.array[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
                 get the current joint velocities (but note that this could lead to a decrease of performance).
             q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N),
                 and the joint velocities vector.
@@ -2320,7 +2262,7 @@ class Robot(ControllableBody):
         return self.get_gravity_potential_energy(q, q_idx)
 
     def get_lagrangian(self, q=None, dq=None, q_idx=None):
-        """
+        r"""
         Return the Lagrangian evaluate at the given configuration.
 
         .. math:: L(q, \dot{q}) = T(q, \dot{q}) - V(q)
@@ -2328,9 +2270,9 @@ class Robot(ControllableBody):
         where :math:`T` and :math:`V` are the kinetic and potential energy respectively.
 
         Args:
-            q (float[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
+            q (np.array[N], None): joint positions of size N, where N is the total number of DoFs. If None, it will
                 get the current joint positions (but note that this could lead to a decrease of performance).
-            dq (float[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
+            dq (np.array[M], None): joint velocities of size M (with 0 < M <= N). If None, it will
                 get the current joint velocities (but note that this could lead to a decrease of performance).
             q_idx (int[M], None): if provided, it will slice the inertia matrix at the given q indices (0 < M <= N),
                 and the joint velocities vector.
@@ -2342,39 +2284,9 @@ class Robot(ControllableBody):
         V = self.get_potential_energy(q=q, q_idx=q_idx)
         return T - V
 
-    def apply_external_force(self, force, link_id=-1, position=(0., 0., 0.), frame=1):
-        """
-        Apply an external force on a body, or a link of the body. Note that after each simulation step, the external
-        forces are cleared to 0.
-
-        Warnings: This does not work when using `sim.setRealTimeSimulation(1)`.
-
-        Args:
-            force (float[3]): Cartesian forces to be applied on the body
-            link_id (int): link id to apply the force, if -1 it will apply the force on the base
-            position (float[3]): position on the link where the force is applied.
-            frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local
-                link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the link frame.
-        """
-        self.sim.apply_external_force(self.id, link_id, force, position, frame)
-
-    def apply_external_torque(self, torque, link_id=-1, frame=1):
-        """
-        Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external
-        torques are cleared to 0.
-
-        Warnings: This does not work when using `sim.setRealTimeSimulation(1)`.
-
-        Args:
-            torque (float[3]): Cartesian torques to be applied on the body
-            link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
-            frame (int): allows to specify the coordinate system of torque. sim.LINK_FRAME (=1) for local
-                link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the link frame.
-        """
-        self.sim.apply_external_torque(self.id, link_id, force, frame)
-
-    def get_joint_torques_from_cartesian_wrench(self, jacobian, wrench):
-        """
+    @staticmethod
+    def get_joint_torques_from_cartesian_wrench(jacobian, wrench):
+        r"""
         Return the joint torques from the given Cartesian wrench (=force and torque) using the provided Jacobian.
 
         .. math:: \tau = J^T(q) f
@@ -2383,12 +2295,13 @@ class Robot(ControllableBody):
         applied at the link), and :math:`J` is the geometric Jacobian.
 
         Returns:
-            float[N]: joint torques [Nm]
+            np.array[N]: joint torques [Nm]
         """
         return jacobian.T.dot(wrench)
 
-    def get_cartesian_wrench_from_joint_torques(self, jacobian, torque):
-        """
+    @staticmethod
+    def get_cartesian_wrench_from_joint_torques(jacobian, torque):
+        r"""
         Return the Cartesian wrench (=force and torque) from the given joint torques using the provided Jacobian.
 
         .. math:: f = J(J^TJ)^{-1} \tau
@@ -2397,7 +2310,7 @@ class Robot(ControllableBody):
         applied at the link), and :math:`J` is the geometric Jacobian.
 
         Returns:
-            float[6]: forces and torques in the Cartesian world space [N,Nm]
+            np.array[6]: forces and torques in the Cartesian world space [N,Nm]
         """
         J = jacobian
         return J.dot(np.linalg.inv(J.T.dot(J))).dot(torque)
@@ -2412,8 +2325,8 @@ class Robot(ControllableBody):
         """
         self.coriolis_and_gravity_compensation = enable
 
-    def get_coriolis_and_gravity_compensation_torques(self, q=None, dq=None, qIdx=None):
-        """
+    def get_coriolis_and_gravity_compensation_torques(self, q=None, dq=None, q_idx=None):
+        r"""
         Return the torques that need to be applied to the robot joints such that it compensates for gravity and
         Coriolis effects.
 
@@ -2423,46 +2336,63 @@ class Robot(ControllableBody):
 
         we can see that if we set :math:`F` and :math:`\ddot{q}` to 0, then we have:
 
-        .. math::  \tau = C(q,\dot{q}) \dot{q} + g(q).
+        .. math:: \tau = C(q,\dot{q}) \dot{q} + g(q).
 
         These are the torques that need to be applied to the robot joints to compensate for gravity and Coriolis
         effects.
 
         Args:
-            q (float[N], None): all the joint positions. If None, it will get the current joint positions of all the
+            q (np.array[N], None): all the joint positions. If None, it will get the current joint positions of all the
                 joints. However, note that if you already got the joint positions in your code,
                 it is better to pass them to this method for performance.
-            dq (float[N], None): all the joint velocities. If None, it will get the current joint velocities of
+            dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
                 all the joints.
-            qIdx (int[M], None): slice the torques at the given q indices (0 < M <= N).
+            q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
 
         Returns:
-            float[M]: joint torques to be applied [Nm]
+            np.array[M]: joint torques to be applied [Nm]
         """
         if q is None:
             q = self.get_joint_positions()
         if dq is None:
             dq = self.get_joint_velocities()
 
-        ddq = np.zeros(len(self.joints)).tolist()
+        ddq = np.zeros(len(self.joints))
 
-        if isinstance(q, np.ndarray):
-            q = q.tolist()
-        if isinstance(dq, np.ndarray):
-            dq = dq.tolist()
-
-        if qIdx is None:
+        if q_idx is None:
             return self.sim.calculate_inverse_dynamics(self.id, q, dq, ddq)
-        return self.sim.calculate_inverse_dynamics(self.id, q, dq, ddq)[qIdx]
+        return self.sim.calculate_inverse_dynamics(self.id, q, dq, ddq)[q_idx]
 
-    def get_gravity_compensation_torques(self, q=None, qIdx=None):
+    def get_gravity_compensation_torques(self, q=None, q_idx=None):
+        r"""
+        Return the torques that need to be applied to the robot joints such that it compensates for gravity.
+
+        From the equations of motion:
+
+        .. math:: H(q) \ddot{q} + C(q,\dot{q}) \dot{q} + g(q) = \tau + J^T(q) F,
+
+        we can see that if we set :math:`F, \dot{q}, \ddot{q}` to 0, then we have:
+
+        .. math:: \tau = g(q).
+
+        These are the torques that need to be applied to the robot joints to compensate for gravity.
+
+        Args:
+            q (np.array[N], None): all the joint positions. If None, it will get the current joint positions of all the
+                joints. However, note that if you already got the joint positions in your code,
+                it is better to pass them to this method for performance.
+            q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
+
+        Returns:
+            np.array[M]: joint torques to be applied [Nm]
+        """
         if q is None:
             q = self.get_joint_positions()
         dq = np.zeros(len(q))
-        return self.get_coriolis_and_gravity_compensation_torques(q, dq, qIdx)
+        return self.get_coriolis_and_gravity_compensation_torques(q, dq, q_idx)
 
-    def apply_coriolis_and_gravity_compensation(self, q=None, dq=None, qIdx=None, external_torques=0.):
-        """
+    def apply_coriolis_and_gravity_compensation(self, q=None, dq=None, q_idx=None, external_torques=0.):
+        r"""
         Apply Coriolis and Gravity Compensation; set the torques using torque control.
 
         The torques are given by:
@@ -2470,20 +2400,22 @@ class Robot(ControllableBody):
         .. math::  \tau = C(q,\dot{q}) \dot{q} + g(q).
 
         Args:
-            q (float[N], None): all the joint positions. If None, it will get the current joint positions of all the
+            q (np.array[N], None): all the joint positions. If None, it will get the current joint positions of all the
                 joints. However, note that if you already got the joint positions in your code,
                 it is better to pass them to this method for performance.
-            dq (float[N], None): all the joint velocities. If None, it will get the current joint velocities of
+            dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
                 all the joints.
-            qIdx (int[M], None): slice the torques at the given q indices (0 < M <= N).
+            q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
+            external_torques (np.array[M], float): external torques to be applied.
         """
-        jointId = self.joints if qIdx is None else self.joints[qIdx]
-        torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, qIdx)
-        self.set_joint_torques(jointId, torques + external_torques)
+        joint_ids = self.joints if q_idx is None else self.joints[q_idx]
+        torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, q_idx)
+        self.set_joint_torques(torques=torques + external_torques, joint_ids=joint_ids)
 
     # TODO: finish to implement the method + think about multiple links + think about dimensions
-    def get_active_compliant_torques(self, q=None, dq=None, qIdx=None, jacobian=None, linkVelocity=None, link_ids=None, kd=60):
-        """
+    def get_active_compliant_torques(self, q=None, dq=None, q_idx=None, jacobian=None, link_velocity=None,
+                                     link_ids=None, kd=60):
+        r"""
         Return the torques that need to be applied to enable active compliance. This is done by enabling Coriolis
         and gravity compensation along with a damping force projected from the Cartesian space to the joint space.
 
@@ -2494,15 +2426,15 @@ class Robot(ControllableBody):
         where :math:`F = - D v` with :math:`v` are the Cartesian velocities, and :math:`D` is the damping factor.
 
         Args:
-            q (float[N], None): all the joint positions. If None, it will get the current joint positions of all the
+            q (np.array[N], None): all the joint positions. If None, it will get the current joint positions of all the
                 joints. However, note that if you already got the joint positions in your code,
                 it is better to pass them to this method for performance.
-            dq (float[N], None): all the joint velocities. If None, it will get the current joint velocities of
+            dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
                 all the joints.
-            qIdx (int[M], None): slice the torques at the given q indices (0 < M <= N).
+            q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
 
         Returns:
-            float[M]: joint torques to be applied [Nm]
+            np.array[M]: joint torques to be applied [Nm]
         """
         if q is None:
             q = self.get_joint_positions()
@@ -2510,47 +2442,47 @@ class Robot(ControllableBody):
             dq = self.get_joint_velocities()
         if jacobian is None:
             jacobian = self.get_jacobian(link_id, q)
-        if linkVelocity is None:
-            linkVelocity = self.get_link_world_velocities(link_id)
+        if link_velocity is None:
+            link_velocity = self.get_link_world_velocities(link_id)
         if isinstance(kd, int):
             kd = kd * np.identity(6)
 
-        torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, qIdx)
-        torques += jacobian.T.dot(-kd * linkVelocity)
+        torques = self.get_coriolis_and_gravity_compensation_torques(q, dq, q_idx)
+        torques += jacobian.T.dot(-kd * link_velocity)
         return torques
 
     # TODO: finish to implement the method
-    def apply_active_compliance(self, q=None, dq=None, qIdx=None, external_torques=0.):
-        """
+    def apply_active_compliance(self, q=None, dq=None, q_idx=None, external_torques=0.):
+        r"""
         Apply active compliance; this is done by enabling Coriolis and gravity compensation along with a damping
         force projected from the Cartesian space to the joint space.
 
         Args:
-            q (float[N], None): all the joint positions. If None, it will get the current joint positions of all the
+            q (np.array[N], None): all the joint positions. If None, it will get the current joint positions of all the
                 joints. However, note that if you already got the joint positions in your code,
                 it is better to pass them to this method for performance.
-            dq (float[N], None): all the joint velocities. If None, it will get the current joint velocities of
+            dq (np.array[N], None): all the joint velocities. If None, it will get the current joint velocities of
                 all the joints.
-            qIdx (int[M], None): slice the torques at the given q indices (0 < M <= N).
+            q_idx (int[M], None): slice the torques at the given q indices (0 < M <= N).
         """
-        jointId = self.joints if qIdx is None else self.joints[qIdx]
-        torques = self.get_active_compliant_torques(q, dq, qIdx)
-        self.set_joint_torques(jointId, torques + external_torques)
+        joint_id = self.joints if q_idx is None else self.joints[q_idx]
+        torques = self.get_active_compliant_torques(q, dq, q_idx)
+        self.set_joint_torques(joint_id, torques + external_torques)
 
     def get_impedance_torques(self, x=0, dx=0, ddx=0):
-        """
+        r"""
         .. math:: F_{a} = H_m (\ddot{x} - \ddot{x}_d) + D_m (\dot{x} - \dot{x}_d) + K_m (x - x_d)
         """
         pass
 
     def apply_task_impedance_control(self):
-        """
+        r"""
         .. math:: F_{a} = H_m (\ddot{x} - \ddot{x}_d) + D_m (\dot{x} - \dot{x}_d) + K_m (x - x_d)
         """
         pass
 
     def get_attractor_torques(self):
-        """
+        r"""
         The torques to be applied are given by:
 
         .. math::  \tau = C(q,\dot{q}) \dot{q} + g(q) + J^T F
@@ -2565,7 +2497,7 @@ class Robot(ControllableBody):
     ######################
 
     def get_symbolic_equations_of_motion(self):
-        """
+        r"""
         This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the
         `sympy.mechanics` module.
 
@@ -2579,7 +2511,7 @@ class Robot(ControllableBody):
         pass
 
     def linearize_equations_of_motion(self, point=None):
-        """
+        r"""
         Linearize the equation of motions around the given point. That is, instead of having :math:`\dot{x} = f(x,u)`
         where :math:`f` is in general a non-linear function, linearize it around a certain point.
 
@@ -2592,8 +2524,8 @@ class Robot(ControllableBody):
             point:
 
         Returns:
-            float[M,M]: :math:`A` matrix, where M is the size of the state vector
-            float[M,N]: :math:`B` matrix, where N is the size of the input vector
+            np.array[M,M]: :math:`A` matrix, where M is the size of the state vector
+            np.array[M,N]: :math:`B` matrix, where N is the size of the input vector
 
         References:
             [1] "State-Space Representation of LTI Systems", Rowell, 2002 (handout):
@@ -2712,37 +2644,12 @@ class Robot(ControllableBody):
             return self.actuators
         return self.actuators[idx]
 
-    #######################
-    # Contacts/Collisions #
-    #######################
-
-    def get_contacts(self):
-        """
-        Return all the contacts made by the robot.
-
-        Warnings: note that in reality, you can't know if your link(s) is/are in contact with an object unless there
-        is a sensor attached to it. However, this can be useful in simulation to optimize, for instance, trajectories.
-
-        Returns:
-            list: list of contact points where each contact point has:
-                int: contact flag
-                int: unique id of body A (this should be the robot id)
-                int: unique id of body B
-                int: link index of body A (-1 for base, this should be the same as the given link)
-                int: link index of body B (-1 for base)
-                float[3]: contact position on A (in Cartesian world coordinates)
-                float[3]: contact position on B (in Cartesian world coordinates)
-                float[3]: contact normal on B pointing towards A
-                float: contact distance (positive for separation and negative for penetration)
-                float: normal force applied during the last simulation step
-        """
-        return self.sim.get_contact_points(body1=self.id)
-
     #########
     # Debug #
     #########
 
-    def _get_joint_type_str(self, idx):
+    @staticmethod
+    def _get_joint_type_str(idx):
         """
         Return the joint type as a string based on the flag.
 
@@ -3018,10 +2925,10 @@ class Robot(ControllableBody):
 
         Args:
             radius (float): radius of the sphere representing the CoM of the robot.
-            color (float[4]): rgba color of the sphere. By default, it is red.
+            color (tuple/list of 4 float): rgba color of the sphere. By default, it is red.
 
         Returns:
-            float[3]: center of mass
+            np.array[3]: center of mass
         """
         self.get_center_of_mass_position()
         self.draw_com_position(radius=radius, color=color)
@@ -3036,7 +2943,7 @@ class Robot(ControllableBody):
 
         Args:
             radius (float): radius of the sphere representing the CoM of the robot
-            color (float[4]): rgba color of the sphere. By default it is red.
+            color (tuple/list of 4 float): rgba color of the sphere. By default it is red.
         """
         if self.com_visual is None:  # create visual shape if not already created
             com_visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color)
@@ -3062,7 +2969,7 @@ class Robot(ControllableBody):
             max_depth (float): if there is an object more than max_depth, it is not considered
 
         Returns:
-            float[3], None: position of the projected CoM, or None if it couldn't project the CoM
+            np.array[3], None: position of the projected CoM, or None if it couldn't project the CoM
         """
         com = self.get_center_of_mass_position()
         object_id, _, _, hit_position, _ = self.sim.ray_test(com, com - np.array([0., 0., max_depth]))[0]
@@ -3077,10 +2984,10 @@ class Robot(ControllableBody):
 
         Args:
             radius (float): radius of the sphere representing the CoM of the robot
-            color (float[4]): rgba color of the sphere. By default it is blue.
+            color (tuple/list of 4 float): rgba color of the sphere. By default it is blue.
 
         Returns:
-            float[3], None: position of the projected CoM, or None if it couldn't project the CoM
+            np.array[3], None: position of the projected CoM, or None if it couldn't project the CoM
         """
         projected_com = self.get_projected_com_position()
         if projected_com is not None:
@@ -3192,10 +3099,10 @@ class Robot(ControllableBody):
         Warnings: Currently, PyBullet doesn't support to load an ellipsoid, so we load from a mesh file.
 
         Args:
-            position (float[3]): position in the world space
-            orientation (float[4]): orientation in the world space
-            scale (float[3]): scale in the (x,y,z) directions
-            color (float[4]): RGBA color
+            position (np.array[3]): position in the world space
+            orientation (np.array[4]): orientation in the world space
+            scale (list/tuple of 3 float): scale in the (x,y,z) directions
+            color (list/tuple of 4 float): RGBA color
 
         Returns:
             int: id of the ellipsoid
@@ -3223,7 +3130,7 @@ class Robot(ControllableBody):
 
         # evals, evecs = np.linalg.eigh(X)
         # evals, evecs = evals[::-1], evecs[:,::-1]
-        # #S, orientation = np.sqrt(evals), self.angular_converter.convert_from(quaternion.from_rotation_matrix(evecs.T))
+        # S, orientation = np.sqrt(evals), self.angular_converter.convert_from(quaternion.from_rotation_matrix(evecs.T))
         #
         # print(V[0])
         # print(V[1])
@@ -3242,14 +3149,14 @@ class Robot(ControllableBody):
         return orientation, scale
 
     def draw_velocity_manipulability_ellipsoid(self, link_id, Jlin=None, JJT=None, color=(0, 1, 0, 0.7)):
-        """
+        r"""
         evecs of JJ^T = directions
         singular values of JJ^T = dimensions
 
         Args:
             link_id (int): link id. This will be used to check where to draw the ellipsoid.
-            J (float[3,N], None): linear Jacobian matrix. It doesn't need to be provided if `JJT` is given.
-            JJT (float[3,3], None): if None, it will compute it using the provided linear Jacobian matrix.
+            J (np.array[3,N], None): linear Jacobian matrix. It doesn't need to be provided if `JJT` is given.
+            JJT (np.array[3,3], None): if None, it will compute it using the provided linear Jacobian matrix.
 
         Returns:
             int: id of the visual ellipsoid
@@ -3266,11 +3173,11 @@ class Robot(ControllableBody):
         self.draw3d_ellipsoid(position, orientation, scale=scale, color=color)
 
     def draw_force_manipulability_ellipsoid(self, link_id, J=None, JJT=None):
-        """
+        r"""
         Kineto-statics duality: direction with good velocity manipulability is obtained a direction along which poor
         force manipulability is obtained.
 
-        evecs((JJ^T)^{-1})
+        ..math:: evecs((JJ^T)^{-1})
 
         Args:
             link_id:
diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py
index a720af9..599c657 100644
--- a/pyrobolearn/simulators/bullet.py
+++ b/pyrobolearn/simulators/bullet.py
@@ -117,7 +117,8 @@ class Bullet(Simulator):
         # The items in the history container are in the same order there were called. Each item is a tuple where the
         # first item is the name of the method called, and the second item is the parameters that were passed to that
         # method.
-        self.history = []
+        self.history = []  # keep track of every commands
+        self.ids = []  # keep track of created unique ids
 
         # main camera in the simulator
         self._camera = None
@@ -160,16 +161,6 @@ class Bullet(Simulator):
         """Return the version of the simulator in a year-month-day format."""
         return self.sim.getAPIVersion()
 
-    @property
-    def gravity(self):
-        """Return the gravity in the simulator."""
-        return self.get_physics_properties()['gravity']
-
-    @property
-    def camera(self):
-        """Return the camera (yaw, pitch, distance, target_position) or None."""
-        return self._camera
-
     #############
     # Operators #
     #############
@@ -206,6 +197,10 @@ class Bullet(Simulator):
         Returns:
             Bullet: bullet simulator in DIRECT mode.
         """
+        # if the object has already been copied return the reference to the copied object
+        if self in memo:
+            return memo[self]
+
         # check if the memo has arguments that specify how to deep copy the simulator
         copy_models = memo.get('copy_parameters', False)
         copy_properties = memo.get('copy_properties', False)
@@ -221,10 +216,8 @@ class Bullet(Simulator):
         if copy_properties:
             pass
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
+        # update the memodict
         memo[self] = sim
-
         return sim
 
     ###########
@@ -408,7 +401,7 @@ class Bullet(Simulator):
         Args:
             time_step (float): Each time you call 'step' the time step will proceed with 'time_step'.
         """
-        self.history.append(('set_time_step', {'time_step': time_step}))
+        # self.history.append(('set_time_step', {'time_step': time_step}))
         self.sim.setTimeStep(timeStep=time_step)
 
     def set_real_time(self, enable=True):
@@ -606,6 +599,10 @@ class Bullet(Simulator):
         """
         self.sim.stopStateLogging(logger_id)
 
+    def get_gravity(self):
+        """Return the gravity set in the simulator."""
+        return self.get_physics_properties()['gravity']
+
     def set_gravity(self, gravity=(0, 0, -9.81)):
         """Set the gravity in the simulator with the given acceleration.
 
@@ -754,10 +751,10 @@ class Bullet(Simulator):
             kwargs['globalScaling'] = scale
 
         model_id = self.sim.loadURDF(filename, **kwargs)
-        if model_id > -1:
-            frame = inspect.currentframe()
-            args, _, _, values = inspect.getargvalues(frame)
-            self.history.append(('load_urdf', {arg: values[arg] for arg in args[1:]}))
+        # if model_id > -1:
+        #     frame = inspect.currentframe()
+        #     args, _, _, values = inspect.getargvalues(frame)
+        #     self.history.append(('load_urdf', {arg: values[arg] for arg in args[1:]}))
         return model_id
 
     def load_sdf(self, filename, scaling=1., *args, **kwargs):
@@ -1366,8 +1363,7 @@ class Bullet(Simulator):
         """
         self.sim.resetBaseVelocity(body_id, angularVelocity=angular_velocity)
 
-    def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=(0., 0., 0.),
-                             frame=pybullet.LINK_FRAME):
+    def apply_external_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=None, frame=Simulator.LINK_FRAME):
         """
         Apply the specified external force on the specified position on the body / link.
 
@@ -1380,14 +1376,22 @@ class Bullet(Simulator):
             body_id (int): unique body id.
             link_id (int): unique link id. If -1, it will be the base.
             force (np.float[3]): external force to be applied.
-            position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate
-                systems.
+            position (np.float[3], None): position on the link where the force is applied. See `flags` for coordinate
+                systems. If None, it is the center of mass of the body (or the link if specified).
             frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
                 Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
+        if position is None:
+            if frame == Simulator.WORLD_FRAME:  # world frame
+                if link_id == -1:
+                    position = self.get_base_pose(body_id)[0]
+                else:
+                    position = self.get_link_state(body_id, link_id)[0]
+            else:  # local frame
+                position = (0., 0., 0.)
         self.sim.applyExternalForce(body_id, link_id, force, position, frame)
 
-    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)):
+    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=Simulator.LINK_FRAME):
         """
         Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external
         torques are cleared to 0.
@@ -1398,6 +1402,8 @@ class Bullet(Simulator):
             body_id (int): unique body id.
             link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
             torque (float[3]): Cartesian torques to be applied on the body
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+                Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
         self.sim.applyExternalTorque(body_id, link_id, torque)
 
@@ -2912,27 +2918,33 @@ class Bullet(Simulator):
 
     def get_collision_shape_data(self, object_id, link_id=-1):
         """
-        Get the collision shape data associated with the specified object id and link id.
+        Get the collision shape data associated with the specified object id and link id. If the given object_id has
+        no collision shape, it returns an empty tuple.
 
         Args:
             object_id (int): object unique id.
             link_id (int): link index or -1 for the base.
 
         Returns:
-            int: object unique id.
-            int: link id.
-            int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
-            np.float[3]: depends on geometry type:
-                for GEOM_BOX: extents,
-                for GEOM_SPHERE: dimensions[0] = radius,
-                for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
-                For GEOM_MESH: dimensions is the scaling factor.
-            str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
-            np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
-            np.float[4]: Local orientation of the collision frame with respect to the inertial frame
+            if not has_collision_shape_data:
+                tuple: empty tuple
+            else:
+                int: object unique id.
+                int: link id.
+                int: geometry type; GEOM_BOX (=3), GEOM_SPHERE (=2), GEOM_CAPSULE (=7), GEOM_MESH (=5), GEOM_PLANE (=6)
+                np.float[3]: depends on geometry type:
+                    for GEOM_BOX: extents,
+                    for GEOM_SPHERE: dimensions[0] = radius,
+                    for GEOM_CAPSULE and GEOM_CYLINDER: dimensions[0] = height (length), dimensions[1] = radius.
+                    For GEOM_MESH: dimensions is the scaling factor.
+                str: Only for GEOM_MESH: file name (and path) of the collision mesh asset.
+                np.float[3]: Local position of the collision frame with respect to the center of mass/inertial frame
+                np.float[4]: Local orientation of the collision frame with respect to the inertial frame
         """
-        object_id, link_id, geom_type, dimensions, filename, \
-            position, orientation = self.sim.getCollisionShapeData(object_id, link_id)
+        collision = self.sim.getCollisionShapeData(object_id, link_id)
+        if len(collision) == 0:
+            return collision
+        object_id, link_id, geom_type, dimensions, filename, position, orientation = collision
         return object_id, link_id, geom_type, np.array(dimensions), filename, np.array(position), np.array(orientation)
 
     def get_overlapping_objects(self, aabb_min, aabb_max):
@@ -3047,7 +3059,7 @@ class Bullet(Simulator):
         if link2_id is not None:
             kwargs['linkIndexB'] = link2_id
 
-        results = self.sim.getContactPoints(body1, body2, distance, **kwargs)
+        results = self.sim.getClosestPoints(body1, body2, distance, **kwargs)
         if len(results) == 0:
             return results
         return [[r[0], r[1], r[2], r[3], r[4], np.array(r[5]), np.array(r[6]), np.array(r[7]), r[8], r[9], r[10],
@@ -3156,7 +3168,7 @@ class Bullet(Simulator):
 
         Returns:
             float: mass in kg
-            float: friction coefficient
+            float: lateral friction coefficient
             np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
                 aligned with the principal axes of inertia.
             np.float[3]: position of inertial frame in local coordinates of the joint frame
@@ -3177,7 +3189,7 @@ class Bullet(Simulator):
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
                         local_inertia_diagonal=None, joint_damping=None):
         """
-        Change dynamic properties such as mass, friction and restitution coefficients .
+        Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
         Args:
             body_id (int): object unique id, as returned by `load_urdf`, etc.
diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py
index 5978406..ec17b1c 100644
--- a/pyrobolearn/simulators/simulator.py
+++ b/pyrobolearn/simulators/simulator.py
@@ -45,147 +45,151 @@ class Simulator(object):
     [2] PEP8: https://www.python.org/dev/peps/pep-0008/
     """
 
+    # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators...
+
+    B3G_ALT = 65308
+    B3G_BACKSPACE = 65305
+    B3G_CONTROL = 65307
+    B3G_DELETE = 65304
+    B3G_DOWN_ARROW = 65298
+    B3G_END = 65301
+    B3G_F1 = 65280
+    B3G_F10 = 65289
+    B3G_F11 = 65290
+    B3G_F12 = 65291
+    B3G_F13 = 65292
+    B3G_F14 = 65293
+    B3G_F15 = 65294
+    B3G_F2 = 65281
+    B3G_F3 = 65282
+    B3G_F4 = 65283
+    B3G_F5 = 65284
+    B3G_F6 = 65285
+    B3G_F7 = 65286
+    B3G_F8 = 65287
+    B3G_F9 = 65288
+    B3G_HOME = 65302
+    B3G_INSERT = 65303
+    B3G_LEFT_ARROW = 65295
+    B3G_PAGE_DOWN = 65300
+    B3G_PAGE_UP = 65299
+    B3G_RETURN = 65309
+    B3G_RIGHT_ARROW = 65296
+    B3G_SHIFT = 65306
+    B3G_UP_ARROW = 65297
+
+    COV_ENABLE_DEPTH_BUFFER_PREVIEW = 14
+    COV_ENABLE_GUI = 1
+    COV_ENABLE_KEYBOARD_SHORTCUTS = 9
+    COV_ENABLE_MOUSE_PICKING = 10
+    COV_ENABLE_PLANAR_REFLECTION = 16
+    COV_ENABLE_RENDERING = 7
+    COV_ENABLE_RGB_BUFFER_PREVIEW = 13
+    COV_ENABLE_SEGMENTATION_MARK_PREVIEW = 15
+    COV_ENABLE_SHADOWS = 2
+    COV_ENABLE_SINGLE_STEP_RENDERING = 17
+    COV_ENABLE_TINY_RENDERER = 12
+    COV_ENABLE_WIREFRAME = 3
+    COV_ENABLE_Y_AXIS_UP = 11
+
+    DIRECT = 2
+    ER_BULLET_HARDWARE_OPENGL = 131072
+    ER_NO_SEGMENTATION_MASK = 4
+    ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1
+    ER_TINY_RENDERER = 65536
+    ER_USE_PROJECTIVE_TEXTURE = 2
+
+    GEOM_FORCE_CONCAVE_TRIMESH = 1
+    GEOM_SPHERE = 2
+    GEOM_CONCAVE_INTERNAL_EDGE = 2
+    GEOM_BOX = 3
+    GEOM_CYLINDER = 4
+    GEOM_MESH = 5
+    GEOM_PLANE = 6
+    GEOM_CAPSULE = 7
+
+    GUI = 1
+    GUI_MAIN_THREAD = 8
+    GUI_SERVER = 7
+    IK_DLS = 0
+    IK_HAS_JOINT_DAMPING = 128
+    IK_HAS_NULL_SPACE_VELOCITY = 64
+    IK_HAS_TARGET_ORIENTATION = 32
+    IK_HAS_TARGET_POSITION = 16
+    IK_SDLS = 1
+
+    JOINT_FEEDBACK_IN_JOINT_FRAME = 2
+    JOINT_FEEDBACK_IN_WORLD_SPACE = 1
+    JOINT_FIXED = 4
+    JOINT_GEAR = 6
+    JOINT_PLANAR = 3
+    JOINT_POINT2POINT = 5
+    JOINT_PRISMATIC = 1
+    JOINT_REVOLUTE = 0
+    JOINT_SPHERICAL = 2
+
+    KEY_IS_DOWN = 1
+    KEY_WAS_RELEASED = 4
+    KEY_WAS_TRIGGERED = 2
+
+    LINK_FRAME = 1
+    WORLD_FRAME = 2
+
+    MAX_RAY_INTERSECTION_BATCH_SIZE = 16384
+
+    VELOCITY_CONTROL = 0
+    TORQUE_CONTROL = 1
+    POSITION_CONTROL = 2
+    PD_CONTROL = 3
+
+    SENSOR_FORCE_TORQUE = 1
+    SHARED_MEMORY = 3
+    SHARED_MEMORY_KEY = 12347
+    SHARED_MEMORY_KEY2 = 12348
+    SHARED_MEMORY_SERVER = 9
+    STATE_LOGGING_ALL_COMMANDS = 7
+    STATE_LOGGING_CONTACT_POINTS = 5
+    STATE_LOGGING_CUSTOM_TIMER = 9
+    STATE_LOGGING_GENERIC_ROBOT = 1
+    STATE_LOGGING_MINITAUR = 0
+    STATE_LOGGING_PROFILE_TIMINGS = 6
+    STATE_LOGGING_VIDEO_MP4 = 3
+    STATE_LOGGING_VR_CONTROLLERS = 2
+    STATE_LOG_JOINT_MOTOR_TORQUES = 1
+    STATE_LOG_JOINT_TORQUES = 3
+    STATE_LOG_JOINT_USER_TORQUES = 2
+    STATE_REPLAY_ALL_COMMANDS = 8
+
+    TCP = 5
+    UDP = 4
+
+    URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024
+    URDF_ENABLE_SLEEPING = 2048
+    URDF_GLOBAL_VELOCITIES_MB = 256
+    URDF_INITIALIZE_SAT_FEATURES = 4096
+    URDF_USE_IMPLICIT_CYLINDER = 128
+    URDF_USE_INERTIA_FROM_FILE = 2
+    URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768
+    URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536
+    URDF_USE_SELF_COLLISION = 8
+    URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32
+    URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16
+    URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192
+
     def __init__(self, render=True, **kwargs):
         self._render = render
         self.real_time = False
         self.kwargs = kwargs
 
-        # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators...
+        # main camera in the simulator
+        self._camera = None
 
+        # TODO: this is really bad to have attributes like that... It doesn't generalize well to other simulators...
         # import pybullet
         # for attribute in dir(pybullet):
         #     if attribute[0].isupper():
         #         print('self.{} = {}'.format(attribute, getattr(pybullet, attribute)))
 
-        self.B3G_ALT = 65308
-        self.B3G_BACKSPACE = 65305
-        self.B3G_CONTROL = 65307
-        self.B3G_DELETE = 65304
-        self.B3G_DOWN_ARROW = 65298
-        self.B3G_END = 65301
-        self.B3G_F1 = 65280
-        self.B3G_F10 = 65289
-        self.B3G_F11 = 65290
-        self.B3G_F12 = 65291
-        self.B3G_F13 = 65292
-        self.B3G_F14 = 65293
-        self.B3G_F15 = 65294
-        self.B3G_F2 = 65281
-        self.B3G_F3 = 65282
-        self.B3G_F4 = 65283
-        self.B3G_F5 = 65284
-        self.B3G_F6 = 65285
-        self.B3G_F7 = 65286
-        self.B3G_F8 = 65287
-        self.B3G_F9 = 65288
-        self.B3G_HOME = 65302
-        self.B3G_INSERT = 65303
-        self.B3G_LEFT_ARROW = 65295
-        self.B3G_PAGE_DOWN = 65300
-        self.B3G_PAGE_UP = 65299
-        self.B3G_RETURN = 65309
-        self.B3G_RIGHT_ARROW = 65296
-        self.B3G_SHIFT = 65306
-        self.B3G_UP_ARROW = 65297
-
-        self.COV_ENABLE_DEPTH_BUFFER_PREVIEW = 14
-        self.COV_ENABLE_GUI = 1
-        self.COV_ENABLE_KEYBOARD_SHORTCUTS = 9
-        self.COV_ENABLE_MOUSE_PICKING = 10
-        self.COV_ENABLE_PLANAR_REFLECTION = 16
-        self.COV_ENABLE_RENDERING = 7
-        self.COV_ENABLE_RGB_BUFFER_PREVIEW = 13
-        self.COV_ENABLE_SEGMENTATION_MARK_PREVIEW = 15
-        self.COV_ENABLE_SHADOWS = 2
-        self.COV_ENABLE_SINGLE_STEP_RENDERING = 17
-        self.COV_ENABLE_TINY_RENDERER = 12
-        self.COV_ENABLE_WIREFRAME = 3
-        self.COV_ENABLE_Y_AXIS_UP = 11
-
-        self.DIRECT = 2
-        self.ER_BULLET_HARDWARE_OPENGL = 131072
-        self.ER_NO_SEGMENTATION_MASK = 4
-        self.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX = 1
-        self.ER_TINY_RENDERER = 65536
-        self.ER_USE_PROJECTIVE_TEXTURE = 2
-
-        self.GEOM_FORCE_CONCAVE_TRIMESH = 1
-        self.GEOM_SPHERE = 2
-        self.GEOM_CONCAVE_INTERNAL_EDGE = 2
-        self.GEOM_BOX = 3
-        self.GEOM_CYLINDER = 4
-        self.GEOM_MESH = 5
-        self.GEOM_PLANE = 6
-        self.GEOM_CAPSULE = 7
-
-        self.GUI = 1
-        self.GUI_MAIN_THREAD = 8
-        self.GUI_SERVER = 7
-        self.IK_DLS = 0
-        self.IK_HAS_JOINT_DAMPING = 128
-        self.IK_HAS_NULL_SPACE_VELOCITY = 64
-        self.IK_HAS_TARGET_ORIENTATION = 32
-        self.IK_HAS_TARGET_POSITION = 16
-        self.IK_SDLS = 1
-
-        self.JOINT_FEEDBACK_IN_JOINT_FRAME = 2
-        self.JOINT_FEEDBACK_IN_WORLD_SPACE = 1
-        self.JOINT_FIXED = 4
-        self.JOINT_GEAR = 6
-        self.JOINT_PLANAR = 3
-        self.JOINT_POINT2POINT = 5
-        self.JOINT_PRISMATIC = 1
-        self.JOINT_REVOLUTE = 0
-        self.JOINT_SPHERICAL = 2
-
-        self.KEY_IS_DOWN = 1
-        self.KEY_WAS_RELEASED = 4
-        self.KEY_WAS_TRIGGERED = 2
-
-        self.LINK_FRAME = 1
-        self.WORLD_FRAME = 2
-
-        self.MAX_RAY_INTERSECTION_BATCH_SIZE = 16384
-
-        self.VELOCITY_CONTROL = 0
-        self.TORQUE_CONTROL = 1
-        self.POSITION_CONTROL = 2
-        self.PD_CONTROL = 3
-
-        self.SENSOR_FORCE_TORQUE = 1
-        self.SHARED_MEMORY = 3
-        self.SHARED_MEMORY_KEY = 12347
-        self.SHARED_MEMORY_KEY2 = 12348
-        self.SHARED_MEMORY_SERVER = 9
-        self.STATE_LOGGING_ALL_COMMANDS = 7
-        self.STATE_LOGGING_CONTACT_POINTS = 5
-        self.STATE_LOGGING_CUSTOM_TIMER = 9
-        self.STATE_LOGGING_GENERIC_ROBOT = 1
-        self.STATE_LOGGING_MINITAUR = 0
-        self.STATE_LOGGING_PROFILE_TIMINGS = 6
-        self.STATE_LOGGING_VIDEO_MP4 = 3
-        self.STATE_LOGGING_VR_CONTROLLERS = 2
-        self.STATE_LOG_JOINT_MOTOR_TORQUES = 1
-        self.STATE_LOG_JOINT_TORQUES = 3
-        self.STATE_LOG_JOINT_USER_TORQUES = 2
-        self.STATE_REPLAY_ALL_COMMANDS = 8
-
-        self.TCP = 5
-        self.UDP = 4
-
-        self.URDF_ENABLE_CACHED_GRAPHICS_SHAPES = 1024
-        self.URDF_ENABLE_SLEEPING = 2048
-        self.URDF_GLOBAL_VELOCITIES_MB = 256
-        self.URDF_INITIALIZE_SAT_FEATURES = 4096
-        self.URDF_USE_IMPLICIT_CYLINDER = 128
-        self.URDF_USE_INERTIA_FROM_FILE = 2
-        self.URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768
-        self.URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536
-        self.URDF_USE_SELF_COLLISION = 8
-        self.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32
-        self.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16
-        self.URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192
-
     ##############
     # Properties #
     ##############
@@ -195,6 +199,21 @@ class Simulator(object):
         """Return the version of the simulator."""
         return 0
 
+    @property
+    def gravity(self):
+        """Return the gravity in the simulator."""
+        return self.get_gravity()
+
+    @gravity.setter
+    def gravity(self, gravity):
+        """Set the gravity in the simulator."""
+        self.set_gravity(gravity)
+
+    @property
+    def camera(self):
+        """Return the camera (yaw, pitch, distance, target_position) or None."""
+        return self._camera
+
     #############
     # Operators #
     #############
@@ -217,14 +236,14 @@ class Simulator(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass.
         """
+        # if the object has already been copied return the reference to the copied object
+        if self in memo:
+            return memo[self]
+
         # create a new copy of the simulator
         sim = self.__class__(render=self._render, **self.kwargs)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = sim
-
-        # return the copy
         return sim
 
     ###########
@@ -318,8 +337,16 @@ class Simulator(object):
         """Stop the logging."""
         pass
 
+    def get_gravity(self):
+        """Return the gravity set in the simulator."""
+        pass
+
     def set_gravity(self, gravity=(0, 0, -9.81)):
-        """Set the gravity in the simulator."""
+        """Set the gravity in the simulator with the given acceleration.
+
+        Args:
+            gravity (list, tuple of 3 floats): acceleration in the x, y, z directions.
+        """
         pass
 
     def save(self, filename=None, *args, **kwargs):
@@ -835,13 +862,13 @@ class Simulator(object):
             link_id (int): unique link id. If -1, it will be the base.
             force (np.float[3]): external force to be applied.
             position (np.float[3]): position on the link where the force is applied. See `flags` for coordinate
-                systems.
+                systems. If None, it is the center of mass of the body (or the link if specified).
             frame (int): if frame = 1, then the force / position is described in the link frame. If frame = 2, they
                 are described in the world frame.
         """
         pass
 
-    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.)):
+    def apply_external_torque(self, body_id, link_id=-1, torque=(0., 0., 0.), frame=1):
         """
         Apply an external torque on a body, or a link of the body. Note that after each simulation step, the external
         torques are cleared to 0.
@@ -850,6 +877,8 @@ class Simulator(object):
             body_id (int): unique body id.
             link_id (int): link id to apply the torque, if -1 it will apply the torque on the base
             torque (float[3]): Cartesian torques to be applied on the body
+            frame (int): Specify the coordinate system of force/position: either `pybullet.WORLD_FRAME` (=2) for
+                Cartesian world coordinates or `pybullet.LINK_FRAME` (=1) for local link coordinates.
         """
         pass
 
@@ -2008,7 +2037,7 @@ class Simulator(object):
 
         Returns:
             float: mass in kg
-            float: friction coefficient
+            float: lateral friction coefficient
             np.float[3]: local inertia diagonal. Note that links and base are centered around the center of mass and
                 aligned with the principal axes of inertia.
             np.float[3]: position of inertial frame in local coordinates of the joint frame
@@ -2026,7 +2055,7 @@ class Simulator(object):
                         contact_stiffness=None, contact_damping=None, friction_anchor=None,
                         local_inertia_diagonal=None, joint_damping=None):
         """
-        Change dynamic properties such as mass, friction and restitution coefficients .
+        Change dynamic properties of the given body (or link) such as mass, friction and restitution coefficients, etc.
 
         Args:
             body_id (int): object unique id, as returned by `load_urdf`, etc.
diff --git a/pyrobolearn/states/state.py b/pyrobolearn/states/state.py
index b7730a4..390c866 100644
--- a/pyrobolearn/states/state.py
+++ b/pyrobolearn/states/state.py
@@ -1076,16 +1076,16 @@ class State(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
         states = [copy.deepcopy(state, memo) for state in self.states]
         data = copy.deepcopy(self.window[0]) if self.has_data() else None
         space = copy.deepcopy(self._space)
         state = self.__class__(states=states, data=data, space=space, name=self.name, window_size=self.window_size,
                                axis=self.axis, ticks=self.ticks)
 
-        # update the memodict (note that `copy.deepcopy` will automatically check this dictionary and return the
-        # reference if already present)
         memo[self] = state
-
         return state
 
     # def __invert__(self):
diff --git a/pyrobolearn/utils/transformation.py b/pyrobolearn/utils/transformation.py
index 1a62923..3f6dc86 100644
--- a/pyrobolearn/utils/transformation.py
+++ b/pyrobolearn/utils/transformation.py
@@ -24,6 +24,42 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+def get_homogeneous_transform(position, orientation):
+    r"""
+    Return the Homogeneous transform matrix given the position vector and the orientation.
+
+    .. math::
+
+        H = [[R, p],
+             [zeros(3),1]]
+
+    where :math:`R` is the 3x3 rotation matrix, :math:`p` is the 3x1 position vector.
+
+    Args:
+        position (np.array[3]): position vector
+        orientation (np.array[4], np.array[3,3], np.array[3]): orientation (expressed as a quaternion [x,y,z,w],
+            3x3 rotation matrix, or roll-pitch-yaw angles).
+
+    Returns:
+        np.array[4,4]: homogeneous matrix
+    """
+    if isinstance(orientation, quaternion.quaternion):
+        R = quaternion.as_rotation_matrix(orientation)
+    else:
+        orientation = np.array(orientation)
+        if orientation.shape == (3,):  # RPY Euler angles
+            R = get_matrix_from_rpy(orientation)
+        elif orientation.shape == (4,):  # quaternion in the form [x,y,z,w]
+            R = get_matrix_from_quaternion(orientation)
+        elif orientation.shape == (3, 3):  # Rotation matrix
+            R = orientation
+        else:
+            raise ValueError("Expecting a quaternion, RPY Euler angles, or rotation matrix")
+
+    H = np.vstack((np.hstack((R, position.reshape(-1, 1))), np.array([[0, 0, 0, 1]])))
+    return H
+
+
 def get_matrix_from_axis_angle(axis, angle):
     """Return the rotation matrix from the specified axis and angle.
 
diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py
index 32a0767..f521a5c 100644
--- a/pyrobolearn/worlds/world.py
+++ b/pyrobolearn/worlds/world.py
@@ -17,7 +17,7 @@ import cv2
 
 from pyrobolearn.simulators import Simulator
 from pyrobolearn.worlds.world_camera import WorldCamera
-from pyrobolearn.utils import has_method, has_variable
+# from pyrobolearn.utils import has_method, has_variable
 from pyrobolearn.robots import Body, Robot, robot_names_to_classes
 # TODO: to install the `gdal` library, run the script `pyrobolearn/scripts/install_gdal.sh`, by default do not
 #  import it
@@ -40,13 +40,13 @@ __status__ = "Development"
 class World(object):
     r"""World class.
 
-    The world contains all the objects that constitutes the world. This includes immovable objects such as the
-    terrain/floor, walls, and so on, as well as movable objects such as the various robots, etc.
+    The world contains all the objects that constitutes the world. This includes immovable bodies such as the
+    terrain/floor, walls, and so on, as well as movable bodies such as the various robots, etc.
     Properties of the world are also defined here, such as gravity, friction, and other dynamical properties.
 
-    The world is responsible to load the different objects part of the world and keeping a map of objects;
-    based on where the agent(s) is(are), the objects will be removed or added from/to the simulator allowing it
-    to run faster.
+    The world is responsible to load the different objects part of the world and keeping a map of bodies;
+    based on where the agent(s) is(are), the bodies will be removed or added from/to the simulator allowing it
+    to run faster. (TODO)
 
     It is independent of the simulator and environment used in RL, and is often provided as inputs to some
     `rewards/costs` and to the `environment`.
@@ -62,23 +62,26 @@ class World(object):
     """
 
     def __init__(self, simulator, gravity=(0., 0., -9.81)):
+        """
+        Initialize the world.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            gravity (tuple/list of 3 float, np.array[3]): gravity vector.
+        """
         # set simulator
         self.simulator = simulator
-
-        # By default, set the gravity
         self.gravity = gravity
 
         # set world camera
         self.camera = WorldCamera(self.simulator)
 
-        # keep track of the objects present in the world
-        # TODO: check what is already inside the simulator!
-        self.robots = {}
-        self.movable_bodies = {}  # set()
-        self.immovable_bodies = {}  # set()
-        self.visual_objects = {}  # set()
+        # keep track of the all the unique ids present in the world
+        # the following dictionary contains {id1: [(method_name, args), [parent_ids], [child_ids]], id2: Body}
+        # ids like id1 include visual shapes, collision shapes, textures, bodies that were created here
+        self.ids = collections.OrderedDict()
+        self.bodies = {}  # this contains {id: Body}
 
-        self.visual_shapes = {}
         self.map = None
         self.floor_id = -1
 
@@ -89,8 +92,8 @@ class World(object):
         self.sim.configure_debug_visualizer(self.sim.COV_ENABLE_GUI, 0)
 
         # interfaces and bridges
-        self.interfaces = set([])
-        self.bridges = []
+        # self.interfaces = set([])
+        # self.bridges = []
 
     ##############
     # Properties #
@@ -112,7 +115,7 @@ class World(object):
     @property
     def gravity(self):
         """Return the gravity vector."""
-        return self._gravity
+        return self.simulator.gravity
 
     @gravity.setter
     def gravity(self, gravity):
@@ -121,9 +124,7 @@ class World(object):
         Args:
             gravity (np.float[3]): 3d gravity vector.
         """
-        gravity = np.array(gravity)
-        self.sim.set_gravity(gravity)
-        self._gravity = gravity
+        self.simulator.gravity = gravity
 
     @property
     def lateral_friction(self):
@@ -237,16 +238,14 @@ class World(object):
         Check if the given item is in the world.
 
         Args:
-            item (int, Object, Robot): if it is an integer, it will check if the given object id is in the world.
+            item (int, Body): if it is an integer, it will check if the given body id is in the world.
 
         Returns:
             bool: True if the world contains the given item
         """
-        if not isinstance(item, int):
+        if isinstance(item, Body):
             item = item.id
-
-        return (item in self.robots) or (item in self.movable_bodies) or (item in self.immovable_bodies) or \
-               (item in self.visual_objects)
+        return item in self.bodies
 
     def __copy__(self):  # TODO: add the bodies in the copy
         """Return a shallow copy of the world. This can be overridden in the child class."""
@@ -258,9 +257,25 @@ class World(object):
         Args:
             memo (dict): memo dictionary of objects already copied during the current copying pass
         """
+        if self in memo:
+            return memo[self]
+
+        # copy world
         simulator = copy.deepcopy(self.simulator, memo)
         gravity = copy.deepcopy(self.gravity)
         world = self.__class__(simulator=simulator, gravity=gravity)
+
+        # load bodies in world
+        for id_, items in self.ids.iteritems():
+            if not isinstance(items, list):
+                items = [items]
+            for item in items:
+                if isinstance(item, tuple):  # (method_name, arguments)
+                    method = getattr(world, item[0])
+                    method(**item[1])
+                else:
+                    copy.deepcopy(item, memo)
+
         memo[self] = world
         return world
 
@@ -268,31 +283,49 @@ class World(object):
     # Methods #
     ###########
 
-    def set_bridges(self, bridges):  # TODO: remove this
+    @staticmethod
+    def __get_method_and_parameters(frame):
         """
-        This append the given bridges to various interfaces to the list of bridges.
-
-        See `pyrobolearn.tools.interface` and `pyrobolearn.tools.bridge` for more information.
+        Return the method name and the parameters with their values.
 
         Args:
-            bridges (list, Bridge): list of bridges
+            frame (types.FrameType): frame of the method
+
+        Returns:
+            str: method name.
+            dict: parameters with their values.
         """
-        if isinstance(bridges, collections.Iterable):
-            for bridge in bridges:
-                # if not isinstance(bridge, Bridge):
-                #    raise TypeError("Expecting a list of bridges (must be an instance of Bridge)")
-                if not has_method(bridge, 'step') and not has_variable(bridge, 'interface'):
-                    raise TypeError("Expecting bridge to have a `step` method and an `interface` variable")
-                if not has_method(bridge.interface, 'step'):
-                    raise TypeError("Expecting the bridge.interface to have a `step` method")
-                self.bridges.append(bridge)
-                self.interfaces.add(bridge.interface)
-        # elif isinstance(bridges, Bridge):
-        elif has_method(bridges, 'step') and has_variable(bridges, 'interface') and has_method(bridges.interface, 'step'):
-            self.bridges.append(bridges)
-            self.interfaces.add(bridges.interface)
-        else:
-            raise TypeError("Expecting a bridge (instance of Bridge) or a list of instances of Bridge")
+        args, _, _, values = inspect.getargvalues(frame)
+        method_name = frame.f_code.co_name
+        parameters = {arg: values[arg] for arg in args[1:]}
+        return method_name, parameters
+
+    # def set_bridges(self, bridges):  # TODO: remove this
+    #     """
+    #     This append the given bridges to various interfaces to the list of bridges.
+    #
+    #     See `pyrobolearn.tools.interface` and `pyrobolearn.tools.bridge` for more information.
+    #
+    #     Args:
+    #         bridges (list, Bridge): list of bridges
+    #     """
+    #     if isinstance(bridges, collections.Iterable):
+    #         for bridge in bridges:
+    #             # if not isinstance(bridge, Bridge):
+    #             #    raise TypeError("Expecting a list of bridges (must be an instance of Bridge)")
+    #             if not has_method(bridge, 'step') and not has_variable(bridge, 'interface'):
+    #                 raise TypeError("Expecting bridge to have a `step` method and an `interface` variable")
+    #             if not has_method(bridge.interface, 'step'):
+    #                 raise TypeError("Expecting the bridge.interface to have a `step` method")
+    #             self.bridges.append(bridge)
+    #             self.interfaces.add(bridge.interface)
+    #     # elif isinstance(bridges, Bridge):
+    #     elif has_method(bridges, 'step') and has_variable(bridges, 'interface') and \
+    #             has_method(bridges.interface, 'step'):
+    #         self.bridges.append(bridges)
+    #         self.interfaces.add(bridges.interface)
+    #     else:
+    #         raise TypeError("Expecting a bridge (instance of Bridge) or a list of instances of Bridge")
 
     def save(self, filename=None):
         """
@@ -305,8 +338,6 @@ class World(object):
         Returns:
             str or int: filename, or unique state id.
         """
-        # save approximate world state on the disk
-        # self.sim.saveWorld(filename)
         self.world_state = self.sim.save(filename)
         return self.world_state
 
@@ -350,10 +381,10 @@ class World(object):
         """
         Perform one step for the interfaces and bridges, and one step in the world/simulator.
         """
-        for interface in self.interfaces:
-            interface.step()
-        for bridge in self.bridges:
-            bridge.step()
+        # for interface in self.interfaces:
+        #     interface.step()
+        # for bridge in self.bridges:
+        #     bridge.step()
         self.sim.step()
         if sleep_dt is not None:
             time.sleep(sleep_dt)
@@ -377,7 +408,7 @@ class World(object):
         """
         Load the robot into the world. If the robot parameter is a known robot name or the path to the urdf file,
         it will create a `Robot` instance and return it. If the robot is already an instance of `Robot` it will
-        just add it to the list of objects present in the world.
+        just add it to the list of bodies present in the world.
 
         Args:
             robot (Robot, str, class): the robot instance or name. For the list of possible robot names, import
@@ -420,54 +451,95 @@ class World(object):
             raise TypeError('Unknown type for robot: {}. It must be a string or '
                             'an instance of Robot'.format(type(robot)))
 
-        self.robots[robot.id] = robot
+        self.bodies[robot.id] = robot
+        self.ids[robot.id] = [robot]
         return robot
 
-    def is_robot_id(self, robot_id):
+    def is_body_id(self, body_id):
+        """
+        Check if the given id is a body id.
+
+        Args:
+            body_id (int): the possible body id
+
+        Returns:
+            bool: True if the id is a body id, False otherwise
+        """
+        if body_id in self.bodies:
+            return True  # isinstance(self.bodies[body_id], Body)
+        return False
+
+    def is_robot_id(self, body_id):
         """
         Check if the given id is a robot id.
 
         Args:
-            robot_id (int): the possible robot id
+            body_id (int): the possible robot id
 
         Returns:
             bool: True if the id is a robot id, False otherwise
         """
-        return robot_id in self.robots
+        if body_id in self.bodies:
+            body = self.bodies[body_id]
+            return isinstance(body, Robot)
+        return False
 
-    def get_robot(self, robot_id):
+    def get_body(self, body_id):
         """
-        Return the robot object (instance of Robot) associated to the given robot id.
+        Return the instance (Body, Robot) associated to the given body id.
 
         Args:
-            robot_id (int): unique id of the robot
+            body_id (int): unique body id.
 
         Raises:
-            KeyError: if the given robot id is not in the world.
+            KeyError: if the given body id is not in the world.
 
         Returns:
-            Robot: robot instance
+            Body, Robot, int: Body/Robot instance, or unique id.
         """
-        return self.robots[robot_id]
+        return self.bodies[body_id]
+
+    def wrap(self, body_id, wrapper=Body, *args, **kwargs):
+        """
+        Wrap the given body_id with the provided wrapper. This will replace the
+
+        Args:
+            body_id (int): unique body id.
+            wrapper (class, Body): wrapper class. By default, it will wrap the provided body_id with the `Body` class.
+                The wrapper (its constructor) must at least accepts two parameters: the simulator and the body_id.
+            args (tuple, list): list of arguments that are given to the wrapper class.
+            kwargs (dict): dictionary of arguments that are given to the wrapper class.
+
+        Returns:
+            type(wrapper), Body: instance of the wrapper (by default, it is `Body`)
+        """
+        if body_id not in self.bodies:
+            raise TypeError("Expecting the 'body_id' to be in `self.bodies` (i.e. to have been loaded with one of the "
+                            "methods provided in `World`)")
+        body = wrapper(self.sim, body_id, *args, **kwargs)
+        self.bodies[body_id] = body
+        self.ids[body_id].append(body)
+        return body
 
     def reset_robots(self):
         """
         Reset the base and joint states of each robot
         """
-        for robot_id, robot in self.robots.items():
-            # reset base
-            self.sim.reset_base_pose(robot_id, robot.init_position, robot.init_orientation)
-            self.sim.reset_base_velocity(robot_id, linear_velocity=[0, 0, 0], angular_velocity=[0, 0, 0])
+        for body_id, body in self.bodies.items():
+            if isinstance(body, Robot):
+                # reset base
+                self.sim.reset_base_pose(body_id, body.init_position, body.init_orientation)
+                self.sim.reset_base_velocity(body_id, linear_velocity=[0, 0, 0], angular_velocity=[0, 0, 0])
 
-            # reset joint positions
-            positions = robot.init_joint_positions
-            velocities = np.zeros(len(positions))
-            for joint_id, position, velocity in zip(robot.joints, positions, velocities):
-                self.sim.reset_joint_state(robot_id, joint_id, position, velocity)
+                # reset joint positions
+                positions = body.init_joint_positions
+                velocities = np.zeros(len(positions))
+                for joint_id, position, velocity in zip(body.joints, positions, velocities):
+                    self.sim.reset_joint_state(body_id, joint_id, position, velocity)
 
-    def load_urdf(self, filename, position, orientation=(0, 0, 0, 1), fixed_base=False, scale=1., name=None):
+    def load_urdf(self, filename, position, orientation=(0, 0, 0, 1), fixed_base=False, scale=1.):
         """
-        Load URDF specified by the given path. This is basically a wrapper around the simulator's `load_urdf` method.
+        Load the URDF specified by the given path. This will return the body described in the URDF.
 
         Args:
             filename (str): path to the URDF file
@@ -478,26 +550,28 @@ class World(object):
             name (str, None): name of the object. If None, it will extract it from the URDF.
 
         Returns:
-            int: unique id of the loaded body.
+            int: unique id.
         """
         body = self.sim.load_urdf(filename, position, orientation, use_fixed_base=fixed_base, scale=scale)
-        self.movable_bodies[body] = self.sim.get_body_info(body) if name is None else name
+        self.bodies[body] = body
+        self.ids[body] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return body
 
     def load_sdf(self, filename, scaling=1.):
         """
-        Load the given SDF file; this will thus load all the object described in a SDF file.
+        Load the given SDF file; this will thus load all the bodies described in a SDF file.
 
         Args:
             filename (str): path to the SDF file
             scaling (float): scale factor for the object
 
         Returns:
-            list(int): list of ids
+            list of int: list of unique ids.
         """
         bodies = self.sim.load_sdf(filename, scaling=scaling)
+        self.ids[tuple(bodies)] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         for body in bodies:
-            self.movable_bodies[body] = self.sim.get_body_info(body)
+            self.bodies[body] = body
         return bodies
 
     def load_mjcf(self, filename, scaling=1.):
@@ -509,25 +583,34 @@ class World(object):
             scaling (float): scale factor for the object
 
         Returns:
-            list(int): list of ids
+            list of int: list of bodies
         """
         bodies = self.sim.load_mjcf(filename, scaling=scaling)
+        self.ids[tuple(bodies)] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         for body in bodies:
-            self.movable_bodies[body] = self.sim.get_body_info(body)
+            self.bodies[body] = body
         return bodies
 
-    def _load_sdf_or_urdf(self, path, position, orientation, scaling, objectType=None):
-        extension_name = path.split('.')[-1]
-        if extension_name == 'urdf':
-            object_id = self.sim.load_urdf(path, position, orientation, scale=scaling)
-            self.movable_bodies[object_id] = 'urdf' if objectType is None else objectType
-        elif extension_name == 'sdf':
-            object_id = self.sim.load_sdf(path, scale=scaling)  # list of ids
-            for i in object_id:  # assume for now that the objects are movable...
-                self.movable_bodies[i] = 'sdf' if objectType is None else objectType
-        else:
-            raise ValueError('Extension name of the file is not known; this method only accepts URDF/SDF files.')
-        return object_id
+    def create_body(self, position, visual_shape_id, collision_shape_id=-1, mass=0., orientation=(0., 0., 0., 1.),
+                    *args, **kwargs):
+        """Create a body in the simulator.
+
+        Args:
+            position (np.float[3]): Cartesian world position of the base
+            visual_shape_id (int): unique id from createVisualShape or -1. You can reuse the visual shape (instancing)
+            collision_shape_id (int): unique id from createCollisionShape or -1. You can re-use the collision shape
+                for multiple multibodies (instancing)
+            mass (float): mass of the base, in kg (if using SI units)
+            orientation (np.float[4]): Orientation of base as quaternion [x,y,z,w]
+
+        Returns:
+            int: non-negative unique id or -1 for failure.
+        """
+        body = self.sim.create_body(visual_shape_id=visual_shape_id, collision_shape_id=collision_shape_id, mass=mass,
+                                    position=position, orientation=orientation, *args, **kwargs)
+        self.bodies[body] = body
+        self.ids[body] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
+        return body
 
     def get_available_sdfs(self, fullpath=False):
         """Return the list of available SDFs from the `pybullet_data.getDataPath()` method.
@@ -565,55 +648,22 @@ class World(object):
         """
         return self.sim.get_available_objs(fullpath=fullpath)
 
-    def load_object(self, object_type, path=None, position=(0, 0, 0), orientation=(0, 0, 0, 1), scaling=1.):
-        """
-        Load the specified object. This is a method that allows you to quickly load stuffs however it is less
-        accurate than other methods in this class.
-
-        Args:
-            object_type (str): type of the object (name, 'sphere',
-            path (str): path to the object
-            position (np.float[3]): position of the object in the world frame.
-            orientation (np.float[4]): orientation of the object in the world frame.
-            scaling (float): scaling factor
-
-        Returns:
-            int or int[]: object ids
-        """
-        # check if an object has already been loaded at that place.
-
-        if path is not None:
-            object_id = self._load_sdf_or_urdf(path, position, orientation, scaling=1., objectType=object_type)
-        else:
-            if object_type == 'sphere':
-                object_id = self.load_sphere(position)
-            elif object_type == 'box':
-                object_id = self.load_box(position, orientation)
-            elif object_type == 'cylinder':
-                object_id = self.load_cylinder(position, orientation)
-            elif object_type == 'capsule':
-                object_id = self.load_capsule(position, orientation)
-            else:
-                raise TypeError("Object type not known...")
-
-        return object_id
-
-    def move_object(self, object_id, position=None, orientation=None):
+    def move_object(self, body_id, position=None, orientation=None):
         """
         Move the given object at the specified position and orientation.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
             position (float[3]): new position of the object. If None, it will keep the old position.
             orientation (float[4]): new orientation of the object. If None, it will keep the old orientation.
         """
         if position is None:
-            position = self.sim.get_base_pose(object_id)[0]
+            position = self.sim.get_base_pose(body_id)[0]
         if orientation is None:
-            orientation = self.sim.get_base_pose(object_id)[1]
-        self.sim.reset_base_pose(object_id, position, orientation)
+            orientation = self.sim.get_base_pose(body_id)[1]
+        self.sim.reset_base_pose(body_id, position, orientation)
 
-    def apply_force(self, object_id, link_id=-1, force=(0., 0., 0.), position=None, frame=2):
+    def apply_force(self, body_id, link_id=-1, force=(0., 0., 0.), position=None, frame=2):
         """
         Apply the given force on the specified object or link of the object.
 
@@ -622,206 +672,197 @@ class World(object):
             - this does not work when using `sim.setRealTimeSimulation(1)`.
 
         Args:
-            object_id (int): object id to apply the force on
+            body_id (int): body id to apply the force on
             link_id (int): link id to apply the force, if -1 it will apply the force on the base
-            force (float[3]): Cartesian forces to be applied on the body
-            position (float[3]): position on the link where the force is applied. If None, it is the center of mass
+            force (np.array[3]): Cartesian forces to be applied on the body
+            position (np.array[3]): position on the link where the force is applied. If None, it is the center of mass
                 of the object (or the link if specified)
             frame (int): allows to specify the coordinate system of force/position. sim.LINK_FRAME (=1) for local
                 link frame, and sim.WORLD_FRAME (=2) for world frame. By default, it is the world frame.
         """
-        if position is None:
-            if link_id != -1:
-                position = self.sim.get_base_pose(object_id)[0]
-            else:
-                position = self.sim.get_link_state(object_id, link_id)[0]
-        self.sim.apply_external_force(object_id, link_id, force, position, frame)
+        self.sim.apply_external_force(body_id, link_id, force, position, frame)
 
-    def get_object_color(self, object_id):
+    def get_body_color(self, body_id):
         """
-        Return the RGBA color of the given object.
+        Return the RGBA color of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
             float[4]: RGBA color
         """
-        return self.sim.get_visual_shape_data(object_id)[-1]
+        return self.sim.get_visual_shape_data(body_id)[-1]
 
-    def change_object_color(self, object_id, color, link_id=-1):
+    def change_body_color(self, body_id, color, link_id=-1):
         """
-        Change the color of the given object.
+        Change the color of the given body.
 
         Args:
-            object_id (int): object id
-            color (float[4]): RGBA color
+            body_id (int, Body): body (id)
+            color (float[4]): RGBA color where each channel is between 0 and 1.
             link_id (int): link id
         """
-        self.sim.change_visual_shape(object_id, link_id, rgba_color=color)
+        self.sim.change_visual_shape(body_id, link_id, rgba_color=color)
 
-    def get_object_position(self, object_id):
+    def get_body_position(self, body_id):
         """
-        Return the position of the given object.
+        Return the position of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
-            float[3]: position of the object
+            np.array[3]: position of the body (expressed in the world Cartesian frame)
         """
-        return np.array(self.sim.get_base_pose(object_id)[0])
+        return self.sim.get_base_pose(body_id)[0]
 
-    def get_object_orientation(self, object_id):
+    def get_body_orientation(self, body_id):
         """
-        Return the orientation of the given object.
+        Return the orientation of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
-            float[4]: orientation of the object
+            np.array[4]: orientation of the body (expressed as a quaternion [x,y,z,w]).
         """
-        return np.array(self.sim.get_base_pose(object_id)[1])
+        return self.sim.get_base_pose(body_id)[1]
 
-    def get_object_velocity(self, object_id):
+    def get_body_velocity(self, body_id):
         """
-        Return the linear and angular velocities of the given object.
+        Return the linear and angular velocities of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
-            float[6]: linear and angular velocities of the object
+            np.array[6]: linear and angular velocities of the body (in the world frame)
         """
-        lin_vel, ang_vel = self.sim.get_base_velocity(object_id)
-        return np.array(lin_vel + ang_vel)
+        lin_vel, ang_vel = self.sim.get_base_velocity(body_id)
+        return np.concatenate((lin_vel, ang_vel))
 
-    def get_object_linear_velocity(self, object_id):
+    def get_body_linear_velocity(self, body_id):
         """
-        Return the linear velocity of the given object.
+        Return the linear velocity of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
-            float[3]: linear velocity of the object
+            np.array[3]: linear velocity of the body
         """
-        return np.array(self.sim.get_base_velocity(object_id)[0])
+        return self.sim.get_base_velocity(body_id)[0]
 
-    def get_object_angular_velocity(self, object_id):
+    def get_body_angular_velocity(self, body_id):
         """
-        Return the angular velocity of the given object.
+        Return the angular velocity of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
-            float[3]: angular velocity of the object
+            np.array[3]: angular velocity of the body
         """
-        return np.array(self.sim.get_base_velocity(object_id)[1])
+        return self.sim.get_base_velocity(body_id)[1]
 
-    def hide_object(self, object_id):
+    def hide_body(self, body_id):
         """
-        Hide (visually) the given object; by making it transparent.
+        Hide (visually) the given body; by making it transparent.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
         """
-        color = self.get_object_color(object_id)
+        color = self.get_body_color(body_id)
         color[-1] = 0.
-        self.change_object_color(object_id, color=color)
+        self.change_body_color(body_id, color=color)
 
-    def show_object(self, object_id):
+    def show_body(self, body_id):
         """
-        Show (visually) a hidden object; by making it opaque.
+        Show (visually) a hidden body; by making it opaque.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
         """
-        color = self.get_object_color(object_id)
+        color = self.get_body_color(body_id)
         color[-1] = 1.
-        self.change_object_color(object_id, color=color)
+        self.change_body_color(body_id, color=color)
 
     def remove(self, body):
         """
-        Remove the object specified by its unique id from the world/simulator.
+        Remove the body specified by its unique id from the world/simulator.
 
         Args:
-            body (int, Robot): unique id of the object in the simulator.
+            body (int, Body): unique id of the body in the simulator.
 
         Returns:
             bool: True if succeeded, False if not. This method does not raise any errors.
         """
-        if isinstance(body, Robot):
+        if isinstance(body, Body):
             body = body.id
-        if body in self.robots:
-            self.robots.pop(body)
-        elif body in self.movable_bodies:
-            self.movable_bodies.pop(body)
-        elif body in self.immovable_bodies:
-            self.immovable_bodies.pop(body)
-        elif body in self.visual_objects:
-            self.visual_objects.pop(body)
+        if body in self.bodies:
+            self.bodies.pop(body)
+            if body in self.ids:
+                self.ids.pop(body)
         else:
             return False
 
         self.sim.remove_body(body)
         return True
 
-    def get_object_dimensions(self, object_id):
+    def get_body_dimensions(self, body_id):
         """
-        Return the object dimensions of the given object.
+        Return the body dimensions of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
 
         Returns:
-            float[3]: dimensions of the object
+            float[3]: dimensions of the body
         """
-        return np.array(self.sim.get_visual_shape_data(object_id)[3])
+        return self.sim.get_visual_shape_data(body_id)[0][3]
 
-    def change_object_scale(self, object_id, scale=(1., 1., 1.)):
+    def change_body_scale(self, body_id, scale=(1., 1., 1.)):
         """
-        Change the scale of the given object; it changes the scale for the visual and collision shapes.
+        Change the scale of the given body; it changes the scale for the visual and collision shapes.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
             scale (float[3]): scaling factors in each direction
         """
         # TODO: currently not possible in PyBullet
         pass
 
-    def get_object_aabb(self, object_id, link_id=-1):
+    def get_body_aabb(self, body_id, link_id=-1):
         """
-        Return the axis-aligned bounding box (AABB) in world space of the given object.
+        Return the axis-aligned bounding box (AABB) in world space of the given body.
 
         Args:
-            object_id (int): object id
+            body_id (int): body id
             link_id (int): optional link id
 
         Returns:
-            float[3]: coordinates in world space of the min corner of the AABB
-            float[3]: coordinates in world space of the max corner of the AABB
+            np.array[3]: coordinates in world space of the min corner of the AABB
+            np.array[3]: coordinates in world space of the max corner of the AABB
         """
-        aabb_min, aabb_max = self.sim.get_aabb(object_id, link_id)
-        return np.array(aabb_min), np.array(aabb_max)
+        aabb_min, aabb_max = self.sim.get_aabb(body_id, link_id)
+        return aabb_min, aabb_max
 
-    def get_object_ids_in_aabb(self, aabb_min, aabb_max):
+    def get_body_ids_in_aabb(self, aabb_min, aabb_max):
         """
-        Get the list of object ids that have AABB overlap with a given AABB.
+        Get the list of body ids that have AABB overlap with a given AABB.
 
         Args:
             aabb_min (float[3]): coordinates of the min corner of the bounding box
             aabb_max (float[3]): coordinates of the max corner of the bounding box
 
         Returns:
-            int[N]: list of object ids
+            int[N]: list of body ids
         """
-        overlapping_objects = self.sim.get_overlapping_objects(aabb_min, aabb_max)
-        if overlapping_objects is None:
+        overlapping_bodies = self.sim.get_overlapping_objects(aabb_min, aabb_max)
+        if overlapping_bodies is None:
             return []
-        return overlapping_objects
+        return overlapping_bodies
 
     def is_there_an_object(self, aabb_min, aabb_max, except_floor=True):
         """
@@ -835,23 +876,23 @@ class World(object):
         Returns:
             bool: True if there is an object in the specified bounding box
         """
-        objects = self.sim.get_overlapping_objects(aabb_min, aabb_max)
-        if len(objects) > 2:
+        bodies = self.sim.get_overlapping_objects(aabb_min, aabb_max)
+        if len(bodies) > 2:
             return True
-        if len(objects) == 0:
+        if len(bodies) == 0:
             return False
-        idx = objects[0]
+        idx = bodies[0]
         if idx == self.floor_id and except_floor:
             return False
         return True
 
-    def get_closest_objects(self, body, radius=1, link_id=-1, body2=None, link2_id=-1):  # Not possible for now
+    def get_closest_bodies(self, body, radius=1, link_id=-1, body2=None, link2_id=-1):  # Not possible for now
         """
-        Get the closest objects from the specified body (or link) within the specified radius.
+        Get the closest bodies from the specified body (or link) within the specified radius.
 
         Args:
             body (Body): body.
-            radius (float): radius around the body in which we check the closest objects.
+            radius (float): radius around the body in which we check the closest bodies.
             link_id (int): link id. Only report contact points that involve link index of body A.
             body2 (int): only report contact points that involve body B. Important: you need to have a valid body A
                 if you provide body B
@@ -876,7 +917,7 @@ class World(object):
         """
         if body2 is not None:
             return self.sim.get_closest_points(body1=body.id, body2=body2, distance=radius,
-                                               link1_id=link_id, link2_id=link_id)
+                                               link1_id=link_id, link2_id=link2_id)
 
     def load_floor(self, scaling=1.):
         """
@@ -935,8 +976,7 @@ class World(object):
             if filename[-3:] == 'obj':  # obj (mesh)
                 if not isinstance(scaling, (list, tuple)):
                     scaling = [scaling] * 3
-                self.floor_id = self.load_mesh(filename, position, orientation, mass=0., scale=scaling, flags=1,
-                                               object_type='terrain')
+                self.floor_id = self.load_mesh(filename, position, orientation, mass=0., scale=scaling, flags=1)
 
             elif filename[-3:] == 'sdf':  # SDF
                 self.floor_id = self.load_sdf(filename=filename, scaling=scaling)
@@ -959,8 +999,7 @@ class World(object):
                 # load the obj
                 if not isinstance(scaling, (list, tuple)):
                     scaling = [scaling] * 3
-                self.floor_id = self.load_mesh(filename, position, orientation, mass=0., scale=scaling, flags=1,
-                                               object_type='terrain')
+                self.floor_id = self.load_mesh(filename, position, orientation, mass=0., scale=scaling, flags=1)
         else:
             raise TypeError("Expecting the given 'heightmap' to be a string or a numpy array, instead got: "
                             "{}".format(type(heightmap)))
@@ -1168,8 +1207,7 @@ class World(object):
         Returns:
             int: unique id of the table
         """
-        table = self.sim.load_urdf('table/table.urdf', position=position, orientation=orientation, scale=scaling)
-        self.movable_bodies[table] = 'table'
+        table = self.load_urdf('table/table.urdf', position=position, orientation=orientation, scale=scaling)
         return table
 
     def load_kiva_shelf(self, scaling=1.):
@@ -1183,7 +1221,8 @@ class World(object):
             int: unique id of the shelf
         """
         shelf = self.sim.load_sdf('kiva_shelf/model.sdf', scale=scaling)[0]
-        self.movable_bodies[shelf] = 'shelf'
+        self.bodies[shelf] = shelf
+        self.ids[shelf] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return shelf
 
     def load_visual_sphere(self, position, radius=0.5, color=None):
@@ -1200,7 +1239,8 @@ class World(object):
         """
         visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color)
         sphere = self.sim.create_body(visual_shape_id=visual_shape, mass=0., position=position)
-        self.visual_objects[sphere] = 'sphere'
+        self.bodies[sphere] = sphere
+        self.ids[sphere] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return sphere
 
     def load_sphere(self, position, mass=1., radius=0.5, color=None):
@@ -1218,13 +1258,12 @@ class World(object):
         """
         collision_shape = self.sim.create_collision_shape(self.sim.GEOM_SPHERE, radius=radius)
         visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color)
+
         sphere = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape,
                                       position=position)
-        if mass == 0.0:
-            self.immovable_bodies[sphere] = 'sphere'
-        else:
-            self.movable_bodies[sphere] = 'sphere'
 
+        self.bodies[sphere] = sphere
+        self.ids[sphere] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return sphere
 
     def load_visual_box(self, position, orientation=(0, 0, 0, 1), dimensions=(1., 1., 1.), color=None):
@@ -1242,8 +1281,11 @@ class World(object):
         """
         dimensions = np.array(dimensions) / 2.
         visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dimensions, rgba_color=color)
+
         box = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position, orientation=orientation)
-        self.visual_objects[box] = 'box'
+
+        self.bodies[box] = box
+        self.ids[box] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return box
 
     def load_box(self, position, orientation=(0, 0, 0, 1), mass=1., dimensions=(1., 1., 1.), color=None):
@@ -1263,14 +1305,12 @@ class World(object):
         dimensions = np.array(dimensions) / 2.
         collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=dimensions)
         visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dimensions, rgba_color=color)
-        
+
         box = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape,
                                    position=position, orientation=orientation)
 
-        if mass == 0.0:
-            self.immovable_bodies[box] = 'box'
-        else:
-            self.movable_bodies[box] = 'box'
+        self.bodies[box] = box
+        self.ids[box] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return box
 
     def load_visual_cylinder(self, position, orientation=(0, 0, 0, 1), radius=0.5, height=1., color=None):
@@ -1292,7 +1332,8 @@ class World(object):
         
         cylinder = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position,
                                         orientation=orientation)
-        self.visual_objects[cylinder] = 'cylinder'
+        self.bodies[cylinder] = cylinder
+        self.ids[cylinder] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return cylinder
 
     def load_cylinder(self, position, orientation=(0, 0, 0, 1), mass=1., radius=0.5, height=1., color=None):
@@ -1317,10 +1358,8 @@ class World(object):
         cylinder = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape,
                                         position=position, orientation=orientation)
 
-        if mass == 0.0:
-            self.immovable_bodies[cylinder] = 'cylinder'
-        else:
-            self.movable_bodies[cylinder] = 'cylinder'
+        self.bodies[cylinder] = cylinder
+        self.ids[cylinder] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return cylinder
 
     def load_visual_capsule(self, position, orientation=(0, 0, 0, 1), radius=0.5, height=1., color=None):
@@ -1340,11 +1379,11 @@ class World(object):
         height = height/2.
         visual_shape = self.sim.create_visual_shape(self.sim.GEOM_CAPSULE, radius=radius, length=height,
                                                     rgba_color=color)
-        
         capsule = self.sim.create_body(mass=0., visual_shape_id=visual_shape, position=position,
                                        orientation=orientation)
 
-        self.visual_objects[capsule] = 'capsule'
+        self.bodies[capsule] = capsule
+        self.ids[capsule] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return capsule
 
     def load_capsule(self, position, orientation=(0, 0, 0, 1), mass=1., radius=0.5, height=1., color=None):
@@ -1369,14 +1408,12 @@ class World(object):
         
         capsule = self.sim.create_body(mass=mass, collision_shape_id=collision_shape, visual_shape_id=visual_shape,
                                        position=position, orientation=orientation)
-        if mass == 0.0:
-            self.immovable_bodies[capsule] = 'capsule'
-        else:
-            self.movable_bodies[capsule] = 'capsule'
+
+        self.bodies[capsule] = capsule
+        self.ids[capsule] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return capsule
 
-    def load_visual_mesh(self, filename, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None,
-                         object_type='mesh'):
+    def load_visual_mesh(self, filename, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None):
         """
         Load a visual mesh in the world (only available in the simulator).
 
@@ -1393,11 +1430,12 @@ class World(object):
         """
         mesh = self.sim.load_mesh(filename, position, orientation, mass=0., scale=scale, color=color,
                                   with_collision=False)
-        self.visual_objects[mesh] = object_type
+        self.bodies[mesh] = mesh
+        self.ids[mesh] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return mesh
 
     def load_mesh(self, filename, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None,
-                  flags=None, object_type='mesh'):
+                  flags=None):
         """
         Load a mesh in the world (only available in the simulator).
 
@@ -1410,17 +1448,15 @@ class World(object):
             scale (float[3]): scale the mesh in the (x,y,z) directions
             color (int[4], None): color of the mesh for red, green, blue, and alpha, each in range [0,1]
             flags (int, None): if flag = `sim.GEOM_FORCE_CONCAVE_TRIMESH` (=1), this will create a concave static
-                triangle mesh. This should not be used with dynamic/moving objects, only for static (mass=0) terrain.
+                triangle mesh. This should not be used with dynamic/moving bodies, only for static (mass=0) terrain.
 
         Returns:
             int: unique id of the mesh in the world
         """
         mesh = self.sim.load_mesh(filename, position, orientation, mass, scale, color, with_collision=True,
                                   flags=flags)
-        if mass == 0.0:
-            self.immovable_bodies[mesh] = object_type
-        else:
-            self.movable_bodies[mesh] = object_type
+        self.bodies[mesh] = mesh
+        self.ids[mesh] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return mesh
 
     # The following commented code does not work currently because URDF_GEOM_PLANE is not set in Bullet
@@ -1444,7 +1480,8 @@ class World(object):
     #                                     visual_shape_id=visual_shape,
     #                                     position=position,
     #                                     orientation=orientation)
-    #     self.visual_objects[plane] = 'plane'
+    #     self.bodies[plane] = plane
+    #     self.ids[plane] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
     #     return plane
     #
     # def load_plane(self, position, orientation, mass=1., normal=(0.,0.,1.), color=(1,1,1,1)):
@@ -1469,10 +1506,8 @@ class World(object):
     #                                      visual_shape_id=visual_shape,
     #                                      position=position,
     #                                      orientation=orientation)
-    #     if mass == 0.0:
-    #         self.immovable_bodies[plane] = 'plane'
-    #     else:
-    #         self.movable_bodies[plane] = 'plane'
+    #     self.bodies[plane] = plane
+    #     self.ids[plane] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
     #     return plane
 
     # Temporary because the code above doesn't work
@@ -1489,7 +1524,8 @@ class World(object):
             int: unique id of the plane
         """
         plane = self.sim.load_urdf('plane.urdf', position, orientation, use_fixed_base=True, scale=scale)
-        self.immovable_bodies[plane] = plane
+        self.bodies[plane] = plane
+        self.ids[plane] = [self.__get_method_and_parameters(frame=inspect.currentframe())]
         return plane
 
     def load_visual_ellipsoid(self, position, orientation=(0, 0, 0, 1), scale=(1., 1., 1.), color=None):
@@ -1599,12 +1635,12 @@ class World(object):
     def load_arrow(self, position, orientation=(0, 0, 0, 1), mass=1., scale=(1., 1., 1.), color=None):
         pass
 
-    def distribute_objects(self, distributor, objects):
+    def distribute_bodies(self, distributor, bodies):
         pass
 
     def get_dynamics_info(self, body_id, link_id=-1):
         """
-        Return the dynamics information about objects that are in the world.
+        Return the dynamics information about bodies that are in the world.
 
         Args:
             body_id (int): object unique id.
@@ -1675,9 +1711,6 @@ class World(object):
             texture (str): path to the texture.
             body_id (int): unique body id.
             link_id (int): link id. If -1, it will be the base.
-
-        Returns:
-
         """
         texture = self.sim.load_texture(texture)
         self.sim.change_visual_shape(object_id=body_id, link_id=link_id, texture_id=texture)
@@ -1759,6 +1792,7 @@ if __name__ == '__main__':
 
     # load basic shapes
     sphere = world.load_visual_sphere([1., 0, 1.], color=(1, 0, 0, 0.5))
+    sphere = world.wrap(sphere, name='sphere')
     # world.load_visual_box([-1,0,1], dimensions=[1.,1.,1.], color=[0,0,1,0.5])
     # world.load_cylinder([0, -1, 1], color=[1, 0, 0, 1])
     #  world.load_capsule([0, 1, 1],  color=[1, 0, 0, 1])
@@ -1794,9 +1828,11 @@ if __name__ == '__main__':
 
         if t % T == 0:
             if red:
-                world.change_object_color(sphere, (1,0,0,0.5))
+                world.change_body_color(sphere.id, (1, 0, 0, 0.5))
             else:
-                world.change_object_color(sphere, (0,0,1,0.5))
+                world.change_body_color(sphere.id, (0, 0, 1, 0.5))
             red = not red
-        world.move_object(sphere, p)
+        # world.move_object(sphere, p)
+        sphere.position = p
+        world.apply_force(body_id=sphere.id, force=(0, 0, 100))
         world.step(sleep_dt=1./240)