diff --git a/docs/source/models.rst b/docs/source/models.rst
index 6ed93c9..4cc4781 100644
--- a/docs/source/models.rst
+++ b/docs/source/models.rst
@@ -13,7 +13,7 @@ Learning models can be divided into 2 categories:
 	- Polynomial models
 	- Deep Neural Networks (DNNs)
 	- Gaussian processes (GPs)
-- Trajectory based learning models:
+- Trajectory based learning models
 	- Dynamic Movement Primitives (DMPs)
 	- Central Pattern Generators (CPGs)
 	- Gaussian Mixture Models and Gaussian Mixture Regression (GMMs/GMRs)
diff --git a/docs/source/overview.rst b/docs/source/overview.rst
new file mode 100644
index 0000000..e69de29
diff --git a/docs/source/priorities.rst b/docs/source/priorities.rst
deleted file mode 100644
index 54d6d64..0000000
--- a/docs/source/priorities.rst
+++ /dev/null
@@ -1,3 +0,0 @@
-Priority Tasks
-==============
-
diff --git a/docs/source/pyrobolearn.rst b/docs/source/pyrobolearn.rst
index e3a145b..6adfbd0 100644
--- a/docs/source/pyrobolearn.rst
+++ b/docs/source/pyrobolearn.rst
@@ -14,7 +14,7 @@ General idea.
 - lack of benchmarks
 - lack of flexibility and modularity
 - lack of generalization
-- high coupling
+- high coupling between different modules
 
 For instance:
 
diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py
index a3763db..bce8877 100644
--- a/pyrobolearn/actions/robot_actions/joint_actions.py
+++ b/pyrobolearn/actions/robot_actions/joint_actions.py
@@ -8,7 +8,7 @@ import copy
 import numpy as np
 from abc import ABCMeta
 
-from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
+from pyrobolearn.actions.robot_actions.robot_actions import RobotAction, Robot
 
 
 __author__ = "Brian Delhaisse"
@@ -85,11 +85,18 @@ class JointPositionAction(JointAction):
             joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
             kp (float, np.array[N], None): position gain(s)
             kd (float, np.array[N], None): velocity gain(s)
-            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
-                default maximum force values.
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If None, it will apply the
+                default maximum force values (read from the URDF).
         """
-        self.kp, self.kd, self.max_force = kp, kd, max_force
         super(JointPositionAction, self).__init__(robot, joint_ids)
+        self.kp, self.kd, self.max_force = kp, kd, max_force
+
+        # # check max force and take the one by default
+        # if self.max_force is None:
+        #     self.max_force = self.robot.get_joint_max_forces(self.joints)
+        #     if np.allclose(self.max_force, 0):
+        #         self.max_force = None
+
         self.data = robot.get_joint_positions(self.joints)
 
     def _write(self, data):
@@ -303,6 +310,16 @@ class JointTorqueAction(JointAction):
         """
         super(JointTorqueAction, self).__init__(robot, joint_ids)
         self.data = robot.get_joint_torques(self.joints)
+
+        # check torque bounds
+        if f_min is None or f_max is None:
+            f = robot.get_joint_max_forces(joint_ids=self.joints)
+            f_min = -f if f_min is None else f_min
+            f_max = f if f_max is None else f_max
+            if np.allclose(f_min, 0):
+                f_min = -np.infty
+            if np.allclose(f_max, 0):
+                f_max = np.infty
         self.f_min = f_min
         self.f_max = f_max
 
diff --git a/pyrobolearn/actions/robot_actions/robot_actions.py b/pyrobolearn/actions/robot_actions/robot_actions.py
index e1b5004..bcd03c3 100644
--- a/pyrobolearn/actions/robot_actions/robot_actions.py
+++ b/pyrobolearn/actions/robot_actions/robot_actions.py
@@ -34,9 +34,15 @@ class RobotAction(Action):
     __metaclass__ = ABCMeta
 
     def __init__(self, robot):
+        """Initialize the abstract robot action.
+
+        Args:
+            robot (Robot): a robot instance.
+        """
         super(RobotAction, self).__init__()
         if not isinstance(robot, Robot):
-            raise TypeError("The 'robot' parameter has to be an instance of Robot")
+            raise TypeError("The 'robot' parameter has to be an instance of Robot, but instead got: "
+                            "{}".format(type(robot)))
         self._robot = robot
 
     @property
@@ -44,9 +50,11 @@ class RobotAction(Action):
         return self._robot
 
     def is_discrete(self):
+        """By default, robot actions are continuous."""
         return False
 
     def is_continuous(self):
+        """By default, robot actions are continuous."""
         return True
 
     def __copy__(self):
diff --git a/pyrobolearn/envs/control/pendulum.py b/pyrobolearn/envs/control/pendulum.py
index c276585..988eb0e 100644
--- a/pyrobolearn/envs/control/pendulum.py
+++ b/pyrobolearn/envs/control/pendulum.py
@@ -9,7 +9,6 @@ References:
     - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
 """
 
-import os
 import numpy as np
 
 import pyrobolearn as prl
@@ -29,22 +28,25 @@ __status__ = "Development"
 class InvertedPendulumSwingUpEnv(ControlEnv):
     r"""Inverted Pendulum Swing-up Environment
 
-    This is based on the control problem proposed in OpenAI Gym:
+    This is based on the control problem proposed in OpenAI Gym [1]:
     "The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the
     problem, the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
 
     Here are the various environment features:
 
-    - world: basic world with gravity, a basic floor, and the pendulum.
-    - state: the state is given by :math:`[np.cos(q_1), np.sin(q_1), dq_1]`
+    - world: basic world with gravity, a basic floor, and the pendulum loaded at the center.
+    - state: the state is given by :math:`[cos(q_1), sin(q_1), \dot{q}_1]`
     - action: the action is the joint torque :math:`\tau_1`
-    - reward:
+    - cost: :math:`||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2`, where :math:`d(\cdot, \cdot)`
+      is the minimum angle difference between two angles.
+    - initial state generator: initialize the joint angle between [-pi, pi] (q=0 when the pendulum is pointing up)
+    - physics randomizer: uniform distribution of the mass of the pendulum [mass - mass/10, mass + mass/10]
 
     References:
         - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
     """
 
-    def __init__(self, simulator=None, verbose=False):
+    def __init__(self, simulator, verbose=False):
         """
         Initialize the inverted pendulum swing-up environment.
 
@@ -52,46 +54,80 @@ class InvertedPendulumSwingUpEnv(ControlEnv):
             simulator (Simulator): simulator instance.
             verbose (bool): if True, it will print information when creating the environment
         """
-        # create basic world
+        # create basic world with the robot
         world = prl.worlds.BasicWorld(simulator)
         robot = world.load_robot('pendulum')
         robot.disable_motor()
-        # robot.print_info()
+        if verbose:
+            robot.print_info()
 
         # create state
-        position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+        trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
         velocity_state = prl.states.JointVelocityState(robot=robot)
-        state = position_state + velocity_state
+        state = trig_position_state + velocity_state
+        if verbose:
+            print("\nObservation: {}".format(state))
 
         # create action
         action = prl.actions.JointTorqueAction(robot, f_min=-2., f_max=2.)
+        if verbose:
+            print("\nAction: {}".format(action))
 
-        # create reward
+        # create reward/cost
         position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
-                                                      np.zeros(len(robot.joints)))
+                                                      target_state=np.zeros(len(robot.joints)),
+                                                      update_state=True)
         velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
-        torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot))
+        torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot), update_state=True)
         reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+        if verbose:
+            print("Reward: {}".format(reward))
 
-        # create initial state generator
-        initial_state_generator = None
+        # create initial state generator: generate the state each time we reset the environment
+        def reset_robot(robot):  # function to disable the motors every time we reset the joint state
+            def reset():
+                robot.disable_motor()
+            return reset
+
+        init_state = prl.states.JointPositionState(robot)
+        low, high = np.array([-np.pi] * len(robot.joints)), np.array([np.pi] * len(robot.joints))
+        # init_state.data = np.array([np.pi / 2])  # initial data
+        # initial_state_generator = prl.states.generators.FixedStateGenerator(state=init_state, fct=reset_robot(robot))
+        initial_state_generator = prl.states.generators.UniformStateGenerator(state=init_state, low=low, high=high,
+                                                                              fct=reset_robot(robot))
+
+        # create physics randomizer: randomize the mass each time we reset the environment
+        masses = robot.get_link_masses(link_ids=robot.joints)
+        masses = (masses - masses/10., masses + masses/10.)
+        physics_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
+
+        # could create terminal conditions (if necessary) such as:
+        # - success if we stay at the upper position for more than 20 steps
+        # - failure if we can achieve the goal after 10,000 steps
+        # In the OpenAI gym, there are no terminal conditions for this problem
+        terminal_condition = None
 
         # create environment using composition
         super(InvertedPendulumSwingUpEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
-                                                         initial_state_generators=initial_state_generator)
+                                                         initial_state_generators=initial_state_generator,
+                                                         physics_randomizers=physics_randomizer,
+                                                         terminal_conditions=terminal_condition)
 
 
 # Test
 if __name__ == "__main__":
-    from itertools import count
-    import pyrobolearn as prl
 
     # create simulator
     sim = prl.simulators.Bullet()
 
     # create environment
-    env = InvertedPendulumSwingUpEnv(sim)
+    env = InvertedPendulumSwingUpEnv(sim, verbose=True)
 
     # run simulation
-    for _ in count():
-        env.step(sleep_dt=1./240)
+    env.reset()
+    for t in prl.count():
+        # if (t % 800) == 0:
+        #     env.reset()  # test reset function
+        states, rewards, done, info = env.step(sleep_dt=1./240)
+        # print("State: {}".format(states))
+        print("Reward: {}".format(rewards))
diff --git a/pyrobolearn/envs/env.py b/pyrobolearn/envs/env.py
index 0e79991..0647a8d 100644
--- a/pyrobolearn/envs/env.py
+++ b/pyrobolearn/envs/env.py
@@ -27,7 +27,7 @@ from pyrobolearn.states.generators import StateGenerator
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "MIT"
+__license__ = "GNU GPLv3"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"
diff --git a/pyrobolearn/physics/body_physics_randomizer.py b/pyrobolearn/physics/body_physics_randomizer.py
index 4ab1212..92f9820 100644
--- a/pyrobolearn/physics/body_physics_randomizer.py
+++ b/pyrobolearn/physics/body_physics_randomizer.py
@@ -6,11 +6,13 @@ Dependencies:
 - `pyrobolearn.robots`
 """
 
+from abc import ABCMeta
+
 from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer
-# from pyrobolearn.robots.base import Object  # TODO: change to Body or MultiBody
+from pyrobolearn.robots.base import Body
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -25,6 +27,7 @@ class BodyPhysicsRandomizer(PhysicsRandomizer):
     The body physics randomizer can randomize the physical attributes of a body. It is an abstract class which is
     inherited notably by `LinkPhysicsRandomizer` and `JointPhysicsRandomizer`.
     """
+    __metaclass__ = ABCMeta
 
     def __init__(self, body):
         """
@@ -49,10 +52,8 @@ class BodyPhysicsRandomizer(PhysicsRandomizer):
     @body.setter
     def body(self, body):
         """Set the body / object instance."""
-        # TODO: uncomment the following lines
-        # if not isinstance(body, Object):
-        #     raise TypeError("Expecting the given body to be an instance of `Object`, instead got: "
-        #                     "{}".format(type(body)))
+        if not isinstance(body, Body):
+            raise TypeError("Expecting the given body to be an instance of `Body`, instead got: {}".format(type(body)))
         self._body = body
 
     @property
diff --git a/pyrobolearn/physics/joint_physics_randomizer.py b/pyrobolearn/physics/joint_physics_randomizer.py
index 95c2862..1a9e1a3 100644
--- a/pyrobolearn/physics/joint_physics_randomizer.py
+++ b/pyrobolearn/physics/joint_physics_randomizer.py
@@ -7,12 +7,14 @@ Dependencies:
 """
 
 import collections
+import numpy as np
 
 from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
+from pyrobolearn.robots.robot import Robot
 
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -25,30 +27,33 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
     r"""Joint Physics Randomizer
 
     The joint physics randomizer can randomize the physical attributes of a joint. For instance, this can be the
-    joint friction or damping coefficients. Other attributes can be the maximum force or velocity the joint(s) can
-    achieve.
+    joint friction or damping coefficients.
     """
 
-    def __init__(self, body, joint_ids=None, joint_damping=None, **kwargs):
+    def __init__(self, robot, joint_ids=None, joint_frictions=None, joint_dampings=None):
         """
         Initialize the joint physics randomizer.
 
         Args:
-            body (Body): multi-body object.
+            robot (Robot): robot instance.
             joint_ids (int, list of int, None): joint id(s).
-            joint_damping (float, list of float, tuple of float, list of tuple of float, None): joint damping
-                coefficient. If None, it will take the default joint damping value associated with the given
-                `joint_ids` of the given `body`. If float, it will set that value to the specified joints and will
-                always return this value when sampling. If list of float, it will set each value to each joint and will
-                always return these values when sampling. If tuple of float, the first item is the lower bound of the
-                joint damping and the second item is its upper bound. It will set these bounds for each joint. If list
-                of tuples of joints, it will have a tuple of lower / upper bound for each joint.
-            **kwargs (dict): range of possible physical properties. If given one value, that property won't be
-                randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
+            joint_frictions (float, list/tuple of float, np.array[N], None): joint friction bounds. If None, it
+                doesn't randomize this parameter.
+            joint_dampings (float, list/tuple of float, np.array[N], None): joint damping bounds. If None, it doesn't
+                randomize this parameter.
         """
-        super(JointPhysicsRandomizer, self).__init__(body)
+        if not isinstance(robot, Robot):
+            raise TypeError("Expecting the given 'robot' to be an instance of `Robot`, instead got: "
+                            "{}".format(type(robot)))
+        super(JointPhysicsRandomizer, self).__init__(robot)
+
+        # set joint ids
         self.joints = joint_ids
 
+        # set bounds
+        self.joint_friction_bounds = joint_frictions
+        self.joint_damping_bounds = joint_dampings
+
     ##############
     # Properties #
     ##############
@@ -75,6 +80,27 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
                             "{}".format(type(joints)))
         self._joints = joints
 
+    @property
+    def joint_frictions(self):
+        """Return the joint friction associated with the joints."""
+        return self.body.get_joint_frictions(joint_ids=self.joints)
+
+    @joint_frictions.setter
+    def joint_frictions(self, values):
+        """Set the given joint friction values to each joint."""
+        raise NotImplementedError("This feature is not available yet.")
+
+    @property
+    def joint_friction_bounds(self):
+        """Return the joint friction bounds."""
+        return self._joint_friction_bounds
+
+    @joint_friction_bounds.setter
+    def joint_friction_bounds(self, values):
+        """Set the given joint frictions bounds."""
+        self._check_bounds('joint_frictions', values)
+        self._joint_friction_bounds = values
+
     @property
     def joint_dampings(self):
         """Return the joint dampings associated with the joints."""
@@ -83,8 +109,27 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
     @joint_dampings.setter
     def joint_dampings(self, values):
         """Set the given joint damping values to each joint."""
+        # check values
+        if isinstance(values, (float, int)):
+            values = [values] * len(self.joints)
+        elif len(values) != len(self.joints):
+            raise ValueError("The number of given joint damping values (={}) does not match with the number of "
+                             "joints (={})".format(len(values), len(self.joints)))
+
+        # set the joint dampings
         for joint, value in zip(self.joints, values):
-            self.body.set_joint_damping(joint, value)
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=joint, joint_damping=value)
+
+    @property
+    def joint_damping_bounds(self):
+        """Return the joint damping bounds."""
+        return self._joint_damping_bounds
+
+    @joint_damping_bounds.setter
+    def joint_damping_bounds(self, values):
+        """Set the given joint damping bounds."""
+        self._check_bounds('joint_dampings', values)
+        self._joint_damping_bounds = values
 
     ###########
     # Methods #
@@ -92,21 +137,25 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
 
     def names(self):
         """Return an iterator over the property names."""
-        for name in ['joint_damping']:
-            yield name
+        yield 'joint_damping'
+        yield 'joint_friction'
 
     def bounds(self):
         """Return an iterator over the bounds for each property."""
-        pass
+        yield self.joint_damping_bounds
+        yield self.joint_friction_bounds
 
     def get_properties(self):
         """
-        Get the physics properties.
+        Get the current physical properties.
 
         Returns:
-            dict: current physic property values.
+            dict: current physical property values.
         """
-        pass
+        joint_dampings = self.joint_dampings
+        joint_frictions = self.joint_frictions
+        return {joint_id: {'joint_damping': joint_dampings[joint_id], 'joint_friction': joint_frictions[joint_id]}
+                for joint_id in self.joints}
 
     def set_properties(self, properties):
         """
@@ -115,4 +164,38 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):
         Args:
             properties (dict): the physic property values to be set in the simulator.
         """
-        pass
+        # check the given properties
+        if not isinstance(properties, dict):
+            raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: "
+                            "{}".format(type(properties)))
+
+        # set the properties of each joint in the simulator
+        if len(properties) > 0:
+            for joint_id in self.joints:
+                self.simulator.change_dynamics(self.body.id, link_id=joint_id, **properties[joint_id])
+
+    def sample(self, seed=None):
+        """
+        Sample a new set of physics properties and returns it. Note that it doesn't set them in the simulator.
+        This sampling can be useful if the user wishes to check more carefully the sampled physic property values.
+        Once satisfied, the user can set them by calling the `set_properties` method.
+
+        Note that it samples uniformly the physics properties between their specified lower and upper bounds.
+
+        Args:
+            seed (int, None): random seed.
+
+        Returns:
+            dict: sampled physic properties.
+        """
+        # set random seed
+        if seed is not None:
+            np.random.seed(seed)
+
+        # sample each property
+        properties = dict()
+        for joint in self.joints:
+            for name, bound in zip(self.names(), self.bounds()):
+                if bound is not None:
+                    properties.setdefault(joint, {})[name] = np.random.uniform(low=bound[0], high=bound[1])
+        return properties
diff --git a/pyrobolearn/physics/link_physics_randomizer.py b/pyrobolearn/physics/link_physics_randomizer.py
index 7f1b02c..1864fa2 100644
--- a/pyrobolearn/physics/link_physics_randomizer.py
+++ b/pyrobolearn/physics/link_physics_randomizer.py
@@ -7,12 +7,13 @@ Dependencies:
 """
 
 import collections
+import numpy as np
 
 from pyrobolearn.physics.body_physics_randomizer import BodyPhysicsRandomizer
 
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -30,20 +31,56 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
     def __init__(self, body, link_ids=None, masses=None, local_inertia_diagonals=None, local_inertia_positions=None,
                  local_inertia_orientations=None, lateral_frictions=None, spinning_frictions=None,
                  rolling_frictions=None, restitutions=None, linear_dampings=None, angular_dampings=None,
-                 contact_stiffnesses=None, contact_dampings=None, **kwargs):
+                 contact_stiffnesses=None, contact_dampings=None):
         """
         Initialize the link physics randomizer.
 
         Args:
-            body (Body): multi-body object.
+            body (Body, Robot): a body or robot instance.
             link_ids (int, list of int, None): link id(s).
-            **kwargs (dict): range of possible physical properties. If given one value, that property won't be
-                randomized. Each range is a tuple of two values `[lower_bound, upper_bound]`.
+            masses (tuple/list of 2 float/np.array[N], np.array[2,N], None): lower and upper bounds for the mass of
+                each given link. If None, it won't randomize them.
+            local_inertia_diagonals (tuple/list of 2 np.array[3], np.array[2,N,3]): lower and upper bounds for the
+                local inertia diagonal for each given link. If None, it won't randomize them.
+            local_inertia_positions: lower and upper bounds for the local inertia position for each given link. If
+                None, it won't randomize them.
+            local_inertia_orientations: lower and upper bounds for the local inertia orientation for each given link.
+                If None, it won't randomize them.
+            lateral_frictions: lower and upper bounds for the lateral friction for each given link. If None, it won't
+                randomize them.
+            spinning_frictions: lower and upper bounds for the spinning friction for each given link. If None, it
+                won't randomize them.
+            rolling_frictions: lower and upper bounds for the rolling friction for each given link. If None, it won't
+                randomize them.
+            restitutions: lower and upper bounds for the restitution coefficient for each given link. If None, it
+                won't randomize them.
+            linear_dampings: lower and upper bounds for the linear damping for each given link. If None, it won't
+                randomize them.
+            angular_dampings: lower and upper bounds for the angular damping for each given link. If None, it won't
+                randomize them.
+            contact_stiffnesses: lower and upper bounds for the contact stiffness for each given link. If None, it
+                won't randomize them.
+            contact_dampings: lower and upper bounds for the contact damping for each given link. If None, it won't
+                randomize them.
         """
         super(LinkPhysicsRandomizer, self).__init__(body)
         self.links = link_ids
 
         # set the bounds
+        self.mass_bounds = masses
+        self.local_inertia_diagonal_bounds = local_inertia_diagonals
+
+        # self.local_inertia_position_bounds = local_inertia_positions
+        # self.local_inertia_orientation_bounds = local_inertia_orientations
+        # self.linear_damping_bounds = linear_dampings
+        # self.angular_damping_bounds = angular_dampings
+
+        self.lateral_friction_bounds = lateral_frictions
+        self.spinning_friction_bounds = spinning_frictions
+        self.rolling_friction_bounds = rolling_frictions
+        self.restitution_bounds = restitutions
+        self.contact_stiffness_bounds = contact_stiffnesses
+        self.contact_damping_bounds = contact_dampings
 
     ##############
     # Properties #
@@ -57,7 +94,9 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
     @links.setter
     def links(self, links):
         """Set the link id or the list of link ids."""
-        if isinstance(links, int):
+        if links is None:
+            links = list(range(self.body.num_links))
+        elif isinstance(links, int):
             links = [links]
         elif isinstance(links, collections.Iterable):
             for idx, link in enumerate(links):
@@ -72,54 +111,286 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
     @property
     def masses(self):
         """Return the mass of each specified link."""
-        return self.body.get_masses(self.links)
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[0] for link in self.links]
 
     @masses.setter
     def masses(self, values):
         """Set the mass values."""
-        self.body.set_masses(self.links, values)
+        # check values
+        values = self._check_values('mass', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, mass=value)
 
     @property
     def mass_bounds(self):
-        """Return the lower and upper bounds of each link mass."""
+        """Return the lower and upper bounds of each link's mass."""
         return self._mass_bounds
 
     @mass_bounds.setter
     def mass_bounds(self, bounds):
         """Set the mass bound for each link."""
-        if isinstance(bounds, (float, int)):
-            bounds = [(bounds, bounds) for _ in self.links]
-        elif isinstance(bounds, (list, tuple, np.ndarray)):
-            pass
+        self._check_bounds('masses', bounds)
         self._mass_bounds = bounds
 
     @property
-    def dynamics(self):
-        return None
+    def local_inertia_diagonals(self):
+        """Return the local inertial diagonal of each specified link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[2] for link in self.links]
+
+    @local_inertia_diagonals.setter
+    def local_inertia_diagonals(self, values):
+        """Set the given local inertia diagonals."""
+        if not isinstance(values, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given local inertia diagonals to be list/tuple/np.ndarray of float/int, "
+                            "but got instead: {}".format(values))
+        if len(values) != len(self.links):
+            raise ValueError("The number of given mass values (={}) does not match with the number of links "
+                             "(={})".format(len(values), len(self.links)))
+
+        # set the link local inertia diagonal
+        for link, value in zip(self.links, values):
+            if len(value) != 3:
+                raise ValueError("Expecting the given local inertia diagonal to be a tuple/list/np.ndarray of length "
+                                 "3, but instead got a length of: {}".format(len(value)))
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, local_inertia_diagonal=value)
+
+    @property
+    def local_inertia_diagonal_bounds(self):
+        """Return the lower and upper bounds of each link's local inertia diagonal."""
+        return self._local_inertia_diagonal_bounds
+
+    @local_inertia_diagonal_bounds.setter
+    def local_inertia_diagonal_bounds(self, bounds):
+        """Set the local inertia diagonal bound for each link."""
+        self._check_bounds('local_inertia_diagonals', bounds)
+        self._local_inertia_diagonal_bounds = bounds
+
+    @property
+    def local_inertia_positions(self):
+        """Return the local inertia position of each specified link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[3] for link in self.links]
+
+    @local_inertia_positions.setter
+    def local_inertia_positions(self, values):
+        """Set the given local inertia positions."""
+        raise NotImplementedError("This features is not yet available.")
+
+    @property
+    def local_inertia_orientations(self):
+        """Return the local inertia orientation of each specified link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[4] for link in self.links]
+
+    @local_inertia_orientations.setter
+    def local_inertia_orientations(self, values):
+        """Set the given local inertia orientations."""
+        raise NotImplementedError("This features is not yet available.")
+
+    @property
+    def lateral_frictions(self):
+        """Return the lateral friction of each specified link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[1] for link in self.links]
+
+    @lateral_frictions.setter
+    def lateral_frictions(self, values):
+        """Set the given lateral frictions."""
+        # check values
+        self._check_values('lateral_friction', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, lateral_friction=value)
+
+    @property
+    def lateral_friction_bounds(self):
+        """Return the lower and upper bounds of each link's lateral friction."""
+        return self._lateral_friction_bounds
+
+    @lateral_friction_bounds.setter
+    def lateral_friction_bounds(self, bounds):
+        """Set the lateral friction bound for each link."""
+        self._check_bounds('lateral_frictions', bounds)
+        self._lateral_friction_bounds = bounds
+
+    @property
+    def restitutions(self):
+        """Return the restitution coefficient of each link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[5] for link in self.links]
+
+    @restitutions.setter
+    def restitutions(self, values):
+        """Set the given restitution coefficients."""
+        # check values
+        self._check_values('restitution', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, restitution=value)
+
+    @property
+    def restitution_bounds(self):
+        """Return the lower and upper bounds of each link's restitution coefficient."""
+        return self._restitution_bounds
+
+    @restitution_bounds.setter
+    def restitution_bounds(self, bounds):
+        """Set the restitution coefficient bound for each link."""
+        self._check_bounds('restitutions', bounds)
+        self._restitution_bounds = bounds
+
+    @property
+    def rolling_frictions(self):
+        """Return the rolling friction of each link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[6] for link in self.links]
+
+    @rolling_frictions.setter
+    def rolling_frictions(self, values):
+        """Set the given rolling frictions."""
+        # check values
+        self._check_values('rolling_friction', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, rolling_friction=value)
+
+    @property
+    def rolling_friction_bounds(self):
+        """Return the lower and upper bounds of each link's rolling friction."""
+        return self._rolling_friction_bounds
+
+    @rolling_friction_bounds.setter
+    def rolling_friction_bounds(self, bounds):
+        """Set the rolling friction bound for each link."""
+        self._check_bounds('rolling_frictions', bounds)
+        self._rolling_friction_bounds = bounds
+
+    @property
+    def spinning_frictions(self):
+        """Return the spinning friction of each link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[7] for link in self.links]
+
+    @spinning_frictions.setter
+    def spinning_frictions(self, values):
+        """Set the given spinning frictions."""
+        # check values
+        self._check_values('spinning_friction', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, spinning_friction=value)
+
+    @property
+    def spinning_friction_bounds(self):
+        """Return the lower and upper bounds of each link's spinning friction."""
+        return self._spinning_friction_bounds
+
+    @spinning_friction_bounds.setter
+    def spinning_friction_bounds(self, bounds):
+        """Set the spinning friction bound for each link."""
+        self._check_bounds('spinning_frictions', bounds)
+        self._spinning_friction_bounds = bounds
+
+    @property
+    def contact_dampings(self):
+        """Return the contact damping of each link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[8] for link in self.links]
+
+    @contact_dampings.setter
+    def contact_dampings(self, values):
+        """Set the given contact dampings."""
+        # check values
+        self._check_values('contact_damping', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, contact_damping=value)
+
+    @property
+    def contact_damping_bounds(self):
+        """Return the lower and upper bounds of each link's contact damping."""
+        return self._contact_damping_bounds
+
+    @contact_damping_bounds.setter
+    def contact_damping_bounds(self, bounds):
+        """Set the contact damping bound for each link."""
+        self._check_bounds('contact_dampings', bounds)
+        self._contact_damping_bounds = bounds
+
+    @property
+    def contact_stiffnesses(self):
+        """Return the contact stiffness of each link."""
+        return [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link)[9] for link in self.links]
+
+    @contact_stiffnesses.setter
+    def contact_stiffnesses(self, values):
+        """Set the given contact stiffnesses."""
+        # check values
+        self._check_values('contact_stiffness', values)
+
+        # set the link masses
+        for link, value in zip(self.links, values):
+            self.simulator.change_dynamics(body_id=self.body.id, link_id=link, contact_stiffness=value)
+
+    @property
+    def contact_stiffness_bounds(self):
+        """Return the lower and upper bounds of each link's contact stiffness."""
+        return self._contact_stiffness_bounds
+
+    @contact_stiffness_bounds.setter
+    def contact_stiffness_bounds(self, bounds):
+        """Set the contact stiffness bound for each link."""
+        self._check_bounds('contact_stiffnesses', bounds)
+        self._contact_stiffness_bounds = bounds
 
     ###########
     # Methods #
     ###########
 
+    def _check_values(self, name, values):
+        """Check the given values to be set."""
+        if isinstance(values, (float, int)):
+            values = [values] * len(self.links)
+        elif not isinstance(values, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given '" + name + "' to be a list/tuple/np.array of float/int, but got "
+                            "instead: {}".format(type(values)))
+        if len(values) != len(self.links):
+            raise ValueError("The number of given '" + name + "' values (={}) does not match with the number of links "
+                             "(={})".format(len(values), len(self.links)))
+        return values
+
     def names(self):
         """Return an iterator over the property names."""
-        for name in ['mass']:
+        # 'local_inertia_position', 'local_inertia_orientation', 'linear_damping', 'angular_damping'
+        for name in ['mass', 'local_inertia_diagonal', 'lateral_friction', 'spinning_friction', 'rolling_friction',
+                     'restitution', 'contact_stiffness', 'contact_damping']:
             yield name
 
     def bounds(self):
         """Return an iterator over the bounds for each property."""
         yield self.mass_bounds
+        yield self.local_inertia_diagonal_bounds
+        yield self.lateral_friction_bounds
+        yield self.spinning_friction_bounds
+        yield self.rolling_friction_bounds
+        yield self.restitution_bounds
+        yield self.contact_stiffness_bounds
+        yield self.contact_damping_bounds
 
     def get_properties(self):
         """
-        Get the physics properties.
+        Get the current physics properties.
 
         Returns:
-            dict: current physic property values.
+            dict: current physical property values {physic property name: corresponding value}.
         """
-        properties = dict()
-        # properties['mass'] =
-        return properties
+        infos = [self.simulator.get_dynamics_info(body_id=self.body.id, link_id=link) for link in self.links]
+
+        return {link: {'mass': info[0], 'local_inertia_diagonal': info[2], 'local_inertia_position': info[3],
+                       'local_inertia_orientation': info[4], 'lateral_friction': info[1], 'spinning_friction': info[7],
+                       'rolling_friction': info[6], 'restitution': info[5], 'contact_stiffness': info[9],
+                       'contact_damping': info[8]}
+                for link, info in zip(self.links, infos)}
 
     def set_properties(self, properties):
         """
@@ -128,7 +399,38 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):
         Args:
             properties (dict): the physic property values to be set in the simulator.
         """
+        # check the given properties
         if not isinstance(properties, dict):
             raise TypeError("Expecting the given 'properties' to be a dictionary, instead got: "
                             "{}".format(type(properties)))
 
+        # set the properties in the simulator
+        if len(properties) > 0:
+            for link in self.links:
+                self.simulator.change_dynamics(self.body.id, link_id=link, **properties[link])
+
+    def sample(self, seed=None):
+        """
+        Sample a new set of physics properties and returns it. Note that it doesn't set them in the simulator.
+        This sampling can be useful if the user wishes to check more carefully the sampled physic property values.
+        Once satisfied, the user can set them by calling the `set_properties` method.
+
+        Note that it samples uniformly the physics properties between their specified lower and upper bounds.
+
+        Args:
+            seed (int, None): random seed.
+
+        Returns:
+            dict: sampled physic properties.
+        """
+        # set random seed
+        if seed is not None:
+            np.random.seed(seed)
+
+        # sample each property
+        properties = dict()
+        for link in self.links:
+            for name, bound in zip(self.names(), self.bounds()):
+                if bound is not None:
+                    properties.setdefault(link, {})[name] = np.random.uniform(low=bound[0], high=bound[1])
+        return properties
diff --git a/pyrobolearn/physics/physics_randomizer.py b/pyrobolearn/physics/physics_randomizer.py
index 942b816..16b36b6 100644
--- a/pyrobolearn/physics/physics_randomizer.py
+++ b/pyrobolearn/physics/physics_randomizer.py
@@ -11,8 +11,9 @@ import numpy as np
 
 from pyrobolearn.simulators import Simulator
 
+
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -25,18 +26,18 @@ class PhysicsRandomizer(object):
     r"""Physics Randomizer
 
     This the main abstract class from which all the physics randomizers inherit from. A physics randomizer randomize
-    the physics properties of a certain object. This can be,  for instance, the world, a robot, or a particular link
-    of that robot.
+    the physics properties of a certain object. This can be, for instance, the world, a robot, or a particular link /
+    joint of that robot.
 
     It can change for example the dynamics of a particular object such as the mass, inertia, or others. It can also
-    change their physical properties such as the friction, bounciness, etc.
+    change their physical properties such as the friction, bounciness (restitution coefficient), etc.
 
     Note that the physics randomizer instance has access to the simulator in order to modify the physical properties.
     Also, note that normally the physics randomizer is called at the beginning of an episode, and not at each time
     step. Changing the physical properties at each time step can lead to weird behaviors.
 
     It is possible to not randomize some physical properties by specifying a specific value instead of a range (=tuple
-    of 2 values; lower and upper bound).
+    of 2 values; lower and upper bound), or by setting None.
     """
 
     def __init__(self, simulator):
@@ -60,16 +61,35 @@ class PhysicsRandomizer(object):
     @simulator.setter
     def simulator(self, simulator):
         """Set the simulator instance."""
-        # TODO: uncomment the following lines
-        # if not isinstance(simulator, Simulator):
-        #     raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
-        #                     "{}".format(type(simulator)))
+        if not isinstance(simulator, Simulator):
+            raise TypeError("Expecting the given simulator to be an instance of `Simulator`, instead got: "
+                            "{}".format(type(simulator)))
         self._simulator = simulator
 
     ###########
     # Methods #
     ###########
 
+    @staticmethod
+    def _check_bounds(name, bounds):
+        """Check that the bounds are of the correct type and size.
+
+        Args:
+            name (str): name of the physical property
+            bounds (list/tuple of float, np.array[2, N], None): bounds. The first item is supposed to be the lower
+                bound and the second item the upper bound. If None, it doesn't do anything.
+        """
+        if bounds is not None:
+            # check bounds type and length
+            if not isinstance(bounds, (tuple, list, np.ndarray)):
+                raise TypeError("Expecting the given '" + name + "' to be a tuple/list/array of len(2) where the "
+                                "first item represents the lower bound, and the second item represents the upper "
+                                "bound, but got instead a type of: {}".format(type(bounds)))
+            if len(bounds) != 2:
+                raise ValueError("Expecting the given '" + name + "' to be a tuple/list/array of len(2) where the "
+                                 "first item represents the lower bound, and the second item represents the upper "
+                                 "bound, but got instead a length of: {}".format(len(bounds)))
+
     def properties(self):
         """Return an iterator over the properties."""
         properties = self.get_properties()
@@ -84,11 +104,13 @@ class PhysicsRandomizer(object):
 
     def names(self):
         """Return an iterator over the property names."""
-        pass
+        properties = self.get_properties()
+        for name in properties.keys():
+            yield name
 
     def bounds(self):
         """Return an iterator over the bounds for each property."""
-        pass
+        raise NotImplementedError
 
     def named_bounds(self):
         """Return an iterator over the property bounds with their name and value."""
@@ -97,16 +119,16 @@ class PhysicsRandomizer(object):
 
     def get_properties(self):
         """
-        Get the physics properties.
+        Get the physics properties. This method has to be implemented in the child class.
 
         Returns:
-            dict: current physic property values.
+            dict: current physic property values {physic property name: corresponding value}
         """
         pass
 
     def set_properties(self, properties):
         """
-        Set the given physic property values using the simulator.
+        Set the given physic property values using the simulator. This method has to be implemented in the child class.
 
         Args:
             properties (dict): the physic property values to be set in the simulator.
@@ -134,7 +156,8 @@ class PhysicsRandomizer(object):
         # sample each property
         properties = dict()
         for name, bound in zip(self.names(), self.bounds()):
-            properties[name] = np.random.uniform(low=bound[0], high=bound[1])
+            if bound is not None:
+                properties[name] = np.random.uniform(low=bound[0], high=bound[1])
         return properties
 
     def randomize(self, seed=None):
@@ -156,3 +179,19 @@ class PhysicsRandomizer(object):
         """
         if seed is not None:
             np.random.seed(seed)
+
+    #############
+    # Operators #
+    #############
+
+    def __str__(self):
+        """Return a string describing the class."""
+        return
+
+    def __call__(self, seed=None):
+        """Randomize the physics properties and set them in the simulator.
+
+        Args:
+            seed (int, None): random seed.
+        """
+        self.randomize(seed=seed)
diff --git a/pyrobolearn/physics/robot_physics_randomizer.py b/pyrobolearn/physics/robot_physics_randomizer.py
index 95a5f1a..01a7a8f 100644
--- a/pyrobolearn/physics/robot_physics_randomizer.py
+++ b/pyrobolearn/physics/robot_physics_randomizer.py
@@ -13,7 +13,7 @@ from pyrobolearn.physics.joint_physics_randomizer import JointPhysicsRandomizer
 
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
diff --git a/pyrobolearn/physics/world_physics_randomizer.py b/pyrobolearn/physics/world_physics_randomizer.py
index fc9d5ea..6a79142 100644
--- a/pyrobolearn/physics/world_physics_randomizer.py
+++ b/pyrobolearn/physics/world_physics_randomizer.py
@@ -13,7 +13,7 @@ from pyrobolearn.physics.physics_randomizer import PhysicsRandomizer
 
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
diff --git a/pyrobolearn/rewards/__init__.py b/pyrobolearn/rewards/__init__.py
index e401266..deb84bf 100644
--- a/pyrobolearn/rewards/__init__.py
+++ b/pyrobolearn/rewards/__init__.py
@@ -9,8 +9,13 @@ from .basic_rewards import *
 # import gym wrapper reward
 from .gym_reward import GymReward
 
+# import terminal rewards
+from .terminal_rewards import TerminalReward
+
 # import costs
 from .cost import *
+from .joint_cost import *
+from .link_cost import *
 
 # import processors
 from .processors import *
diff --git a/pyrobolearn/rewards/action_cost.py b/pyrobolearn/rewards/action_cost.py
new file mode 100644
index 0000000..457efbe
--- /dev/null
+++ b/pyrobolearn/rewards/action_cost.py
@@ -0,0 +1,55 @@
+#!/usr/bin/env python
+"""Define the costs used on actions.
+"""
+
+from abc import ABCMeta
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.rewards.cost import Cost
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ActionCost(Cost):
+    r"""(Abstract) Action Cost."""
+    __metaclass__ = ABCMeta
+    pass
+
+
+class ActionDifferenceCost(ActionCost):
+    r"""Action Difference Cost
+
+    This computes the difference between two actions:
+
+    .. math:: \text{cost} = || a_{t-1} - a_{t} ||^2
+
+    References:
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+    """
+
+    def __init__(self, action):
+        """
+        Initialize the action difference cost.
+
+        Args:
+            action (Action): action instance.
+        """
+        super(ActionDifferenceCost, self).__init__()
+        if not isinstance(action, prl.actions.Action):
+            raise TypeError("Expecting the given 'action' to be an instance of `Action`, instead got: "
+                            "{}".format(type(action)))
+        self.action = action
+
+    def _compute(self):
+        data = self.action.merged_data
+        prev_data = self.action.merged_data
+        return np.sum([- np.sum((curr - prev)**2) for curr, prev in zip(data, prev_data)])
diff --git a/pyrobolearn/rewards/cost.py b/pyrobolearn/rewards/cost.py
index 9e400dc..bb93c82 100644
--- a/pyrobolearn/rewards/cost.py
+++ b/pyrobolearn/rewards/cost.py
@@ -27,6 +27,16 @@ import pyrobolearn.actions as actions
 from pyrobolearn.rewards.reward import Reward
 
 
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
 # class Cost(Objective):
 class Cost(Reward):
     r"""Abstract `Cost` class which inherits from the `Objective` class, and is set to be minimized.
@@ -57,23 +67,6 @@ def logistic_kernel_function(error, alpha):
         return 1. / (np.exp(alpha * error) + 2. + np.exp(- alpha * error))
 
 
-def min_angle_difference(q1, q2):
-    r"""
-    Return the minimum angle difference between two angles.
-
-    Args:
-        q1 (float, np.array[N]): first angle(s)
-        q2 (float, np.array[N]): second angle(s)
-
-    Returns:
-        float, np.array[N]: minimum angle difference(s)
-    """
-    diff = np.maximum(q1, q2) - np.minimum(q1, q2)
-    if diff > np.pi:
-        diff = 2 * np.pi - diff
-    return diff
-
-
 class AngularVelocityErrorCost(Cost):
     r"""Angular Velocity Error Cost
 
@@ -140,26 +133,6 @@ class HeightCost(Cost):
         return 0
 
 
-class JointPositionErrorCost(Cost):
-    r"""Joint Position Error Cost
-
-    Return the joint position error as defined in [1] as :math:`d(\hat{\phi}, \phi) \in [0, \pi]` where :math:`d(.,.)`
-    is the minimum angle difference between two angles, and :math:`\hat{\phi}` and :math:`\phi` are the target and
-    current angles.
-
-    References:
-        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
-    """
-
-    def __init__(self, joint_state, target_joint_state):
-        super(JointPositionErrorCost, self).__init__()
-        self.state = joint_state
-        self.target_state = target_joint_state
-
-    def _compute(self):
-        return - min_angle_difference(self.state.data[0], self.target_state.data[0])
-
-
 class OrientationGravityCost(Cost):
     r"""Orientation Gravity Cost
 
@@ -176,81 +149,6 @@ class OrientationGravityCost(Cost):
         return np.linalg.norm(self.gravity_state.data - self.gravity)
 
 
-class JointPositionCost(Cost):
-    r"""Joint Position Cost
-
-    Return the cost such that measures the L2 norm between the current joint positions and the target joint positions:
-    :math:`||d(q_{target},q)||^2` where :math:`d(\cdot, \cdot) \in [-\pi, \pi]` is the minimum distance between two
-    angles.
-    """
-
-    def __init__(self, joint_position_state, target_joint_position):
-        r"""
-        Initialize the joint position cost.
-
-        Args:
-            joint_position_state (JointPositionState): joint position state.
-            target_joint_position (np.array[N]): target joint positions.
-        """
-        super(JointPositionCost, self).__init__()
-        if not isinstance(joint_position_state, prl.states.JointPositionState):
-            raise TypeError("Expecting the given 'joint_position_state' to be an instance of `JointPositionState`, "
-                            "but instead got: {}".format(type(joint_position_state)))
-        self.q = joint_position_state
-        self.target_q = target_joint_position
-
-    def _compute(self):
-        """Compute and return the cost value."""
-        return - np.sum(min_angle_difference(self.q.data[0], self.target_q)**2)
-
-
-class JointVelocityCost(Cost):
-    r"""Joint Velocity Cost
-
-    Return the cost due to the joint velocities: :math:`|| \dot{q} ||^2`
-    """
-    def __init__(self, joint_velocity_state):
-        """
-        Initialize the joint velocity cost.
-
-        Args:
-            joint_velocity_state (JointVelocityState): joint velocity state.
-        """
-        super(JointVelocityCost, self).__init__()
-        if not isinstance(joint_velocity_state, prl.states.JointVelocityState):
-            raise TypeError("Expecting the given 'joint_velocity_state' to be an instance of `JointVelocityState`, "
-                            "but instead got: {}".format(type(joint_velocity_state)))
-        self.dq = joint_velocity_state
-
-    def _compute(self):
-        """Compute and return the cost value."""
-        return - np.sum(self.dq.data[0] ** 2)
-
-
-class JointTorqueCost(Cost):
-    r"""Torque Cost
-
-    Return the cost due to the joint torques; :math:`|| \tau ||^2`.
-    """
-
-    def __init__(self, joint_torque_state):
-        """
-        Initialize the joint torque cost.
-
-        Args:
-            joint_torque_state (JointForceTorqueState): joint torque state.
-        """
-        super(JointTorqueCost, self).__init__()
-        if not isinstance(joint_torque_state, prl.states.JointForceTorqueState):
-            raise TypeError("Expecting the given 'joint_torque_state' to be an instance of `JointForceTorqueState`, "
-                            "but instead got: {}".format(type(joint_torque_state)))
-        self.tau = joint_torque_state
-
-    def _compute(self):
-        """Compute and return the cost value."""
-        return - np.sum(self.tau.data[0]**2)
-
-
 class PowerCost(Cost):
     r"""Power Consumption Cost
 
@@ -268,85 +166,6 @@ class PowerCost(Cost):
         return - np.sum(np.maximum(self.tau.data[0] * self.vel.data[0], 0))
 
 
-class JointPowerConsumptionCost(Cost):
-    r"""Joint Power Consumption Cost
-
-    Return the joint power consumption cost, where the power is computed as the torque times the velocity.
-    """
-
-    def __init__(self, state, joint_ids=None, update_state=False):
-        """
-        Initialize the Joint Power Consumption cost.
-
-        Args:
-            torque (Robot, State): robot instance, or the state. The state must contains the `JointForceTorqueState`
-                and the `JointVelocityState`.
-            joint_ids (None, int, list of int): joint ids. This used if `torque` is a `Robot` instance.
-            update_state (bool): If True, it will update the state.
-        """
-        self.update_state = update_state
-        if isinstance(state, Robot):
-            torque = states.JointForceTorqueState(state, joint_ids=joint_ids)
-            velocity = states.JointVelocityState(state, joint_ids=joint_ids)
-            self.update_state = True
-            # state = torque + velocity
-        elif isinstance(state, states.State):
-            # check if they have the correct state
-            torque = state.lookfor(states.JointForceTorqueState)
-            if torque is None:
-                raise ValueError("Didn't find a `JointForceTorqueState` instance in the given states.")
-            velocity = state.lookfor(states.JointVelocityState)
-            if velocity is None:
-                raise ValueError("Didn't find a `JointVelocityState` instance in the given states.")
-        else:
-            raise TypeError("Expecting the state to be an instance of `State` or `Robot`.")
-        super(JointPowerConsumptionCost, self).__init__()  # state=state)
-        self.tau = torque
-        self.vel = velocity
-        self.update_state = update_state
-
-    def compute(self):
-        if self.update_state:
-            self.tau()
-            self.vel()
-        return - np.sum((self.tau.data[0] * self.vel.data[0])**2)
-
-
-class JointAccelerationCost(Cost):
-    r"""Joint Acceleration Cost
-
-    Return the joint acceleration cost defined notably in [1] as :math:`cost = ||\ddot{q}||^2`
-
-    References:
-        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
-    """
-    def __init__(self, joint_acceleration_state):
-        super(JointAccelerationCost, self).__init__()
-        self.ddq = joint_acceleration_state
-
-    def compute(self):
-        return - np.sum(self.ddq.data**2)
-
-
-class JointSpeedCost(Cost):
-    r"""Joint Speed Cost
-
-    Return the joint speed cost as computed in [1].
-
-    References:
-        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
-    """
-    def __init__(self, joint_velocity_state, max_joint_speed=None):
-        super(JointSpeedCost, self).__init__()
-        self.dq = joint_velocity_state
-        self.dq_max = max_joint_speed
-        if max_joint_speed is None:
-            self.dq_max = joint_velocity_state.max
-
-    def compute(self):
-        return - np.sum(np.maximum(self.dq_max - np.abs(self.dq.data), 0)**2)
-
-
 class BodyImpulseCost(Cost):
     r"""Body Impulse Cost
 
@@ -425,20 +244,6 @@ class SelfCollisionCost(Cost):
         pass
 
 
-class ActionDifferenceCost(Cost):
-    r"""Action Difference Cost
-
-    References:
-        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
-    """
-    def __init__(self, action):
-        super(ActionDifferenceCost, self).__init__()
-        self.action = action
-
-    def compute(self):
-        return - (self.action.data - self.action.prev_data)**2
-
-
 class PhysicsViolationCost(Cost):
     """Physics Violation Cost.
 
@@ -470,81 +275,16 @@ class ContactInvariantCost(Cost):
         super(ContactInvariantCost, self).__init__()
 
 
-class DistanceCost(Cost):
-    """Distance Cost.
-
-    It penalizes the distance between 2 objects. One of the 2 objects must be movable in order for this
-    cost to change.
-
-    Mathematically, the cost is given by:
-
-    .. math:: c(l1, l2) = d(l1, l2) = - || l1 - l2 ||_2
-
-    where :math:`l1` represents a link attached to the first body, and :math:`l2` represents a link attached on the
-    second body. The distance function used is the Euclidean distance (=L2 norm).
-    """
-
-    def __init__(self, body1, body2, link_id1=-1, link_id2=-1, offset=None):
-        r"""
-        Initialize the distance cost.
-
-        Args:
-            body1 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): first position state. If
-                Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
-                `LinkPositionState` depending on the value of :attr:`link_id1`.
-            body2 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): second position state. If
-                Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
-                `LinkPositionState` depending on the value of :attr:`link_id1`.
-            link_id1 (int): link id associated with the first body that we are interested in. This is only used if
-                the given :attr:`body1` is not a state.
-            link_id2 (int): link id associated with the second body that we are interested in. This is only used if
-                the given :attr:`body2` is not a state.
-            offset (None, np.array[3]): 3d offset between body1 and body2.
-        """
-        super(DistanceCost, self).__init__()
-
-        def check_body_type(body, id_, link_id):
-            update_state = False
-            if isinstance(body, prl.robots.Body):
-                body = states.PositionState(body)
-                update_state = True
-            elif isinstance(body, Robot):
-                if link_id == -1:
-                    body = states.PositionState(body)
-                else:
-                    body = states.LinkWorldPositionState(body, link_ids=link_id)
-                update_state = True
-            elif not isinstance(body, (states.BasePositionState, states.PositionState, states.LinkWorldPositionState)):
-                raise TypeError("Expecting the given 'body"+str(id_)+"' to be an instance of `Body`, `Robot`, "
-                                "`BasePositionState`, `PositionState` or `LinkWorldPositionState`, instead got: "
-                                "{}".format(type(body), id_))
-            return body, update_state
-
-        self.body1, self.update_state1 = check_body_type(body1, id_=1, link_id=link_id1)
-        self.body2, self.update_state2 = check_body_type(body2, id_=2, link_id=link_id2)
-
-    def compute(self):
-        if self.update_state1:
-            self.body1()
-        if self.update_state2:
-            self.body2()
-        p1 = self.body1.data[0]
-        p2 = self.body2.data[0]
-        # print("P1: {}".format(p1))
-        # print("P2: {}".format(p2))
-        return - np.linalg.norm(p1 - p2)
-
-
 class ImpactCost(Cost):
     """Impact cost.
 
     Calculates the impact force using the kinetic energy.
     """
 
-    def __init__(self):
+    def __init__(self, body1, body2):
         super(ImpactCost, self).__init__()
 
-    def compute(self, object1, object2):
+    def _compute(self):
         pass
 
 
@@ -554,10 +294,10 @@ class DriftCost(Cost):
     Calculates the drift of a moving object wrt a direction.
     """
 
-    def __init__(self):
+    def __init__(self, body, direction):
         super(DriftCost, self).__init__()
 
-    def compute(self, object, direction):
+    def _compute(self):
         pass
 
 
@@ -566,10 +306,10 @@ class ShakeCost(Cost):
 
     Calculates the
     """
-    def __init__(self):
+    def __init__(self, body, direction):
         super(ShakeCost, self).__init__()
 
-    def compute(self, object, direction):
+    def _compute(self):
         pass
 
 
diff --git a/pyrobolearn/rewards/joint_cost.py b/pyrobolearn/rewards/joint_cost.py
new file mode 100644
index 0000000..1504d9f
--- /dev/null
+++ b/pyrobolearn/rewards/joint_cost.py
@@ -0,0 +1,358 @@
+#!/usr/bin/env python
+"""Define the costs used on joint states / actions.
+"""
+
+from abc import ABCMeta
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.rewards.cost import Cost
+from pyrobolearn.utils.transformation import min_angle_difference
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class JointCost(Cost):
+    r"""(Abstract) Joint Cost."""
+    __metaclass__ = ABCMeta
+
+    def __init__(self, update_state=False):
+        """
+        Initialize the abstract joint cost.
+
+        Args:
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(JointCost, self).__init__()
+        self.update_state = update_state
+
+    @staticmethod
+    def _check_state(state, cls, update_state=False):
+        # check given state
+        if isinstance(state, prl.robots.Robot):  # if robot, instantiate state class with robot as param.
+            state = cls(robot=state)
+            update_state = True
+        if not isinstance(state, cls):  # if not an instance of the given state, class, raise error
+            raise TypeError("Expecting the given 'state' to be an instance of `Robot` or `" + cls.__name__ + "`, "
+                            "but instead got: {}".format(type(state)))
+        return state, update_state
+
+    @staticmethod
+    def _check_target_state(state, target_state, cls, update_state=False):
+        # check given target state
+        if target_state is None:  # if the target is None, initialize it zero
+            target_state = np.zeros(state.total_size())
+        if isinstance(target_state, (int, float, np.ndarray)):  # if target is a np.array/float/int, create FixedState
+            target_state = prl.states.FixedState(value=target_state)
+            update_state = True
+        elif isinstance(target_state, prl.robots.Robot):  # if robot, instantiate state class with robot as param.
+            target_state = cls(robot=target_state)
+            update_state = True
+        elif not isinstance(target_state, cls):  # if not an instance of the given state class, raise error
+            raise TypeError("Expecting the given 'target_state' to be None, a np.array, or an instance of "
+                            "`Robot` or `" + cls.__name__ + "`, but instead got: {}".format(type(target_state)))
+        return target_state, update_state
+
+
+# class JointPositionErrorCost(JointCost):
+#     r"""Joint Position Error Cost
+#
+#     Return the joint position error as defined in [1] as :math:`d(\hat{\phi}, \phi) \in [0, \pi]` where :math:`d(.,.)`
+#     is the minimum angle difference between two angles, and :math:`\hat{\phi}` and :math:`\phi` are the target and
+#     current angles.
+#
+#     References:
+#         - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+#     """
+#
+#     def __init__(self, joint_state, target_joint_state, update_state=False):
+#         """
+#         Initialize the joint position error cost.
+#
+#         Args:
+#             joint_state (JointPositionState):
+#             target_joint_state (JointPositionState):
+#             update_state (bool): if True it will update the given states before computing the cost.
+#         """
+#         super(JointPositionErrorCost, self).__init__(update_state=update_state)
+#         self.state = joint_state
+#         self.target_state = target_joint_state
+#
+#     def _compute(self):
+#         return - min_angle_difference(self.state.data[0], self.target_state.data[0])
+
+
+class JointPositionCost(JointCost):
+    r"""Joint Position Cost
+
+    Return the cost such that measures the L2 norm between the current joint positions and the target joint positions:
+    :math:`||d(q_{target},q)||^2` where :math:`d(\cdot, \cdot) \in [-\pi, \pi]` is the minimum distance between two
+    angles.
+
+    References:
+        - [1] OpenAI Gym
+        - [2] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+    """
+
+    def __init__(self, state, target_state, update_state=False):
+        r"""
+        Initialize the joint position cost.
+
+        Args:
+            state (JointPositionState, Robot): joint position state.
+            target_state (JointPositionState, np.array[N], None): target joint position state. If None, it will be set
+                to 0.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(JointPositionCost, self).__init__(update_state)
+
+        # check given joint position state
+        self.q, self.update_state = self._check_state(state, prl.states.JointPositionState,
+                                                      update_state=self.update_state)
+
+        # check target joint position state
+        self.q_target, self.update_target_state = self._check_target_state(self.q, target_state,
+                                                                           prl.states.JointPositionState,
+                                                                           self.update_state)
+
+        if self.q.total_size() != self.q_target.total_size():
+            raise ValueError("The given state and target_state do not have the same size: "
+                             "{} != {}".format(self.q.total_size(), self.q_target.total_size()))
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        if self.update_state:
+            self.q()
+        if self.update_target_state:
+            self.q_target()
+        return - np.sum(min_angle_difference(self.q.data[0], self.q_target.data[0])**2)
+
+
+class JointVelocityCost(JointCost):
+    r"""Joint Velocity Cost
+
+    Return the cost due to the joint velocities: :math:`|| \dot{q}_{target} - \dot{q} ||^2`, where
+    :math:`\dot{q}_{target}` can be set to zero if wished.
+    """
+
+    def __init__(self, state, target_state=None, update_state=False):
+        """
+        Initialize the joint velocity cost.
+
+        Args:
+            state (JointVelocityState, Robot): joint velocity state.
+            target_state (JointVelocityState, np.array[N], Robot, None): target joint velocity state. If None, it
+                will be set to 0.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(JointVelocityCost, self).__init__(update_state)
+
+        # check given joint velocity state
+        self.dq, self.update_state = self._check_state(state, prl.states.JointVelocityState,
+                                                       update_state=self.update_state)
+
+        # check target joint velocity state
+        self.dq_target, self.update_target_state = self._check_target_state(self.dq, target_state,
+                                                                            prl.states.JointVelocityState,
+                                                                            self.update_state)
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        if self.update_state:
+            self.dq()
+        if self.update_target_state:
+            self.dq_target()
+        return - np.sum((self.dq_target.data[0] - self.dq.data[0])**2)
+
+
+class JointAccelerationCost(JointCost):
+    r"""Joint Acceleration Cost
+
+    Return the joint acceleration cost defined notably in [1] as :math:`cost = || \ddot{q}_{target} - \ddot{q} ||^2`,
+    where :math:`\ddot{q}_{target}` can be set to zero if wished.
+
+    References:
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+    """
+
+    def __init__(self, state, target_state=None, update_state=False):
+        """
+        Initialize the joint acceleration cost.
+
+        Args:
+            state (JointAccelerationState, Robot): joint acceleration state.
+            target_state (JointAccelerationState, np.array[N], Robot, None): target joint acceleration state. If None,
+                it will be set to 0.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(JointAccelerationCost, self).__init__(update_state=update_state)
+
+        # check given joint acceleration state
+        self.ddq, self.update_state = self._check_state(state, prl.states.JointAccelerationState,
+                                                        update_state=self.update_state)
+
+        # check target joint acceleration state
+        self.ddq_target, self.update_target_state = self._check_target_state(self.ddq, target_state,
+                                                                             prl.states.JointAccelerationState,
+                                                                             self.update_state)
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        if self.update_state:
+            self.ddq()
+        if self.update_target_state:
+            self.ddq_target()
+        return - np.sum((self.ddq_target.data[0] - self.ddq.data[0])**2)
+
+
+class JointTorqueCost(JointCost):
+    r"""Torque Cost
+
+    Return the cost due to the joint torques; :math:`|| \tau_{target} - \tau ||^2`, where :math:`\tau_{target}` can
+    be set to zero if wished.
+    """
+
+    def __init__(self, state, target_state=None, update_state=False):
+        """
+        Initialize the joint torque cost.
+
+        Args:
+            state (JointForceTorqueState, Robot): joint torque state.
+            target_state (JointForceTorqueState, np.array[N], Robot, None): target joint torque state. If None, it
+                will be set to 0.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(JointTorqueCost, self).__init__(update_state)
+
+        # check given joint torque state
+        self.tau, self.update_state = self._check_state(state, prl.states.JointForceTorqueState,
+                                                        update_state=self.update_state)
+
+        # check target joint torque state
+        self.tau_target, self.update_target_state = self._check_target_state(self.tau, target_state,
+                                                                             prl.states.JointForceTorqueState,
+                                                                             self.update_state)
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        if self.update_state:
+            self.tau()
+        if self.update_target_state:
+            self.tau_target()
+        return - np.sum((self.tau_target.data[0] - self.tau.data[0])**2)
+
+
+class JointPowerCost(JointCost):
+    r"""Joint Power Consumption Cost
+
+    Return the joint power consumption cost, where the power is computed as the torque times the velocity.
+    """
+
+    def __init__(self, state, joint_ids=None, update_state=False):
+        """
+        Initialize the Joint Power Consumption cost.
+
+        Args:
+            state (Robot, State): robot instance, or the state. The state must contains the `JointForceTorqueState`
+                and the `JointVelocityState`. Note that if they are multiple torque or velocity states, it will look
+                for the first instance.
+            joint_ids (None, int, list of int): joint ids. This used if `torque` is a `Robot` instance.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        self.update_state = update_state
+
+        # Check the state
+        # if the given state is a robot, create the torque and velocity states
+        if isinstance(state, prl.robots.Robot):
+            torque = prl.states.JointForceTorqueState(state, joint_ids=joint_ids)
+            velocity = prl.states.JointVelocityState(state, joint_ids=joint_ids)
+            self.update_state = True
+            # state = torque + velocity
+
+        # elif the given state is a composite state, look for the torque and velocity states.
+        else:
+
+            if isinstance(state, prl.states.State):
+                state = [state]
+
+            # if the given state is a list of states, check each one of them by looking for the torque/velocity state
+            if isinstance(state, (list, tuple)):
+                # for each state, check if it is a torque, velocity or composite state
+                torque, velocity = None, None
+                for s in state:
+                    if isinstance(s, prl.states.JointForceTorqueState):
+                        if torque is None:
+                            torque = s
+                    elif isinstance(s, prl.states.JointVelocityState):
+                        if velocity is None:
+                            velocity = s
+                    elif isinstance(s, prl.states.State):
+                        if torque is None:
+                            torque = s.lookfor(prl.states.JointForceTorqueState)
+                        if velocity is None:
+                            velocity = s.lookfor(prl.states.JointVelocityState)
+                    else:
+                        raise TypeError("Expecting the state to be an instance of `State` or `Robot`, or a list of "
+                                        "`State`, instead got: {}".format(type(s)))
+                    # if we have found the states, get out of the loop
+                    if torque is not None and velocity is not None:
+                        break
+
+                # check that we have the torque and velocity states
+                if torque is None:
+                    raise ValueError("Didn't find a `JointForceTorqueState` instance in the given states.")
+                if velocity is None:
+                    raise ValueError("Didn't find a `JointVelocityState` instance in the given states.")
+            else:
+                raise TypeError("Expecting the state to be an instance of `State` or `Robot`, instead got: "
+                                "{}".format(type(state)))
+
+        super(JointPowerCost, self).__init__()
+        self.tau = torque
+        self.vel = velocity
+        self.update_state = update_state
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        if self.update_state:
+            self.tau()
+            self.vel()
+        return - np.sum((self.tau.data[0] * self.vel.data[0])**2)
+
+
+# class JointSpeedCost(Cost):
+#     r"""Joint Speed Cost
+#
+#     Return the joint speed cost as computed in [1].
+#
+#     .. math:: \text{cost} = || \max(\dot{q}_{max} - |\dot{q}|, 0) ||^2
+#
+#     References:
+#         - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+#     """
+#
+#     def __init__(self, state, max_joint_speed=None):
+#         """
+#         Initialize the joint speed state.
+#
+#         Args:
+#             state: joint velocity state.
+#             max_joint_speed:
+#         """
+#         super(JointSpeedCost, self).__init__()
+#         self.dq = state
+#         self.dq_max = max_joint_speed
+#         if max_joint_speed is None:
+#             self.dq_max = state.max
+#
+#     def _compute(self):
+#         """Compute and return the cost value."""
+#         return - np.sum(np.maximum(self.dq_max - np.abs(self.dq.data[0]), 0)**2)
diff --git a/pyrobolearn/rewards/link_cost.py b/pyrobolearn/rewards/link_cost.py
new file mode 100644
index 0000000..a118177
--- /dev/null
+++ b/pyrobolearn/rewards/link_cost.py
@@ -0,0 +1,195 @@
+#!/usr/bin/env python
+"""Define the costs used on link states / actions.
+"""
+
+from abc import ABCMeta
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.rewards.cost import Cost
+from pyrobolearn.utils.transformation import quaternion_distance
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class LinkCost(Cost):
+    r"""(Abstract) Link Cost."""
+    __metaclass__ = ABCMeta
+
+    def __init__(self, update_state):
+        """
+        Initialize the abstract link cost.
+
+        Args:
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(LinkCost, self).__init__()
+        self.update_state = update_state
+
+
+class DistanceCost(LinkCost):
+    """Distance Cost.
+
+    It penalizes the distance between 2 objects. One of the 2 objects must be movable in order for this
+    cost to change.
+
+    Mathematically, the cost is given by:
+
+    .. math:: c(p_1, p_2) = - d(p_1, p_2) = - || p_2 - p_1 ||^2
+
+    where :math:`p_1` represents the position of the specified link attached to the first body with respect to a
+    frame, and :math:`p_2` represents the position of the specified link attached on the second body with respect to
+    that same frame. The distance function used is the Euclidean distance (=L2 norm).
+    """
+
+    def __init__(self, body1, body2, link_id1=-1, link_id2=-1, offset=None, update_state=False):
+        r"""
+        Initialize the distance cost.
+
+        Args:
+            body1 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): first position state. If
+                Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
+                `LinkPositionState` depending on the value of :attr:`link_id1`.
+            body2 (BasePositionState, PositionState, LinkWorldPositionState, Body, Robot): second position state. If
+                Body, it will wrap it with a `PositionState`. If Robot, it will wrap it with a `PositionState` or
+                `LinkPositionState` depending on the value of :attr:`link_id1`.
+            link_id1 (int): link id associated with the first body that we are interested in. This is only used if
+                the given :attr:`body1` is not a state.
+            link_id2 (int): link id associated with the second body that we are interested in. This is only used if
+                the given :attr:`body2` is not a state.
+            offset (None, np.array[3]): 3d offset between body1 and body2.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(DistanceCost, self).__init__(update_state=update_state)
+
+        # check body type function
+        def check_body_type(body, id_, link_id):
+            update_state = False
+            if isinstance(body, prl.robots.Body):
+                body = prl.states.PositionState(body)
+                update_state = True
+            elif isinstance(body, prl.robots.Robot):
+                if link_id == -1:
+                    body = prl.states.PositionState(body)
+                else:
+                    body = prl.states.LinkWorldPositionState(body, link_ids=link_id)
+                update_state = True
+            elif not isinstance(body, (prl.states.BasePositionState, prl.states.PositionState,
+                                       prl.states.LinkWorldPositionState)):
+                raise TypeError("Expecting the given 'body"+str(id_)+"' to be an instance of `Body`, `Robot`, "
+                                "`BasePositionState`, `PositionState` or `LinkWorldPositionState`, instead got: "
+                                "{}".format(type(body), id_))
+            return body, update_state
+
+        self.p1, self.update_state1 = check_body_type(body1, id_=1, link_id=link_id1)
+        self.p2, self.update_state2 = check_body_type(body2, id_=2, link_id=link_id2)
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        if self.update_state1:
+            self.p1()
+        if self.update_state2:
+            self.p2()
+        return - np.sum((self.p1.data[0] - self.p2.data[0])**2)
+
+
+# alias
+PositionCost = DistanceCost
+
+
+class OrientationCost(LinkCost):
+    r"""Orientation Cost
+
+    The orientation cost (which uses the distance between two quaternions :math:`q_1` and :math:`q_2`) is given by:
+
+    .. math::
+
+        c(q_1, q_2) = \left\{ \begin{array}{ll}
+                2\pi,                           & q1 * \bar{q}_2 = -1 + [0,0,0]^\top \\
+                2 || \log(q_1 * \bar{q}_2) ||,  & \text{otherwise}
+            \end{array} \right.
+
+    where :math:`*` is the quaternion product, :math:`\bar{q}` is the conjugate of the quaternion,
+    :math:`-1 + [0,0,0]^\top` is the only singularity on :math:`\mathbb{S}^3`, and
+    :math:`\log: \mathbb{S}^3 \rightarrow \mathbb{R}^3` is the logarithm map.
+    """
+
+    def __init__(self, state, target_state, update_state=False):
+        """
+        Initialize the orientation cost.
+
+        Args:
+            state (OrientationState, BaseOrientationState, LinkWorldOrientationState, LinkOrientationState, Body,
+                Robot): the orientation state.
+            target_state (np.array[4], OrientationState, BaseOrientationState, LinkWorldOrientationState,
+                LinkOrientationState, Body, Robot, None): target orientation state. Note that if a np.array is given,
+                it will wrap it with the `FixedState`. If None, it will be initialize to the unit quaternion.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        super(OrientationCost, self).__init__(update_state=update_state)
+        # TODO
+        self.q1 = state
+        self.q2 = target_state
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        return - quaternion_distance(self.q1.data[0], self.q2.data[0])
+
+
+class LinearVelocityCost(LinkCost):
+    r"""Linear velocity cost
+
+    The linear velocity cost is expressed as:
+
+    .. math:: c(v_1, v_2) = || v_2 - v_1 ||^2
+
+    """
+
+    def __init__(self, state, target_state, update_state=False):
+        """
+        Initialize the linear velocity cost.
+
+        Args:
+            state (LinearVelocityState): the linear velocity state.
+            target_state (LinearVelocityState, np.array[3], None): target linear velocity state. Note that if a
+                np.array is given, it will wrap it with the `FixedState`. If None, it will be initialize to zeros.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        raise NotImplementedError
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        pass
+
+
+class AngularVelocityCost(LinkCost):
+    r"""Angular velocity cost
+
+    The angular velocity cost is expressed as:
+
+    .. math:: c(\omega_1, \omega_2) = || \omega_2 - \omega_1 ||^2
+
+    """
+
+    def __init__(self, state, target_state, update_state=False):
+        """
+        Initialize the angular velocity cost.
+
+        Args:
+            state (LinearVelocityState): the angular velocity state.
+            target_state (LinearVelocityState, np.array[3], None): target angular velocity state. Note that if a
+                np.array is given, it will wrap it with the `FixedState`. If None, it will be initialize to zeros.
+            update_state (bool): if True it will update the given states before computing the cost.
+        """
+        raise NotImplementedError
+
+    def _compute(self):
+        """Compute and return the cost value."""
+        pass
diff --git a/pyrobolearn/rewards/terminal_rewards.py b/pyrobolearn/rewards/terminal_rewards.py
new file mode 100644
index 0000000..7d1c374
--- /dev/null
+++ b/pyrobolearn/rewards/terminal_rewards.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python
+"""Define terminal rewards used in RL.
+"""
+
+import numpy as np
+
+from pyrobolearn.rewards.reward import Reward
+from pyrobolearn.rewards.basic_rewards import FixedReward
+from pyrobolearn.terminal_conditions.terminal_condition import TerminalCondition
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class TerminalReward(Reward):
+    r"""Terminal reward.
+
+    This computes the provided subreward until the terminal condition is not fulfilled. Once it has achieved it, it
+    computes the final given reward function. This reward is useful specially for sparse rewards that only returns a
+    value once the goal has been achieved (e.g. games).
+    """
+
+    def __init__(self, terminal_condition, subreward, final_reward):
+        r"""
+        Terminal reward.
+
+        Args:
+            terminal_condition (TerminalCondition): terminal condition.
+            subreward (Reward, float, int): sub reward that is called until the terminal condition is not fulfilled.
+            final_reward (Reward, float, int): final reward that is called when the terminal condition has been reached.
+        """
+        super(TerminalReward, self).__init__()
+
+        # set the attributes
+        self.terminal_condition = terminal_condition
+        self.subreward = subreward
+        self.final_reward = final_reward
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def terminal_condition(self):
+        """Return the terminal condition instance."""
+        return self._terminal_condition
+
+    @terminal_condition.setter
+    def terminal_condition(self, condition):
+        """Set the terminal condition instance."""
+        if not isinstance(condition, TerminalCondition):
+            raise TypeError("Expecting the given 'terminal_condition' to be an instance of `TerminalCondition`, "
+                            "instead got: {}".format(type(condition)))
+        self._terminal_condition = condition
+
+    @property
+    def subreward(self):
+        """Return the sub-reward instance."""
+        return self._reward
+
+    @subreward.setter
+    def subreward(self, reward):
+        """Set the sub-reward instance."""
+        if isinstance(reward, (int, float)):
+            reward = FixedReward(value=reward)
+        elif not isinstance(reward, Reward):
+            raise TypeError("Expecting the given 'subreward' to be an instance of `Reward`, instead got: "
+                            "{}".format(type(reward)))
+        self._reward = reward
+
+    @property
+    def final_reward(self):
+        """Return the final reward instance."""
+        return self._final_reward
+
+    @final_reward.setter
+    def final_reward(self, reward):
+        """Set the final reward instance."""
+        if isinstance(reward, (int, float)):
+            reward = FixedReward(value=reward)
+        elif not isinstance(reward, Reward):
+            raise TypeError("Expecting the given 'final_reward' to be an instance of `Reward`, instead got: "
+                            "{}".format(type(reward)))
+        self._final_reward = reward
+
+    ###########
+    # Methods #
+    ###########
+
+    def _compute(self):
+        """Compute the terminal reward."""
+        done = self.terminal_condition()
+        if done:
+            return self.final_reward()
+        return self.subreward()
diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py
index e893596..a59c248 100644
--- a/pyrobolearn/robots/robot.py
+++ b/pyrobolearn/robots/robot.py
@@ -99,7 +99,7 @@ class Robot(ControllableBody):
             # we rescale manually the mass and inertia matrices of each link
             for link in range(self.num_links):
                 info = self.sim.get_dynamics_info(self.id, link)
-                mass, local_inertia_diagonal = info[0], np.array(info[2])
+                mass, local_inertia_diagonal = info[0], np.asarray(info[2])
                 mass *= scale ** 3      # because the density is unchanged when scaling
                 local_inertia_diagonal *= scale ** 5   # 5 = 3+2; 3 is for the mass, and 2 is for the distance: I~mr^2
                 self.sim.change_dynamics(self.id, link, mass=mass, local_inertia_diagonal=local_inertia_diagonal)
@@ -1423,7 +1423,7 @@ class Robot(ControllableBody):
         return self.sim.get_link_states(self.id, link_ids, compute_velocity=compute_link_velocity,
                                         compute_forward_kinematics=compute_forward_kinematics)
 
-    def get_link_local_position(self, link_ids=None):
+    def get_link_local_positions(self, link_ids=None):
         """
         Get the local position offset of the inertial frame (CoM) of the specified links expressed in the URDF link
         frame.
@@ -1496,7 +1496,7 @@ class Robot(ControllableBody):
             return self.sim.get_dynamics_info(self.id, link_ids)[0]
         if link_ids is None:
             link_ids = list(range(self.num_links))
-        return np.array([self.sim.get_dynamics_info(self.id, link)[0] for link in link_ids])
+        return np.asarray([self.sim.get_dynamics_info(self.id, link)[0] for link in link_ids])
 
     def get_link_frames(self, link_ids=None, flatten=False):
         r"""
@@ -1535,10 +1535,10 @@ class Robot(ControllableBody):
                 np.array[N*3], 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])
+            return np.asarray(self.sim.get_link_state(self.id, link_ids)[4])
         if link_ids is None:
             link_ids = self.joints
-        pos = np.array([self.sim.get_link_state(self.id, link)[4] for link in link_ids])
+        pos = np.asarray([self.sim.get_link_state(self.id, link)[4] for link in link_ids])
         if flatten:
             return pos.reshape(-1)  # 1D array
         return pos  # 2D array
@@ -1562,7 +1562,7 @@ class Robot(ControllableBody):
             return self.sim.get_link_state(self.id, link_ids)[5]
         if link_ids is None:
             link_ids = self.joints
-        orientation = np.array([self.sim.get_link_state(self.id, link)[5] for link in link_ids])
+        orientation = np.asarray([self.sim.get_link_state(self.id, link)[5] for link in link_ids])
         if flatten:
             return orientation.reshape(-1)  # 1D array
         return orientation  # 2D array
@@ -1649,7 +1649,7 @@ class Robot(ControllableBody):
             return self.sim.get_link_state(self.id, link_ids)[1]
         if link_ids is None:
             link_ids = self.joints
-        orientation = np.array([self.sim.get_link_state(self.id, link)[1] for link in link_ids])
+        orientation = np.asarray([self.sim.get_link_state(self.id, link)[1] for link in link_ids])
         if flatten:
             return orientation.reshape(-1)
         return orientation  # 2D array
@@ -1677,7 +1677,8 @@ class Robot(ControllableBody):
             if isinstance(wrt_link_id, int):
                 q0 = get_quaternion_inverse(self.get_link_world_orientations(wrt_link_id))
             else:
-                q0 = np.array([get_quaternion_inverse(self.get_link_world_orientations(link)) for link in wrt_link_id])
+                q0 = np.asarray([get_quaternion_inverse(self.get_link_world_orientations(link))
+                                 for link in wrt_link_id])
 
         q = get_quaternion_product(q0, q1)
         if flatten:
@@ -1728,10 +1729,10 @@ class Robot(ControllableBody):
                 np.array[N*3], 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])
+            return np.asarray(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[6])
         if link_ids is None:
             link_ids = self.joints
-        vel = np.array([self.sim.get_link_state(self.id, link, compute_velocity=True)[6] for link in link_ids])
+        vel = np.asarray([self.sim.get_link_state(self.id, link, compute_velocity=True)[6] for link in link_ids])
         if flatten:
             return vel.reshape(-1)  # 1D array
         return vel  # 2D array
@@ -1752,10 +1753,10 @@ class Robot(ControllableBody):
                 np.array[N*3], 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])
+            return np.asarray(self.sim.get_link_state(self.id, link_ids, compute_velocity=True)[7])
         if link_ids is None:
             link_ids = self.joints
-        vel = np.array([self.sim.get_link_state(self.id, link, compute_velocity=True)[7] for link in link_ids])
+        vel = np.asarray([self.sim.get_link_state(self.id, link, compute_velocity=True)[7] for link in link_ids])
         if flatten:
             return vel.reshape(-1)  # 1d array
         return vel  # 2D array
@@ -2152,7 +2153,7 @@ class Robot(ControllableBody):
             return self.sim.get_dynamics_info(body_id=self.id, link_id=link_ids)[2]
         if link_ids is None:
             link_ids = list(range(self.num_links))
-        return np.array([self.sim.get_dynamics_info(self.id, link)[2] for link in link_ids])
+        return np.asarray([self.sim.get_dynamics_info(self.id, link)[2] for link in link_ids])
 
     def set_link_positions(self, link_ids, positions, orientations=None):
         """
@@ -3135,7 +3136,7 @@ class Robot(ControllableBody):
             q = self.get_joint_positions()
 
         # compute and return joint accelerations
-        torques = np.array(torques)
+        torques = np.asarray(torques)
         if not self.fixed_base:  # if floating base
             torques = np.concatenate((np.zeros(6), torques))
         Hinv = np.linalg.inv(self.get_mass_matrix(q))
@@ -3175,8 +3176,8 @@ class Robot(ControllableBody):
         q_aug[self.joints] = q
 
         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]
+            return np.asarray(self.sim.calculate_mass_matrix(self.id, q_aug))
+        return np.asarray(self.sim.calculate_mass_matrix(self.id, q_aug))[q_idx, q_idx]
 
     # alias
     get_inertia_matrix = get_mass_matrix
@@ -3267,7 +3268,7 @@ class Robot(ControllableBody):
         H = self.get_mass_matrix(q, q_idx)
         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))):
+    def get_gravity_potential_energy(self, q=None, q_idx=None, g=(0., 0., -9.81)):
         r"""
         Return the potential energy due to gravity.
 
@@ -3281,11 +3282,12 @@ class Robot(ControllableBody):
                 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.array[3]): gravity vector.
+            g (np.array[3], tuple/list of 3 float): gravity vector.
 
         Returns:
             float: potential energy due to gravity
         """
+        g = np.asarray(g)
         link_ids = list(range(self.num_links))
         p = self.get_link_world_positions(link_ids=link_ids, flatten=False)
         m = self.get_link_masses(link_ids=link_ids)
diff --git a/pyrobolearn/simulators/dart.py b/pyrobolearn/simulators/dart.py
index 801c5a0..e9bd325 100644
--- a/pyrobolearn/simulators/dart.py
+++ b/pyrobolearn/simulators/dart.py
@@ -26,12 +26,15 @@ References:
         - paper: http://joss.theoj.org/papers/10.21105/joss.00500
         - webpage: https://dartsim.github.io/
         - github: https://github.com/dartsim/dart/
+        - dartpy: http://dartsim.github.io/install_dartpy_on_ubuntu.html
     [2] PyDART
         - source code: https://pydart2.readthedocs.io/en/latest/
         - documentation: https://pydart2.readthedocs.io/en/latest/
     [3] PEP8: https://www.python.org/dev/peps/pep-0008/
 """
 
+# TODO: finish to implement this interface and use dartpy instead of pydart2
+
 # import pydart2
 try:
     import pydart2 as pydart
@@ -68,6 +71,7 @@ class Dart(Simulator):
         [1] Dart:
             - webpage: https://dartsim.github.io/
             - github: https://github.com/dartsim/dart/
+            - dartpy: http://dartsim.github.io/install_dartpy_on_ubuntu.html
         [2] PyDART:
             - source code: https://pydart2.readthedocs.io/en/latest/
             - documentation: https://pydart2.readthedocs.io/en/latest/
diff --git a/pyrobolearn/states/__init__.py b/pyrobolearn/states/__init__.py
index db6e424..83f5dc7 100644
--- a/pyrobolearn/states/__init__.py
+++ b/pyrobolearn/states/__init__.py
@@ -19,10 +19,10 @@ from .gym_states import *
 
 
 # # import state generators
-# from .generators import *
-#
+from . import generators
+
 # # import state processors
-# from .processors import *
+# from . import processors
 #
 # # import interface states
 # from .interfaces import *
diff --git a/pyrobolearn/states/generators/state_generator.py b/pyrobolearn/states/generators/state_generator.py
index 9146fcd..3d39940 100644
--- a/pyrobolearn/states/generators/state_generator.py
+++ b/pyrobolearn/states/generators/state_generator.py
@@ -13,6 +13,7 @@ See Also:
 
 import queue
 import numpy as np
+from abc import ABCMeta
 
 from pyrobolearn.states import State
 
@@ -42,13 +43,15 @@ class StateGenerator(object):
     The mapping function has to return a `state` object.
     """
 
-    def __init__(self, state):
+    def __init__(self, state, fct=None):
         """Initialize the state generator.
 
         Args:
             state (State): state instance.
+            fct (callable, None): callback function to be called after generating the data.
         """
         self.state = state
+        self.fct = fct if callable(fct) else None
 
     @property
     def state(self):
@@ -63,24 +66,48 @@ class StateGenerator(object):
                             "{}".format(type(state)))
         self._state = state
 
-    def generate(self, set_data=True):
+    def generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
+
+        Returns:
+            (list of) np.array: state data
+        """
+        # generate the data
+        data = self._generate(set_data=set_data, reset_state=reset_state)
+
+        # call the user function
+        if self.fct is not None:
+            self.fct()
+
+        return data
+
+    def _generate(self, set_data=True, reset_state=True):
+        """Generate the state.
+
+        Args:
+            set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
         """
         raise NotImplementedError
 
-    def __repr__(self):
-        return self.__class__.__name__
+    # def __repr__(self):
+    #     return self.__class__.__name__
 
     def __str__(self):
+        """Return a string describing the class."""
         return self.__class__.__name__
 
     def __call__(self, set_data=True):
+        """Call the generator."""
         return self.generate(set_data=set_data)
 
 
@@ -90,26 +117,31 @@ class FixedStateGenerator(StateGenerator):
     This generator returns the same initial state each time it is called.
     """
 
-    def __init__(self, state):
+    def __init__(self, state, fct=None):
         """Initialize the fixed state generator.
 
         Args:
             state (State): state instance.
+            fct (callable, None): callback function to be called after generating the data.
         """
-        super(FixedStateGenerator, self).__init__(state)
+        super(FixedStateGenerator, self).__init__(state, fct=fct)
         self.initial_data = self.state.data
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
         """
         if set_data:
             self.state.data = self.initial_data
+        if reset_state:
+            self.state.reset()
         return self.initial_data
 
 
@@ -117,8 +149,16 @@ class QueueStateGenerator(StateGenerator):
     r"""Abstract Queue state generator
     """
 
-    def __init__(self, state, queue):
-        super(QueueStateGenerator, self).__init__(state)
+    def __init__(self, state, queue, fct=None):
+        """
+        Initialize the queue state generator.
+
+        Args:
+            state (State): initial state.
+            queue (queue.Queue): queue instance which contains the data.
+            fct (callable, None): callback function to be called after generating the data.
+        """
+        super(QueueStateGenerator, self).__init__(state, fct=fct)
         self.queue = queue
         self.initial_data = self.state.data
 
@@ -129,10 +169,11 @@ class QueueStateGenerator(StateGenerator):
 
     @queue.setter
     def queue(self, q):
+        """Set the queue instance."""
         if not isinstance(q, queue.Queue):
             raise TypeError("Expecting the given queue to be an instance of `queue.Queue`, instead got: "
                             "{}".format(type(q)))
-        self._queue = queue
+        self._queue = q
 
     def put(self, item, block=False, timeout=None):
         """Put an item into the queue.
@@ -159,7 +200,7 @@ class QueueStateGenerator(StateGenerator):
         ('timeout' is ignored in that case).
         """
         if self.queue.empty():
-           return self.initial_data
+            return self.initial_data
         return self.queue.get(block=block, timeout=timeout)
 
     # alias
@@ -177,11 +218,13 @@ class QueueStateGenerator(StateGenerator):
         """Return the approximate size of the queue (not reliable!)."""
         return self.queue.qsize()
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
@@ -191,6 +234,8 @@ class QueueStateGenerator(StateGenerator):
             data = data.data
         if set_data:
             self.state.data = data
+        if reset_state:
+            self.state.reset()
         return data
 
     def __len__(self):
@@ -222,15 +267,16 @@ class FIFOQueueStateGenerator(QueueStateGenerator):
     The queue is filled by the user during training.
     """
 
-    def __init__(self, state, maxsize=0):
+    def __init__(self, state, maxsize=0, fct=None):
         """Initialize the FIFO queue state generator.
 
         Args:
             state (State): state instance.
             maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite.
+            fct (callable, None): callback function to be called after generating the data.
         """
         q = queue.Queue(maxsize)
-        super(FIFOQueueStateGenerator, self).__init__(state, queue=q)
+        super(FIFOQueueStateGenerator, self).__init__(state, queue=q, fct=fct)
 
 
 class LIFOQueueStateGenerator(QueueStateGenerator):
@@ -240,15 +286,16 @@ class LIFOQueueStateGenerator(QueueStateGenerator):
     The queue is filled by the user during training.
     """
 
-    def __init__(self, state, maxsize=0):
+    def __init__(self, state, maxsize=0, fct=None):
         """Initialize the LIFO queue state generator.
 
         Args:
             state (State): state instance.
             maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite.
+            fct (callable, None): callback function to be called after generating the data.
         """
         q = queue.LifoQueue(maxsize)
-        super(LIFOQueueStateGenerator, self).__init__(state, queue=q)
+        super(LIFOQueueStateGenerator, self).__init__(state, queue=q, fct=fct)
 
 
 class PriorityQueueStateGenerator(QueueStateGenerator):
@@ -261,16 +308,17 @@ class PriorityQueueStateGenerator(QueueStateGenerator):
     poorly during the training.
     """
 
-    def __init__(self, state, maxsize=0, ascending=True):
+    def __init__(self, state, maxsize=0, ascending=True, fct=None):
         """Initialize the priority queue state generator.
 
         Args:
             state (State): state instance.
             maxsize (int): maximum size of the queue. If :attr:`maxsize` is <= 0, the queue size is infinite.
             ascending (bool): if True, the item with the lowest priority will be the first one to be retrieved.
+            fct (callable, None): callback function to be called after generating the data.
         """
         q = queue.PriorityQueue(maxsize)
-        super(PriorityQueueStateGenerator, self).__init__(state, queue=q)
+        super(PriorityQueueStateGenerator, self).__init__(state, queue=q, fct=fct)
         self.ascending = ascending
 
     def get(self, block=False, timeout=None):
@@ -315,15 +363,17 @@ class StateDistributionGenerator(StateGenerator):
     The initial states :math:`s` are generated by a probability distribution :math:`p(s)`, that is :math:`s \sim p(s)`.
     The probability distribution can be learned using generative models.
     """
+    __metaclass__ = ABCMeta
 
-    def __init__(self, state, seed=None):
+    def __init__(self, state, seed=None, fct=None):
         """Initialize the state distribution generator.
 
         Args:
             state (State): state instance.
             seed (None, int): random seed.
+            fct (callable, None): callback function to be called after generating the data.
         """
-        super(StateDistributionGenerator, self).__init__(state)
+        super(StateDistributionGenerator, self).__init__(state, fct=fct)
         self.seed = seed
 
     @property
@@ -338,8 +388,11 @@ class StateDistributionGenerator(StateGenerator):
         Args:
             seed (int): random seed
         """
+        if seed is not None and not isinstance(seed, int):
+            raise TypeError("Expecting the given 'seed' to be an integer, instead got: {}".format(type(seed)))
         if seed is not None:
             np.random.seed(seed)
+        self._seed = seed
 
 
 class UniformStateGenerator(StateDistributionGenerator):
@@ -349,15 +402,17 @@ class UniformStateGenerator(StateDistributionGenerator):
     will be set to be the range of the states.
     """
     
-    def __init__(self, state, low=None, high=None):
+    def __init__(self, state, low=None, high=None, seed=None, fct=None):
         """Initialize the state distribution generator.
 
         Args:
             state (State): state instance.
             low (None, float, np.array, list of np.array): lower bound
             high (None, float, np.array, list of np.array): upper bound
+            seed (None, int): random seed.
+            fct (callable, None): callback function to be called after generating the data.
         """
-        super(UniformStateGenerator, self).__init__(state)
+        super(UniformStateGenerator, self).__init__(state, seed=seed, fct=fct)
         self.low = low
         self.high = high
 
@@ -373,12 +428,13 @@ class UniformStateGenerator(StateDistributionGenerator):
             low = [-np.infty] * len(self.state)
         elif isinstance(low, (int, float)):
             low = [low] * len(self.state)
-        elif isinstance(low, (list, tuple)):
+        elif isinstance(low, (list, tuple, np.ndarray)):
             if len(low) != len(self.state):
                 raise ValueError("The lower bound doesn't have the same size as the number of states; len(low) = {} "
                                  "and len(state) = {}".format(len(low), len(self.state)))
         else:
-            raise TypeError
+            raise TypeError("Expecting the 'low' bound to be an int, float, or list/tuple/np.array of int/float, "
+                            "instead got: {}".format(type(low)))
         self._low = low
 
     @property
@@ -393,29 +449,37 @@ class UniformStateGenerator(StateDistributionGenerator):
             high = [-np.infty] * len(self.state)
         elif isinstance(high, (int, float)):
             high = [high] * len(self.state)
-        elif isinstance(high, (list, tuple)):
+        elif isinstance(high, (list, tuple, np.ndarray)):
             if len(high) != len(self.state):
                 raise ValueError("The higher bound doesn't have the same size as the number of states; len(high) = {} "
                                  "and len(state) = {}".format(len(high), len(self.state)))
         else:
-            raise TypeError
+            raise TypeError("Expecting the 'high' bound to be an int, float, or list/tuple/np.array of int/float, "
+                            "instead got: {}".format(type(high)))
         self._high = high
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
         """
-        spaces = self.state.space
-        data = [space.sample() for space in spaces]
-        for idx, datum, low, high in np.clip(zip(data, self.low, self.high)):
-            data[idx] = np.clip(datum, low, high)
+        # spaces = self.state.space
+        # data = [space.sample() for space in spaces]
+        # for idx, datum, low, high in enumerate(zip(data, self.low, self.high)):
+        #     data[idx] = np.clip(datum, low, high)
+
+        data = [np.random.uniform(low=low, high=high, size=len(state))
+                for state, low, high in zip(self.state, self.low, self.high)]
         if set_data:
             self.state.data = data
+        if reset_state:
+            self.state.reset()
         return data
 
 
@@ -426,7 +490,7 @@ class NormalStateGenerator(StateDistributionGenerator):
     The states are then truncated / clipped to be inside their corresponding range.
     """
 
-    def __init__(self, state, means=0, scales=1.):
+    def __init__(self, state, means=0, scales=1., seed=None, fct=None):
         """
         Initialize the Normal state generator.
 
@@ -434,10 +498,12 @@ class NormalStateGenerator(StateDistributionGenerator):
             state (State): state instance.
             means:
             scales:
+            seed (None, int): random seed.
+            fct (callable, None): callback function to be called after generating the data.
         """
-        super(NormalStateGenerator, self).__init__(state)
+        super(NormalStateGenerator, self).__init__(state, seed=seed, fct=fct)
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         pass
 
 
@@ -446,16 +512,18 @@ class GenerativeStateGenerator(StateGenerator):
 
     This uses a generative model that has been trained to learn a distribution to generate the initial states.
     """
+    __metaclass__ = ABCMeta
 
-    def __init__(self, state, model):
+    def __init__(self, state, model, fct=None):
         """
         Initialize the Generative initial state generator.
 
         Args:
             state (State): state instance.
             model (Model): generative model instance.
+            fct (callable, None): callback function to be called after generating the data.
         """
-        super(GenerativeStateGenerator, self).__init__(state)
+        super(GenerativeStateGenerator, self).__init__(state, fct=fct)
         self.model = model
 
 
@@ -465,14 +533,16 @@ class VAEStateGenerator(GenerativeStateGenerator):
     This uses the decoder a pretrained VAE to generate initial states.
     """
 
-    def __init__(self, state, model):
-        super(VAEStateGenerator, self).__init__(state, model)
+    def __init__(self, state, model, fct=None):
+        super(VAEStateGenerator, self).__init__(state, model, fct=fct)
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
@@ -486,12 +556,12 @@ class GANStateGenerator(GenerativeStateGenerator):
     This uses the generator of a trained GAN model to generate similar states.
     """
 
-    def __init__(self, state, model, distribution=None, mapping=None):
+    def __init__(self, state, model, distribution=None, mapping=None, fct=None):
         """
         Initialize the GAN initial state generator.
 
         Args:
-            states: states that need to be generated
+            state: states that need to be generated
             model: GAN or generator of GAN
             distribution: distribution over the noise vector
             mapping:
@@ -517,13 +587,15 @@ class GANStateGenerator(GenerativeStateGenerator):
         # setting mapping
         self.mapping = mapping
 
-        super(GANStateGenerator, self).__init__(state, model)
+        super(GANStateGenerator, self).__init__(state, model, fct=fct)
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
@@ -541,14 +613,16 @@ class GMMStateGenerator(GenerativeStateGenerator):
     This uses a pretrained GMM to generate the states.
     """
 
-    def __init__(self, state, model):
-        super(GMMStateGenerator, self).__init__(state, model)
+    def __init__(self, state, model, fct=None):
+        super(GMMStateGenerator, self).__init__(state, model, fct=fct)
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
@@ -559,6 +633,7 @@ class GMMStateGenerator(GenerativeStateGenerator):
 class UncertaintyStateGenerator(StateGenerator):
     r"""State generator that exploits the uncertainty of initial states.
     """
+    __metaclass__ = ABCMeta
     pass
 
 
@@ -567,6 +642,7 @@ class BOStateGenerator(UncertaintyStateGenerator):
 
     We use Bayesian Optimization to generate the initial states.
     """
+    __metaclass__ = ABCMeta
     pass
 
 
@@ -586,7 +662,7 @@ class AEBOStateGenerator(GenerativeStateGenerator):
     The exploration will then be carried out in the hyperrectangle formed by these 2 reduced state vector limits.
     """
 
-    def __init__(self, state, model, kernel_capacity=100):
+    def __init__(self, state, model, kernel_capacity=100, fct=None):
         """
         Initialize the autoencoder + bayesian optimization initial state generator.
 
@@ -597,11 +673,13 @@ class AEBOStateGenerator(GenerativeStateGenerator):
         """
         super(AEBOStateGenerator, self).__init__(state, model)
 
-    def generate(self, set_data=True):
+    def _generate(self, set_data=True, reset_state=True):
         """Generate the state.
 
         Args:
             set_data (bool): If True, it will set the generated data to the state.
+            reset_state (bool): If True, it will reset the state with the generated data (if `set_data` has been set
+                to True).
 
         Returns:
             (list of) np.array: state data
diff --git a/pyrobolearn/states/robot_states/__init__.py b/pyrobolearn/states/robot_states/__init__.py
index 69234a6..3113d2a 100644
--- a/pyrobolearn/states/robot_states/__init__.py
+++ b/pyrobolearn/states/robot_states/__init__.py
@@ -8,8 +8,9 @@ from .joint_states import JointState, JointPositionState, JointTrigonometricPosi
     JointForceTorqueState, JointAccelerationState
 
 # import the link states
-from .link_states import LinkState, LinkPositionState, LinkWorldPositionState, LinkOrientationState, \
-    LinkVelocityState, LinkLinearVelocityState, LinkAngularVelocityState
+from .link_states import LinkState, LinkPositionState, LinkOrientationState, LinkVelocityState, \
+    LinkLinearVelocityState, LinkAngularVelocityState, LinkWorldPositionState, LinkWorldOrientationState, \
+    LinkWorldVelocityState, LinkWorldLinearVelocityState, LinkWorldAngularVelocityState
 
 # import the sensor states
 from .sensor_states import SensorState, CameraState, ContactState, FeetContactState
diff --git a/pyrobolearn/states/robot_states/joint_states.py b/pyrobolearn/states/robot_states/joint_states.py
index 4ac1f04..2cb0cbc 100644
--- a/pyrobolearn/states/robot_states/joint_states.py
+++ b/pyrobolearn/states/robot_states/joint_states.py
@@ -113,10 +113,24 @@ class JointPositionState(JointState):
         """
         super(JointPositionState, self).__init__(robot, joint_ids, window_size=window_size, axis=axis, ticks=ticks)
 
+        # define the space based on the joint type  # TODO
+
     def _read(self):
         """Read the next joint position state."""
         self.data = self.robot.get_joint_positions(self.joints)
 
+    def _reset(self):
+        """Reset the state."""
+        # reset counter
+        self.cnt = 0.
+
+        # reset the robot joint position based on the data
+        if len(self.data) > 0:
+            self.robot.reset_joint_states(q=self.data[0], joint_ids=self.joints)
+
+        # read the next data
+        self._read()
+
 
 class JointTrigonometricPositionState(JointState):
     r"""Joint Trigonometric Position State
diff --git a/pyrobolearn/states/robot_states/link_states.py b/pyrobolearn/states/robot_states/link_states.py
index 6b24a62..e4e6d3f 100644
--- a/pyrobolearn/states/robot_states/link_states.py
+++ b/pyrobolearn/states/robot_states/link_states.py
@@ -145,13 +145,48 @@ class LinkWorldPositionState(LinkState):
 
     def _read(self):
         """Read the next link world position state."""
-        self.data = self.robot.get_link_positions(self.links)
+        self.data = self.robot.get_link_world_positions(self.links, flatten=True)
 
 
 class LinkOrientationState(LinkState):
     r"""Link Orientation state
     """
 
+    def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the link world orientation state.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+            wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
+                it will be the base.
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        self.wrt_link_id = wrt_link_id
+        super(LinkOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+
+    def _read(self):  # TODO: convert
+        """Read the next link orientation state."""
+        self.data = self.robot.get_link_orientations(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
+
+
+class LinkWorldOrientationState(LinkState):
+    r"""Link World Orientation state
+    """
+
     def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
         """
         Initialize the link world orientation state.
@@ -173,17 +208,53 @@ class LinkOrientationState(LinkState):
                 state is not a combination of states, but is given some :attr:`data`.
             ticks (int): number of ticks to sleep before getting the next state data.
         """
-        super(LinkOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+        super(LinkWorldOrientationState, self).__init__(robot, link_ids, window_size=window_size, axis=axis,
+                                                        ticks=ticks)
 
     def _read(self):  # TODO: convert
-        """Read the next link orientation state."""
-        self.data = self.robot.get_link_orientations(self.links)
+        """Read the next link world orientation state."""
+        self.data = self.robot.get_link_world_orientations(self.links, flatten=True)
 
 
 class LinkVelocityState(LinkState):
     r"""Link velocity state
     """
 
+    def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the link velocity state.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+            wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
+                it will be the base.
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        self.wrt_link_id = wrt_link_id
+        super(LinkVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+
+    def _read(self):
+        """Read the next link velocity state."""
+        self.data = self.robot.get_link_velocities(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
+
+
+class LinkWorldVelocityState(LinkState):
+    r"""Link world velocity state
+    """
+
     def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
         """
         Initialize the link velocity state.
@@ -205,17 +276,52 @@ class LinkVelocityState(LinkState):
                 state is not a combination of states, but is given some :attr:`data`.
             ticks (int): number of ticks to sleep before getting the next state data.
         """
-        super(LinkVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+        super(LinkWorldVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
 
     def _read(self):
-        """Read the next link velocity state."""
-        self.data = self.robot.get_link_velocities(self.links)
+        """Read the next link world velocity state."""
+        self.data = self.robot.get_link_world_velocities(self.links, flatten=True)
 
 
 class LinkLinearVelocityState(LinkState):
     r"""Link linear velocity state
     """
 
+    def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the link linear velocity state.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+            wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
+                it will be the base.
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        self.wrt_link_id = wrt_link_id
+        super(LinkLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+
+    def _read(self):
+        """Read the next link linear velocity state."""
+        self.data = self.robot.get_link_linear_velocities(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
+
+
+class LinkWorldLinearVelocityState(LinkState):
+    r"""Link world linear velocity state
+    """
+
     def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
         """
         Initialize the link linear velocity state.
@@ -237,17 +343,53 @@ class LinkLinearVelocityState(LinkState):
                 state is not a combination of states, but is given some :attr:`data`.
             ticks (int): number of ticks to sleep before getting the next state data.
         """
-        super(LinkLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+        super(LinkWorldLinearVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis,
+                                                           ticks=ticks)
 
     def _read(self):
-        """Read the next link linear velocity state."""
-        self.data = self.robot.get_link_linear_velocities(self.links)
+        """Read the next link world linear velocity state."""
+        self.data = self.robot.get_link_world_linear_velocities(self.links, flatten=True)
 
 
 class LinkAngularVelocityState(LinkState):
     r"""Link angular velocity state
     """
 
+    def __init__(self, robot, link_ids=None, wrt_link_id=None, window_size=1, axis=None, ticks=1):
+        """
+        Initialize the link angular velocity state.
+
+        Args:
+            robot (Robot): robot instance
+            link_ids (int, int[N]): link id or list of link ids
+            wrt_link_id (int, None): link with respect to which we compute the position of the other links. If None,
+                it will be the base.
+            window_size (int): window size of the state. This is the total number of states we should remember. That
+                is, if the user wants to remember the current state :math:`s_t` and the previous state :math:`s_{t-1}`,
+                the window size is 2. By default, the :attr:`window_size` is one which means we only remember the
+                current state. The window size has to be bigger than 1. If it is below, it will be set automatically
+                to 1. The :attr:`window_size` attribute is only valid when the state is not a combination of states,
+                but is given some :attr:`data`.
+            axis (int, None): axis to concatenate or stack the states in the current window. If you have a state with
+                shape (n,), then if the axis is None (by default), it will just concatenate it such that resulting
+                state has a shape (n*w,) where w is the window size. If the axis is an integer, then it will just stack
+                the states in the specified axis. With the example, for axis=0, the resulting state has a shape of
+                (w,n), and for axis=-1 or 1, it will have a shape of (n,w). The :attr:`axis` attribute is only when the
+                state is not a combination of states, but is given some :attr:`data`.
+            ticks (int): number of ticks to sleep before getting the next state data.
+        """
+        self.wrt_link_id = wrt_link_id
+        super(LinkAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+
+    def _read(self):
+        """Read the next link angular velocity state."""
+        self.data = self.robot.get_link_angular_velocities(self.links, wrt_link_id=self.wrt_link_id, flatten=True)
+
+
+class LinkWorldAngularVelocityState(LinkState):
+    r"""Link world angular velocity state
+    """
+
     def __init__(self, robot, link_ids=None, window_size=1, axis=None, ticks=1):
         """
         Initialize the link angular velocity state.
@@ -269,8 +411,9 @@ class LinkAngularVelocityState(LinkState):
                 state is not a combination of states, but is given some :attr:`data`.
             ticks (int): number of ticks to sleep before getting the next state data.
         """
-        super(LinkAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis, ticks=ticks)
+        super(LinkWorldAngularVelocityState, self).__init__(robot, link_ids, window_size=window_size, axis=axis,
+                                                            ticks=ticks)
 
     def _read(self):
-        """Read the next link angular velocity state."""
-        self.data = self.robot.get_link_angular_velocities(self.links)
+        """Read the next link world angular velocity state."""
+        self.data = self.robot.get_link_world_angular_velocities(self.links, flatten=True)
diff --git a/pyrobolearn/utils/transformation.py b/pyrobolearn/utils/transformation.py
index 5d9dc99..980e0d7 100644
--- a/pyrobolearn/utils/transformation.py
+++ b/pyrobolearn/utils/transformation.py
@@ -27,6 +27,24 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
+def min_angle_difference(q1, q2):
+    r"""
+    Return the minimum angle difference between two (set of) angles (expressed in radians). The differences are
+    always between [-pi, pi].
+
+    Args:
+        q1 (float, np.array[N]): first angle(s)
+        q2 (float, np.array[N]): second angle(s)
+
+    Returns:
+        float, np.array[N]: minimum angle difference(s)
+    """
+    diff = np.maximum(q1, q2) - np.minimum(q1, q2)
+    if diff > np.pi:
+        diff = 2 * np.pi - diff
+    return diff
+
+
 def get_homogeneous_transform(position, orientation):
     r"""
     Return the Homogeneous transform matrix given the position vector and the orientation.
@@ -457,31 +475,42 @@ def get_symbolic_matrix_from_quaternion(q, convention='xyzw'):
 
 def get_rpy_from_quaternion(q, convention='xyzw'):
     """
-    Get the Roll-Pitch-Yaw angle from the given quaternion.
+    Get the Roll-Pitch-Yaw angle(s) from the given quaternion(s).
 
     Args:
-        q (np.array[4], quaternion.quaternion): quaternion
+        q (np.array[4], np.array[N,4], (list of) quaternion.quaternion): quaternion(s)
         convention: convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
             'wxyz'.
 
     Returns:
-        np.float[3]: roll-pitch-yaw angles.
+        np.float[3], np.float[N,3]: roll-pitch-yaw angles.
     """
-    if isinstance(q, quaternion.quaternion):
-        x, y, z, w = q.x, q.y, q.z, q.w
+    multiple_quaternions = True
+    if isinstance(q, quaternion.quaternion) or (isinstance(q, np.ndarray) and len(q.shape) == 1):
+        multiple_quaternions = False
+        q = np.array([q])  # (1,4)
+
+    if isinstance(q[0], quaternion.quaternion):
+        q = quaternion.as_float_array(q)
+        x, y, z, w = q[:, 1], q[:, 2], q[:, 3], q[:, 0]  # (N,)
     elif isinstance(q, Iterable):
         if convention == 'xyzw':
-            x, y, z, w = q
+            x, y, z, w = q[:, 0], q[:, 1], q[:, 2], q[:, 3]  # (N,)
         elif convention == 'wxyz':
-            w, x, y, z = q
+            w, x, y, z = q[:, 0], q[:, 1], q[:, 2], q[:, 3]  # (N,)
         else:
             raise NotImplementedError("Asking for a convention that has not been implemented")
     else:
         raise TypeError
-    roll = np.arctan2(2*(w*x + y*z), 1 - 2 * (x**2 + y**2))
-    pitch = np.arcsin(2 * (w*y - z*x))
-    yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2))
-    return np.array([roll, pitch, yaw])
+
+    roll = np.arctan2(2 * (w*x + y*z), 1 - 2 * (x**2 + y**2))   # (N,)
+    pitch = np.arcsin(2 * (w*y - z*x))                          # (N,)
+    yaw = np.arctan2(2 * (w*z + x*y), 1 - 2 * (y**2 + z**2))    # (N,)
+    rpy = np.vstack((roll, pitch, yaw)).T  # (N,3)
+
+    if not multiple_quaternions:
+        return rpy[0]
+    return np.array(rpy)
 
 
 def get_symbolic_rpy_from_quaternion(q, convention='xyzw'):
@@ -520,7 +549,7 @@ def get_quaternion_from_rpy(rpy, convert_to_quat=False, convention='xyzw'):
     Get quaternion from Roll-Pitch-Yaw angle.
 
     Args:
-        rpy (np.float[3]): roll-pitch-yaw angles
+        rpy (np.float[3], np.float[N,3]): roll-pitch-yaw angles
         convert_to_quat (bool): If True, it will return an instance of `quaternion.quaternion`.
         convention (str): convention to be adopted when representing the quaternion. You can choose between 'xyzw' or
             'wxyz'.
@@ -528,26 +557,36 @@ def get_quaternion_from_rpy(rpy, convert_to_quat=False, convention='xyzw'):
     Returns:
         np.array[4], quaternion.quaternion: quaternion
     """
-    r, p, y = rpy
+    rpy = np.asarray(rpy)
+    multiple_rpy = True
+    if len(rpy.shape) < 2:
+        multiple_rpy = False
+        rpy = np.array([rpy])  # (1,3)
+
+    r, p, y = rpy[:, 0], rpy[:, 1], rpy[:, 2]
     cr, sr = np.cos(r/2.), np.sin(r/2.)
     cp, sp = np.cos(p/2.), np.sin(p/2.)
     cy, sy = np.cos(y/2.), np.sin(y/2.)
 
-    w = cr * cp * cy + sr * sp * sy
-    x = sr * cp * cy - cr * sp * sy
-    y = cr * sp * cy + sr * cp * sy
-    z = cr * cp * sy - sr * sp * cy
+    w = cr * cp * cy + sr * sp * sy  # (N,)
+    x = sr * cp * cy - cr * sp * sy  # (N,)
+    y = cr * sp * cy + sr * cp * sy  # (N,)
+    z = cr * cp * sy - sr * sp * cy  # (N,)
 
     if convert_to_quat:
-        return quaternion.quaternion(w, x, y, z)
+        q = quaternion.from_float_array(np.vstack((w, x, y, z)).T)
     else:
         if convention == 'xyzw':
-            return np.array([x, y, z, w])
+            q = np.vstack([x, y, z, w]).T
         elif convention == 'wxyz':
-            return np.array([w, x, y, z])
+            q = np.vstack([w, x, y, z]).T
         else:
             raise NotImplementedError("Asking for a convention that has not been implemented")
 
+    if not multiple_rpy:
+        return q[0]
+    return q
+
 
 def get_symbolic_quaternion_from_rpy(rpy, convention='xyzw'):
     """
@@ -956,11 +995,13 @@ def angular_velocity_from_quaternion(q1, q2):
 
 def quaternion_distance(q1, q2):
     r"""
-    Compute the distance metric (on :math:`\mathbb{S}^3`) betwen two quaternions :math:`q_1` and :math:`q_2`:
+    Compute the distance metric (on :math:`\mathbb{S}^3`) between two quaternions :math:`q_1` and :math:`q_2`:
 
     Assuming a quaternion :math:`q` is represented as :math:`s + \pmb{v}` where :math:`s \in \mathbb{R}` is the scalar
     part and :math:`\pmb{v} \in \mathbb{R}^3` is the vector part, the distance is given by:
 
+    .. math::
+
         d(q_1, q_2) = \left\{ \begin{array}{ll}
                 2\pi, & q1 * \bar{q}_2 = -1 + [0,0,0]^\top \\
                 2 || \log(q_1 * \bar{q}_2) ||, & \text{otherwise}