add 2 control envs + update terminal conditions

2026-09-09 11:31:38 +08:00 · 2019-07-19 03:32:11 +02:00
parent 18b70f01db
commit bcfe7538bf
26 changed files with 1755 additions and 112 deletions
@@ -36,6 +36,9 @@ from . import states
 # import actions
 from . import actions

+# import terminal conditions
+from . import terminal_conditions
+
 # import rewards
 from . import rewards

@@ -91,7 +94,7 @@ from . import algos
 __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"
@@ -6,7 +6,7 @@ from .robot_actions import RobotAction
 from .joint_actions import JointAction, JointPositionAction, JointPositionChangeAction, JointVelocityAction, \
    JointVelocityChangeAction, JointPositionAndVelocityAction, JointPositionAndVelocityChangeAction, \
    JointTorqueAction, JointForceAction, JointTorqueGravityCompensationAction, JointTorqueChangeAction, \
-    JointAccelerationAction
+    JointAccelerationAction, JointAccelerationChangeAction

 # import the link / end-effector actions
 from .link_actions import LinkAction, LinkPositionAction, LinkPositionChangeAction, LinkOrientationAction, \
@@ -71,10 +71,11 @@ class JointAction(RobotAction):
                if not isinstance(value, (list, tuple, np.ndarray)):
                    raise TypeError("Expecting each discrete value set to be a list/tuple/np.ndarray, instead got: "
                                    "{} at index {}".format(type(value), i))
-                discrete_values[i] = np.asarray(value)
-                if len(discrete_values.shape) != 1:
+                value = np.asarray(value)
+                discrete_values[i] = value
+                if value.ndim != 1:
                    raise ValueError("Expecting each discrete value set to be a 1D array, instead got a shape of: "
-                                     "{}".format(discrete_values.shape))
+                                     "{}".format(value.shape))

        # set the discrete values
        self.discrete_values = discrete_values
@@ -84,14 +85,15 @@ class JointAction(RobotAction):
            # set the space
            if len(self.discrete_values) == 1:
                self._space = gym.spaces.Discrete(len(self.discrete_values))
+                self.discrete_values = self.discrete_values[0]
            else:
                self._space = gym.spaces.MultiDiscrete([len(value) for value in self.discrete_values])

            # set the data
            if isinstance(self._space, gym.spaces.Discrete):
-                self.data = 0
+                self.data = np.zeros(1, dtype=np.int)  # the first index is the default values
            else:
-                self.data = np.zeros(len(self._space.nvec))
+                self.data = np.zeros(len(self._space.nvec), dtype=np.int)  # the first indices are the default values

    # @property
    # def size(self):
@@ -99,12 +101,17 @@ class JointAction(RobotAction):

    def _check_continuous_bounds(self, bounds):
        """Check the given continuous bounds."""
+        # check the type of the bounds
        if not isinstance(bounds, (tuple, list, np.ndarray)):
            raise TypeError("Expecting the given bounds to be a tuple/list/np.ndarray of float, instead got: "
                            "{}".format(type(bounds)))
+
+        # check that the bounds have a length of 2 (i.e. lower and upper bounds)
        if len(bounds) != 2:
            raise ValueError("Expecting the bounds to be of length 2 (i.e. lower and upper bounds), instead got a "
                             "length of {}".format(len(bounds)))
+
+        # if both bounds are not None, reshape if necessary
        if bounds[0] is not None and bounds[1] is not None:
            bounds = np.asarray(bounds).reshape(2, -1)
            if len(self.joints) != bounds.shape[1]:
@@ -117,28 +124,70 @@ class JointAction(RobotAction):
            bounds = tuple(bounds)
        return bounds

-    # def _write(self, data):
-    #     """
-    #     Write the data.
-    #
-    #     Args:
-    #         data (int, np.ndarray): the data can be discrete or continuous.
-    #     """
-    #     # if the action is discrete, then the data should be an index, or an array of values from which takes the max
-    #     if self.is_discrete():
-    #         if isinstance(data, np.ndarray):
-    #             data = np.argmax(data.reshape(-1))
-    #         data = self.discrete_values[data]
-    #     self._write_continuous(data)
-    #
-    # def _write_continuous(self, data):
-    #     """
-    #     Write the given continuous data. Child method that has to be implement in the child classes.
-    #
-    #     Args:
-    #         data (np.ndarray): continuous data to be written.
-    #     """
-    #     raise NotImplementedError
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the data can be discrete or continuous.
+        """
+        # - if the action is discrete, then the data should be an index, or an array of values from which takes the max
+        # - if the action is multi-discrete, then the data should be an array of index, or a list of array values from
+        #   which to take the max for each array
+        if self.discrete_values is not None:
+            # if the action is discrete
+            if isinstance(self._space, gym.spaces.Discrete):
+                # if the data is an array of values, take the argmax
+                if isinstance(data, np.ndarray):
+                    if data.size > 1:
+                        data = np.argmax(data.reshape(-1))
+                    else:
+                        data = int(data.reshape(-1)[0])
+
+                # elif not an index, raise an error
+                elif not isinstance(data, int):
+                    raise TypeError("Expecting the given data to be an int (representing the index) for the discrete "
+                                    "values, instead got: {}".format(type(data)))
+
+                # take the corresponding "continuous" data
+                data = self.discrete_values[data]
+
+            # if action is multi-discrete
+            elif isinstance(self._space, gym.spaces.MultiDiscrete):
+
+                # check the type of the data
+                if isinstance(data, (list, np.ndarray)):
+                    raise TypeError("Expecting the data to be an list of int/np.array, or a np.array, instead got: "
+                                    "{}".format(type(data)))
+
+                # check that the number of rows match the number of discrete actions
+                if len(data) != self._space.shape[0]:
+                    raise ValueError("The given data does not have the same length (={}) as the number of discrete "
+                                     "action (={})".format(len(data), self._space.shape[0]))
+
+                # check each data row and convert it to index if necessary
+                data_tmp = []
+                for d in data:
+                    if isinstance(d, np.ndarray):
+                        d = np.argmax(d.reshape(-1))
+                    elif not isinstance(d, int):
+                        raise TypeError("Expecting the given data to be an int (representing the index) for the "
+                                        "discrete values, instead got: {}".format(type(d)))
+                    data_tmp.append(d)
+
+                # take the corresponding "continuous" data
+                data = np.array([self.discrete_values[d] for d in data])
+
+        self._write_continuous(data)
+
+    def _write_continuous(self, data):
+        """
+        Write the given continuous data. Child method that has to be implement in the child classes.
+
+        Args:
+            data (np.ndarray): continuous data to be written.
+        """
+        raise NotImplementedError

    def __copy__(self):
        """Return a shallow copy of the action. This can be overridden in the child class."""
@@ -205,9 +254,10 @@ class JointPositionAction(JointAction):
        """Return the joint limits."""
        return self.robot.get_joint_limits(self.joints)

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
-        self.robot.set_joint_positions(data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force)
+        self.robot.set_joint_positions(data, self.joints, bounds=self.bounds, kp=self.kp, kd=self.kd,
+                                       forces=self.max_force, discrete_values=self.discrete_values)

    def __copy__(self):
        """Return a shallow copy of the action. This can be overridden in the child class."""
@@ -223,10 +273,13 @@ class JointPositionAction(JointAction):
            return memo[self]
        robot = memo.get(self.robot, self.robot)  # copy.deepcopy(self.robot, memo)
        joints = copy.deepcopy(self.joints)
+        bounds = copy.deepcopy(self.bounds)
        kp = copy.deepcopy(self.kp)
        kd = copy.deepcopy(self.kd)
        max_force = copy.deepcopy(self.max_force)
-        action = self.__class__(robot=robot, joint_ids=joints, kp=kp, kd=kd, max_force=max_force)
+        discrete_values = copy.deepcopy(self.discrete_values)
+        action = self.__class__(robot=robot, joint_ids=joints, bounds=bounds, kp=kp, kd=kd, max_force=max_force,
+                                discrete_values=discrete_values)
        memo[self] = action
        return action

@@ -265,11 +318,11 @@ class JointPositionChangeAction(JointPositionAction):
        if self.discrete_values is None:
            self.data = np.zeros(len(self.joints))

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        # add the original joint positions
        data += self.robot.get_joint_positions(self.joints)
-        super(JointPositionChangeAction, self)._write(data)
+        super(JointPositionChangeAction, self)._write_continuous(data)


 class JointVelocityAction(JointAction):
@@ -305,12 +358,12 @@ class JointVelocityAction(JointAction):
                                       np.infty * np.ones(len(self.joints))])
            self._space = gym.spaces.Box(low=bounds[:, 0], high=bounds[:, 1])

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        self.robot.set_joint_velocities(data, self.joints)


-class JointVelocityChangeAction(JointAction):
+class JointVelocityChangeAction(JointVelocityAction):
    r"""Joint Velocity Change Action

    Set the joint velocities using velocity control;  this class expect to receive a change in the joint velocities
@@ -332,19 +385,20 @@ class JointVelocityChangeAction(JointAction):
                the first value along the first axis / dimension are the values by default that are set if no data
                is provided.
        """
-        super(JointVelocityChangeAction, self).__init__(robot, joint_ids, discrete_values=discrete_values)
+        super(JointVelocityChangeAction, self).__init__(robot, joint_ids, bounds=bounds,
+                                                        discrete_values=discrete_values)

        # set data if continuous
        if self.discrete_values is None:
            self.data = np.zeros(len(self.joints))

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        data += self.robot.get_joint_velocities(self.joints)
-        super(JointVelocityChangeAction, self)._write(data)
+        super(JointVelocityChangeAction, self)._write_continuous(data)


-class JointPositionAndVelocityAction(JointAction):
+class JointPositionAndVelocityAction(JointAction):  # TODO: discrete values
    r"""Joint position and velocity action

    Set the joint position using position control using PD control, where the constraint error to be minimized is
@@ -377,7 +431,7 @@ class JointPositionAndVelocityAction(JointAction):
            self.data = np.concatenate((pos, vel))
        self.idx = len(self.joints)

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        self.robot.set_joint_positions(data[:self.idx], self.joints, kp=self.kp, kd=self.kd,
                                       velocities=data[self.idx:], forces=self.max_force)
@@ -434,11 +488,11 @@ class JointPositionAndVelocityChangeAction(JointPositionAndVelocityAction):
        if self.discrete_values is None:
            self.data = np.zeros(2*len(self.joints))

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        pos, vel = self.robot.get_joint_positions(self.joints), self.robot.get_joint_velocities(self.joints)
        data += np.concatenate((pos, vel))
-        super(JointPositionAndVelocityChangeAction, self)._write(data)
+        super(JointPositionAndVelocityChangeAction, self)._write_continuous(data)


 # class JointPositionVelocityAccelerationAction(JointAction):
@@ -491,14 +545,15 @@ class JointTorqueAction(JointAction):
            self.data = robot.get_joint_torques(self.joints)
            self._space = gym.spaces.Box(low=self.f_min, high=self.f_max)

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        data = np.clip(data, self.f_min, self.f_max)
+        print(data)
        self.robot.set_joint_torques(data, self.joints)

    def __copy__(self):
        """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(robot=self.robot, joint_ids=self.joints, f_min=self.f_min, f_max=self.f_max)
+        return self.__class__(robot=self.robot, joint_ids=self.joints, bounds=(self.f_min, self.f_max))

    def __deepcopy__(self, memo={}):
        """Return a deep copy of the action. This can be overridden in the child class.
@@ -550,11 +605,11 @@ class JointTorqueGravityCompensationAction(JointTorqueAction):
        if self.discrete_values is None:
            self.data = np.zeros(len(self.joints))

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        # add gravity compensation torques
        data += self.robot.get_gravity_compensation_torques(q_idx=self.q_indices)
-        super(JointTorqueGravityCompensationAction, self)._write(data)
+        super(JointTorqueGravityCompensationAction, self)._write_continuous(data)


 # alias
@@ -562,7 +617,7 @@ class JointTorqueGravityCompensationAction(JointTorqueAction):
 JointTorqueChangeAction = JointTorqueGravityCompensationAction


-class JointAccelerationAction(JointAction):
+class JointAccelerationAction(JointAction):  # TODO: discrete values
    r"""Joint Acceleration Action

    Set the joint accelerations using force/torque control. In order to produce the given joint accelerations,
@@ -595,14 +650,15 @@ class JointAccelerationAction(JointAction):
        if self.discrete_values is None:
            self.data = robot.get_joint_accelerations(self.joints)

-    def _write(self, data):
+    def _write_continuous(self, data):
        """apply the action data on the robot."""
        data = np.clip(data, self.a_min, self.a_max)
        self.robot.set_joint_accelerations(data, self.joints)

    def __copy__(self):
        """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(robot=self.robot, joint_ids=self.joints, a_min=self.a_min, a_max=self.a_max)
+        return self.__class__(robot=self.robot, joint_ids=self.joints, bounds=(self.a_min, self.a_max),
+                              discrete_values=self.discrete_values)

    def __deepcopy__(self, memo={}):
        """Return a deep copy of the action. This can be overridden in the child class.
@@ -616,6 +672,43 @@ class JointAccelerationAction(JointAction):
        joints = copy.deepcopy(self.joints)
        a_min = copy.deepcopy(self.a_min)
        a_max = copy.deepcopy(self.a_max)
-        action = self.__class__(robot=robot, joint_ids=joints, f_min=a_min, f_max=a_max)
+        discrete_values = copy.deepcopy(self.discrete_values)
+        action = self.__class__(robot=robot, joint_ids=joints, bounds=(a_min, a_max), discrete_values=discrete_values)
        memo[self] = action
        return action
+
+
+class JointAccelerationChangeAction(JointAccelerationAction):
+    r"""Joint Acceleration Change Action
+
+    Set the joint accelerations using force/torque control. In order to produce the given joint accelerations,
+    we use inverse dynamics which given the joint accelerations produce the corresponding joint forces/torques
+    to be applied.
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint acceleration action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): minimum and maximum accelerations. If None, it will use
+                the default joint acceleration limits. If it still doesn't find them, it will set -np.infty and
+                np.infty.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointAccelerationChangeAction, self).__init__(robot, joint_ids, bounds=bounds,
+                                                            discrete_values=discrete_values)
+
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = np.zeros(len(self.joints))
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_joint_accelerations(self.joints)
+        super(JointAccelerationChangeAction, self)._write_continuous(data)
@@ -52,12 +52,12 @@ class LinkAction(RobotAction):  # TODO: multiple links
                raise TypeError("Expecting the given 'discrete_values' to be a list/tuple/np.array of float/int, but "
                                "instead got: {}".format(type(discrete_values)))
            discrete_values = np.asarray(discrete_values)
-            self._space = gym.spaces.Discrete(len(self.discrete_values))
+            self._space = gym.spaces.Discrete(len(discrete_values))
        self.discrete_values = discrete_values

        # set the data in the case it is discrete
        if self.discrete_values is not None:
-            self.data = 0  # set the data to be the first index
+            self.data = np.zeros(1, dtype=np.int)  # set the data to be the first index

    def _check_discrete_values(self, dim, last_dim):
        """Check that the discrete values have the correct dimensions / shape."""
@@ -542,12 +542,13 @@ class ApplyForceAction(LinkAction):  # TODO: multiple links
        super(ApplyForceAction, self).__init__(robot, link_id, discrete_values=discrete_values)

        # check local position
-        if not isinstance(local_position, (list, tuple, np.ndarray)):
-            raise TypeError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, but instead "
-                            "got: {}".format(type(local_position)))
-        if len(local_position) != 3:
-            raise ValueError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, but "
-                             "instead got a length of: {}".format(len(local_position)))
+        if local_position is not None:
+            if not isinstance(local_position, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, or None, "
+                                "but instead got: {}".format(type(local_position)))
+            if len(local_position) != 3:
+                raise ValueError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, but "
+                                 "instead got a length of: {}".format(len(local_position)))
        self.local_position = local_position

        # check axis
@@ -584,7 +585,7 @@ class ApplyForceAction(LinkAction):  # TODO: multiple links
        """apply the action data on the robot."""
        if self.axis is not None:
            data = data * self.axis
-        self.robot.apply_external_force(force=data, link_id=self.link[0], position=self.local_position)
+        self.robot.apply_external_force(force=data, link_id=self.link, position=self.local_position)


 class ApplyTorqueAction(LinkAction):  # TODO: multiple links
@@ -645,7 +646,7 @@ class ApplyTorqueAction(LinkAction):  # TODO: multiple links
        """apply the action data on the robot."""
        if self.axis is not None:
            data = data * self.axis
-        self.robot.apply_external_torque(torque=data, link_id=self.link[0])
+        self.robot.apply_external_torque(torque=data, link_id=self.link)


 # class ApplyWrenchAction(LinkAction):  # TODO: multiple links
@@ -1,3 +1,5 @@
 ## Control processes/algorithms

+THIS IS UNDER CONSTRUCTION
+
 This folder will contain in the future control processes/algorithms.
@@ -1,5 +1,7 @@
 ## Controllers

+THIS IS UNDER CONSTRUCTION
+
 Controllers are basically policies that do not possess any (hyper-)parameters to optimize. They are manually coded by the user.

 Planning (TODO):
@@ -0,0 +1,137 @@
+#!/usr/bin/env python
+"""Provide the acrobot environment.
+
+This is based on the control problem proposed in OpenAI Gym:
+"The acrobot system includes two joints and two links, where the joint between the two links is actuated. Initially,
+the links are hanging downwards, and the goal is to swing the end of the lower link up to a given height." [1]
+
+References:
+    - [1] Acrobot environment in OpenAI Gym: https://gym.openai.com/envs/Acrobot-v1/
+    - [2] "Generalization in Reinforcement Learning: Successful Examples Using Sparse Coarse Coding", Sutton, 1996.
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.control import ControlEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class AcrobotEnv(ControlEnv):
+    r"""Acrobot Environment
+
+    This is based on the control problem proposed in OpenAI Gym [1]:
+    "The acrobot system includes two joints and two links, where the joint between the two links is actuated.
+    Initially, the links are hanging downwards, and the goal is to swing the end of the lower link up to a given
+    height." [1]
+
+    Here are the various environment features:
+
+    - world: basic world with gravity enabled, a basic floor and the acrobot.
+    - state: the state is given by :math:`[cos(q_1), sin(q_1), cos(q_2), sin(q_2), \dot{q}_1, \dot{q}_2]`
+    - action: discrete joint torques :math:`\tau_2 \in \{-1., 0., +1.\}`
+    - reward: -1 if not terminal
+    - initial state generator: initialize uniformly the joint position and velocity states between [-0.1, 0.1]
+    - physics randomizer: uniform distribution of the mass of the  [mass - mass/10, mass + mass/10]
+    - terminal condition: if the end-effector link is above a certain height.
+
+    References:
+        - [1] Acrobot environment in OpenAI Gym: https://gym.openai.com/envs/Acrobot-v1/
+        - [2] "Generalization in Reinforcement Learning: Successful Examples Using Sparse Coarse Coding", Sutton, 1996.
+    """
+
+    def __init__(self, simulator=None, use_reward_shaping=False, verbose=False):
+        """
+        Initialize the acrobot environment.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            use_reward_shaping (bool): if True, it will use a reward that guides how to achieve the goal.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+        robot = world.load_robot('acrobot')
+        robot.disable_motor()
+        if verbose:
+            robot.print_info()
+
+        # create state: [cos(q_1), sin(q_1), cos(q_2), sin(q_2), \dot{q}_1, \dot{q}_2]
+        trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+        velocity_state = prl.states.JointVelocityState(robot=robot)
+        state = trig_position_state + velocity_state
+        if verbose:
+            print("\nObservation: {}".format(state))
+
+        # create action: \tau_2 in {0., -1., +1.}
+        action = prl.actions.JointTorqueAction(robot, joint_ids=robot.joints[-1],
+                                               discrete_values=np.array([0., -1., +1.]))
+        if verbose:
+            print("\nAction: {}".format(action))
+
+        # create terminal condition:
+        terminal_condition = prl.terminal_conditions.LinkPositionCondition(robot, link_id=robot.joints[-1],
+                                                                           bounds=(2.5, np.infty), dim=2,
+                                                                           out=True, stay=False)
+
+        # create reward: -1 if not terminal
+        if use_reward_shaping:  # use continuous reward
+            # distance_cost = prl.rewards.DistanceCost()
+            # orientation_cost = prl.rewards.OrientationCost()
+            position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
+                                                          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), update_state=True)
+            # reward = distance_cost + orientation_cost + 0.1 * velocity_cost + 0.01 * torque_cost
+            reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+        else:  # use discrete reward
+            reward = prl.rewards.TerminalReward(terminal_condition, subreward=-1., final_reward=0.)
+
+        # 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) + velocity_state
+        num_joints = len(robot.joints)
+        low = [[np.pi + 0.1] + [0]*(num_joints-1), [-0.1]*num_joints]
+        high = [[np.pi - 0.1] + [0]*(num_joints-1), [0.1]*num_joints]
+        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)
+
+        # create environment using composition
+        super(AcrobotEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                         initial_state_generators=initial_state_generator,
+                                         physics_randomizers=physics_randomizer, terminal_conditions=terminal_condition)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = AcrobotEnv(sim, verbose=True)
+
+    # run simulation
+    env.reset()
+    for _ in count():
+        env.step(sleep_dt=1./240)
@@ -0,0 +1,192 @@
+#!/usr/bin/env python
+"""Provide the inverted pole on a cart (Cartpole) environment.
+
+This is based on the control problem proposed in OpenAI Gym:
+"A pole is attached by an un-actuated joint to a cart, which moves along a frictionless track. The system is
+controlled by applying a force of +1 or -1 to the cart. The pendulum starts upright, and the goal is to prevent it
+from falling over. A reward of +1 is provided for every timestep that the pole remains upright. The episode ends when
+the pole is more than 15 degrees from vertical, or the cart moves more than 2.4 units from the center." [1]
+
+Note that compared to [1], you can specify the number of links that forms the inverted pole.
+
+References:
+    - [1] Cartpole environment in OpenAI Gym: https://gym.openai.com/envs/CartPole-v1/
+    - [2] "Neuronlike Adaptive Elements That Can Solve Difficult Learning Control Problem", Barto et al., 1993.
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.control import ControlEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class CartpoleEnv(ControlEnv):
+    r"""Cartpole Environment
+
+    This is based on the control problem proposed in OpenAI Gym:
+    "A pole is attached by an un-actuated joint to a cart, which moves along a frictionless track. The system is
+    controlled by applying a force of +1 or -1 to the cart. The pendulum starts upright, and the goal is to prevent it
+    from falling over. A reward of +1 is provided for every timestep that the pole remains upright. The episode ends
+    when the pole is more than 15 degrees from vertical, or the cart moves more than 2.4 units from the center." [1]
+
+    Note that compared to [1], you can specify the number of links that forms the inverted pole.
+
+    Here are the various environment features (from [1]):
+
+    - world: basic world with gravity enabled, a basic floor and the cartpole.
+    - state: the state is given by :math:`[x, \dot{x}, q_1, \dot{q}_1]` for one inverted pole with one link.
+    - action: discrete forces applied on the cart (+10., -10.)
+    - reward: +1 until termination step
+    - initial state generator: initialize uniformly the state with [-0.05, 0.05]
+    - physics randomizer: uniform distribution of the mass of the  [mass - mass/10, mass + mass/10]
+    - terminal conditions:
+        - pole angle is more than 12 degrees
+        - cart position is more than 2.5m from the center
+        - episode length is greater than 200 steps
+
+    References:
+        - [1] Cartpole environment in OpenAI Gym: https://gym.openai.com/envs/CartPole-v1/
+        - [2] "Neuronlike Adaptive Elements That Can Solve Difficult Learning Control Problem", Barto et al., 1993.
+    """
+
+    def __init__(self, simulator=None, num_links=1, num_steps=200, verbose=True):
+        """
+        Initialize the Cartpole environment.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            num_links (int): the number of links that forms the inverted pendulum.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # create basic world
+        world = prl.worlds.World(simulator)
+        robot = prl.robots.CartPole(simulator, position=(0., 0., 0.), num_links=num_links, inverted_pole=False)
+        world.load_robot(robot)
+        robot.disable_motor(robot.joints)
+        if verbose:
+            robot.print_info()
+
+        # create state: [x, \dot{x}, q_i, \dot{q}_i]
+        state = prl.states.JointPositionState(robot) + prl.states.JointVelocityState(robot)
+        if verbose:
+            print("\nState: {}".format(state))
+
+        # create action: f_cart = (-10., +10.)
+        action = prl.actions.JointForceAction(robot=robot, joint_ids=0, discrete_values=[-10., 10.])
+        if verbose:
+            print("\nAction: {}".format(action))
+
+        # create terminal condition
+        pole_angle_condition = prl.terminal_conditions.JointPositionCondition(robot, joint_ids=1,
+                                                                              bounds=(-12 * np.pi/180, 12 * np.pi/180),
+                                                                              out=False, stay=True)
+        cart_position_condition = prl.terminal_conditions.LinkPositionCondition(robot, link_id=1, bounds=(-1., 1.),
+                                                                                dim=0, out=False, stay=True)
+        time_length_condition = prl.terminal_conditions.TimeLimitCondition(num_steps=num_steps)
+        terminal_conditions = [pole_angle_condition, cart_position_condition, time_length_condition]
+
+        # create reward: +1 until termination step
+        reward = prl.rewards.TerminalReward(terminal_conditions=terminal_conditions, subreward=1., final_reward=1.)
+        if verbose:
+            print("\nReward: {}".format(state))
+
+        # 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(robot.joints)
+            return reset
+
+        initial_state_generator = prl.states.generators.UniformStateGenerator(state=state, low=-0.05, high=0.05,
+                                                                              fct=reset_robot(robot))
+
+        # create environment using composition
+        super(CartpoleEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                          terminal_conditions=terminal_conditions,
+                                          initial_state_generators=initial_state_generator)
+
+
+# class CartDoublePoleEnv(Env):
+#     r"""CartDoublepole Environment
+#
+#     This provide the double inverted poles on a cart environment. Compare to the standard inverted pendulum on a cart,
+#     the goal this time is to balance two poles of possibly different lengths / masses, which are initialized at
+#     different angles but connected at the same joint attached to the cart.
+#     """
+#
+#     def __init__(self, simulator=None, pole_lengths=(1., 1.), pole_masses=(1., 1.), pole_angles=(0., 0.)):
+#         """
+#         Initialize the double inverted poles on a cart environment.
+#
+#         Args:
+#             simulator (Simulator): simulator instance.
+#         """
+#         # create basic world
+#         world = prl.worlds.BasicWorld(simulator)
+#         robot = prl.robots.CartDoublePole(simulator, pole_lengths=pole_lengths, pole_masses=pole_masses,
+#                                           pole_angles=pole_angles)
+#         world.load_robot(robot)
+#
+#         # create state
+#         state =
+#
+#         # create action
+#         action =
+#
+#         # create reward
+#         reward =
+#
+#         # create terminal condition
+#         terminal_condition =
+#
+#         # create initial state generator
+#         initial_state_generator =
+#
+#         # create environment using composition
+#         super(CartDoublePoleEnv, self).__init__(world=world, states=state, rewards=reward, actions=action)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = CartpoleEnv(sim)
+
+    state = env.reset()
+    # run simulation
+    for _ in count():
+        state, reward, done, info = env.step(sleep_dt=1./240)
+        print("done: {}, reward: {}, state: {}".format(done, reward, state))
+
+    # # create basic world
+    # sim = prl.simulators.Bullet()
+    # world = prl.worlds.World(sim)
+    # robot = prl.robots.CartPole(sim, num_links=1, inverted_pole=True)
+    # robot.disable_motor(robot.joints)
+    # world.load_robot(robot)
+    #
+    # # create state: [x, \dot{x}, q_i, \dot{q}_i]
+    # state = prl.states.JointPositionState(robot) + prl.states.JointVelocityState(robot)
+    #
+    # # create action
+    # action = prl.actions.JointForceAction(robot=robot, joint_ids=0, discrete_values=[-10., 10.])
+    #
+    # flip = 1
+    # for i in prl.count():
+    #     # if i % 10 == 0:
+    #     #     flip = (flip+1) % 2
+    #     action(flip)
+    #     world.step(sleep_dt=sim.dt)
@@ -205,7 +205,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
            conditions = [conditions]
        elif isinstance(conditions, (list, tuple)):
            for idx, condition in enumerate(conditions):
-                if not callable(conditions):
+                if not isinstance(condition, TerminalCondition):
                    raise TypeError("Expecting the {} item in the given terminal conditions to be an instance of "
                                    "`TerminalCondition`, instead got: {}".format(idx, type(condition)))
        else:
@@ -293,7 +293,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env

        # generate initial states
        for generator in self.state_generators:
-            generator()
+            generator(reset_state=False)

        # reset states and return first states/observations
        states = [state.reset() for state in self.states]
@@ -194,8 +194,8 @@ class JointPhysicsRandomizer(BodyPhysicsRandomizer):

        # sample each property
        properties = dict()
-        for joint in self.joints:
+        for i, joint in enumerate(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])
+                    properties.setdefault(joint, {})[name] = np.random.uniform(low=bound[0][i], high=bound[1][i])
        return properties
@@ -429,8 +429,8 @@ class LinkPhysicsRandomizer(BodyPhysicsRandomizer):

        # sample each property
        properties = dict()
-        for link in self.links:
+        for i, link in enumerate(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])
+                    properties.setdefault(link, {})[name] = np.random.uniform(low=bound[0][i], high=bound[1][i])
        return properties
@@ -1,6 +1,8 @@
 Priority Tasks
 ==============

+THIS IS UNDER CONSTRUCTION
+
 In this folder, you will find the code for priority "tasks". The "tasks" defined here are different from the tasks 
 defined in the ``pyrobolearn/tasks`` folder which defines robot learning tasks. The tasks defined here can be more seen
 as "constraints"; for instance, the constraint for the robot to maintain its balance (i.e. have its center of mass 
@@ -50,6 +50,8 @@ class Acrobot(Robot):  # TODO: create the acrobot dynamically instead of loading
        super(Acrobot, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
        self.name = 'acrobot'

+        self.height = 2. * scale
+
        # set initial joint positions
        self.reset_joint_states(q=[np.pi, 0.], joint_ids=self.joints)

@@ -7,6 +7,8 @@ other joint actuators. Additionally, this is important as more realistic motors
 simulation to reality.
 """

+# TODO: add latency + noise
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
@@ -24,10 +26,17 @@ class Actuator(object):
    Other actuators such as speakers, leds, and others are attached to links.
    """

-    def __init__(self):
+    def __init__(self, latency=0):
+        """
+        Initialize the actuator.
+
+        Args:
+            latency (int, float): latency.
+        """

        # variable to check if the actuator is enabled
        self._enabled = True
+        self._latency = latency

    #     self.sim = simulator
    #
@@ -106,8 +106,6 @@ class CartPole(Robot):
        dims = (15, 0.025, 0.025)
        color = (0, 0.8, 0.8, 1)
        mass = 0
-        position = (0, 0, 0)
-        orientation = (0, 0, 0, 1)
        collision_shape = self.sim.create_collision_shape(self.sim.GEOM_BOX, half_extents=dims)
        visual_shape = self.sim.create_visual_shape(self.sim.GEOM_BOX, half_extents=dims, rgba_color=color)

@@ -50,6 +50,8 @@ class Pendulum(Robot):  # TODO: create the pendulum dynamically instead of loadi
        super(Pendulum, self).__init__(simulator, urdf, position, orientation, fixed_base, scale)
        self.name = 'pendulum'

+        self.height = 2. * scale
+
        # set initial joint positions
        self.reset_joint_states(q=[np.pi / 4], joint_ids=self.joints)

@@ -174,6 +174,15 @@ class Robot(ControllableBody):
    # Operators #
    #############

+    # def __str__(self):
+    #     """Return a string describing the robot."""
+    #     return "\nRobot: {} \nNumber of DoFs: {} \nJoint ids: {} \nActuated joint ids: {} " \
+    #            "\nLink names (associated with actuated joints): {} \nEnd-effector names: {} \nFloating base? {} " \
+    #            "\nTotal mass = {} kg".format(self.__class__.__name__, self.num_dofs, list(range(self.num_joints)),
+    #                                          self.joints, self.get_link_names(self.joints),
+    #                                          self.get_link_names(self.end_effectors), self.has_floating_base(),
+    #                                          self.mass)
+
    def __copy__(self):
        """Return a shallow copy of the robot. This can be overridden in the child class."""
        return self.__class__(simulator=self.simulator, urdf=self.urdf, position=self.position,
@@ -1226,21 +1235,38 @@ class Robot(ControllableBody):

        # check q
        if q is None:
-            q = np.zeros(len(joint_ids))
+            if 'q_reset' in self._state:
+                q = self._state['q_reset']
+            else:
+                q = np.zeros(len(joint_ids))
        elif isinstance(q, (int, float)):
            q = [q]
+            self._state['q_reset'] = q
        else:
            if len(q) != len(joint_ids):
                raise ValueError("The number of joint ids does not match up with the number of q's")
+            self._state['q_reset'] = q

        # check dq
        if dq is None:
-            dq = np.zeros(len(joint_ids))
+            if 'dq_reset' in self._state:
+                dq = self._state['dq_reset']
+            else:
+                dq = np.zeros(len(joint_ids))
        elif isinstance(dq, (int, float)):
            dq = [dq]
+            self._state['dq_reset'] = dq
        else:
            if len(dq) != len(joint_ids):
                raise ValueError("The number of joint ids does not match with the number of dq's")
+            self._state['dq_reset'] = dq
+
+        # import inspect
+        # stack = inspect.stack()
+        # the_class = stack[1][0].f_locals["self"].__class__
+        # the_method = stack[1][0].f_code.co_name
+        # print("I was called by {}.{}()".format(str(the_class), the_method))
+        # print("resetting: {}, {}, {}".format(joint_ids, q, dq))

        # reset the joint state
        for joint_id, position, velocity in zip(joint_ids, q, dq):
@@ -4457,7 +4483,7 @@ class Robot(ControllableBody):
                pos = self.get_link_world_positions(link)
                dim = self.visual_shapes[link]['dimensions']
                # radius = min(dim) * scaling * 0.2
-                radius = 0.01
+                radius = 0.01 * scaling
                self._draw_sphere(pos, radius, color=(0, 0, 0, 1))

    def draw_link_frames(self, link_ids=None, scaling=1.):
@@ -8,6 +8,8 @@ simulation to reality. Also, note that some simulators are deterministic and thu
 add some noise to the returned sense value. The type of noise can also be selected at runtime.
 """

+# TODO: add latency + noise
+
 import copy
 from abc import ABCMeta, abstractmethod
 import numpy as np
@@ -34,7 +36,7 @@ class Sensor(object):  # sensor attached to a link or joint
    """
    __metaclass__ = ABCMeta

-    def __init__(self, simulator, body_id, position=None, orientation=None, rate=1):
+    def __init__(self, simulator, body_id, position=None, orientation=None, rate=1, latency=0):
        """Initialize the sensor.

        Args:
@@ -43,6 +45,7 @@ class Sensor(object):  # sensor attached to a link or joint
            position (vec3): local position of the sensor with respect to the given link
            orientation (vec4): local orientation of the sensor with respect to the given link
            rate (int): number of steps to wait before acquisition of the next sensor value.
+            latency (int, float): latency.
        """
        self.sim = simulator
        self.body_id = body_id
@@ -63,6 +66,7 @@ class Sensor(object):  # sensor attached to a link or joint

        # variable to check if the sensor is enabled
        self._enabled = True
+        self._latency = latency

    ##############
    # Properties #
@@ -106,9 +106,9 @@ class StateGenerator(object):
        """Return a string describing the class."""
        return self.__class__.__name__

-    def __call__(self, set_data=True):
+    def __call__(self, set_data=True, reset_state=True):
        """Call the generator."""
-        return self.generate(set_data=set_data)
+        return self.generate(set_data=set_data, reset_state=reset_state)


 class FixedStateGenerator(StateGenerator):
@@ -474,10 +474,11 @@ class UniformStateGenerator(StateDistributionGenerator):
        # 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))
+        data = [np.random.uniform(low=low, high=high, size=state.total_size())
                for state, low, high in zip(self.state, self.low, self.high)]
        if set_data:
            self.state.data = data
+            # print("Generate: {}".format(self.state.data))
        if reset_state:
            self.state.reset()
        return data
@@ -1,3 +1,19 @@

 # import terminal conditions
-from .terminal_condition import *
+from .terminal_condition import TerminalCondition, HasFallenCondition, HasReachedCondition
+
+# import basic terminal conditions
+from .basic_conditions import TimeLimitCondition, GymTerminalCondition
+
+# import body terminal condition
+from .body_conditions import BodyCondition, PositionCondition, OrientationCondition
+
+# import robot terminal condition
+from .robot_condition import RobotCondition
+
+# import joint conditions
+from .joint_conditions import JointCondition, JointPositionCondition, JointVelocityCondition, \
+    JointAccelerationCondition, JointTorqueCondition
+
+# import link conditions
+from .link_conditions import LinkCondition, LinkPositionCondition, LinkOrientationCondition
@@ -0,0 +1,146 @@
+#!/usr/bin/env python
+"""Define some common terminal conditions for the environment.
+"""
+
+import copy
+import numpy as np
+
+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 TimeLimitCondition(TerminalCondition):
+    r"""Time Limit Terminal Condition
+
+    """
+
+    def __init__(self, num_steps, btype='neutral'):
+        """
+        Initialize the time limit terminal condition.
+
+        Args:
+            num_steps (int):
+            btype (bool, str, None): if the terminal condition represents a failure or success condition. If None, it
+                represents a neutral terminal condition (which is neither a failure or success condition, but just
+                means the episode is over). If string, it has to be among {"success", "failure", "neutral"}.
+        """
+        super(TimeLimitCondition, self).__init__(btype=btype)
+        self.num_steps = num_steps
+        self.cnt = 0
+
+    def reset(self):
+        """
+        Reset the terminal condition.
+        """
+        self.cnt = 0
+
+    def check(self):
+        """
+        Check if the terminating condition has been fulfilled, and return True or False accordingly
+        """
+        if self.cnt >= self.num_steps:
+            self._over = True
+            return self._over
+        self.cnt += 1
+        return self._over
+
+
+class GymTerminalCondition(TerminalCondition):
+    r"""OpenAI Gym Terminal Condition
+
+    Returns if the OpenAI Gym environment has terminated. This does not provide any information if the environment
+    terminated because the policy succeeded or failed to perform the task.
+    """
+
+    def __init__(self, done=False):
+        """
+        Initialize the Gym terminal condition.
+
+        Args:
+            done (bool): True if done.
+        """
+        super(GymTerminalCondition, self).__init__(btype='neutral')  # None because we don't know
+        self._value = done
+
+    def check(self):
+        """Check if the terminating condition has been fulfilled, and return True or False accordingly"""
+        return self._value
+
+
+class FixedTerminalCondition(TerminalCondition):
+    r"""Fixed terminal condition
+
+    Dummy fixed terminal condition.
+    """
+
+    def __init__(self, value=False, btype=None, name=None):
+        """
+        Initialize the dummy fixed terminal condition.
+
+        Args:
+            value (bool): if the terminal condition is over or not. This can be set by the user.
+            btype (bool, str, None): if the terminal condition represents a failure or success condition. If None, it
+                represents a neutral terminal condition (which is neither a failure or success condition, but just
+                means the episode is over). If string, it has to be among {"success", "failure", "neutral"}.
+            name (str): name of the final condition
+        """
+        super(FixedTerminalCondition, self).__init__(btype=btype, name=name)
+        self._over = value
+
+    @property
+    def value(self):
+        return self._over
+
+    @value.setter
+    def value(self, value):
+        self._over = bool(value)
+
+
+class FunctionalTerminalCondition(TerminalCondition):
+    r"""Functional terminal condition
+
+    Terminal condition that calls the given function every time we check if the task was carried out or not.
+    """
+
+    def __init__(self, fct, btype=None, name=None):
+        """
+        Initialize the functional terminal condition.
+
+        Args:
+            fct (callable): function to call each time we check if the task was carried out or not.
+            btype (bool, str, None): if the terminal condition represents a failure or success condition. If None, it
+                represents a neutral terminal condition (which is neither a failure or success condition, but just
+                means the episode is over). If string, it has to be among {"success", "failure", "neutral"}.
+            name (str): name of the final condition
+        """
+        if not callable(fct):
+            raise TypeError("Expecting the provided 'fct' to be callable, but received instead: {}".format(fct))
+        self._fct = fct
+
+    def check(self):
+        """
+        Check if the terminating condition has been fulfilled, and return True or False accordingly
+        """
+        answer = self._fct()
+        if isinstance(answer, tuple) and len(answer) == 2:
+            self._over, self.btype = answer
+        return self._over
+
+
+# Tests
+if __name__ == '__main__':
+
+    condition = TimeLimitCondition(num_steps=2, btype='failure')
+
+    for i in range(4):
+        cond = condition()
+        print("Iter: {}, type={}, over={}".format(i, condition.type_str(), condition.is_over()))
@@ -0,0 +1,340 @@
+#!/usr/bin/env python
+"""Define some body terminal conditions for the environment.
+"""
+
+from abc import ABCMeta
+import numpy as np
+
+from pyrobolearn.robots.base import Body
+from pyrobolearn.terminal_conditions.terminal_condition import TerminalCondition
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion
+
+
+__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 BodyCondition(TerminalCondition):
+    r"""Body Terminal Condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the dimensions of the body state are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the dimension of the body state is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+
+    Body state includes the position and orientation for instance.
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self, body, bounds, dim=None, out=False, stay=False, all=False):
+        """
+        Initialize the body terminal condition.
+
+        Args:
+            body (Body): body instance
+            dim (None, int, int[3]): dimensions that we should consider when looking at the bounds. If None, it will
+                consider all 3 dimensions. If one dimension is provided it will only check along that dimension. If
+                a np.array of 0 and 1 is provided, it will consider the dimensions that are equal to 1. Thus, [1,0,1]
+                means to consider the bounds along the x and z axes.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the state
+                leave the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the state leaves the bounds, it results in a success.
+            all (bool): this is only used if they are multiple dimensions. if True, all the dimensions of the state
+                are checked if they are inside or outside the bounds depending on the other parameters. if False, any
+                dimensions will be checked.
+        """
+        super(BodyCondition, self).__init__()
+        self.body = body
+        self.dim = dim
+        self.bounds = bounds
+        self._out = bool(out)
+        self._stay = bool(stay)
+        self._all = bool(all)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def body(self):
+        """Return the body instance."""
+        return self._body
+
+    @body.setter
+    def body(self, body):
+        """Set the body instance."""
+        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
+    def dim(self):
+        """Return the dimension(s)."""
+        return self._dim
+
+    @dim.setter
+    def dim(self, dim):
+        """Set the dimensions."""
+        if dim is not None:
+            if not isinstance(dim, (int, np.ndarray)):
+                raise TypeError("Expecting the given 'dim' to be an int or an np.array of 3 int, but got instead: "
+                                "{}".format(type(dim)))
+            if isinstance(dim, np.ndarray):
+                if dim.size != 3:
+                    raise ValueError("Expecting the given 'dim' np.array to be of size 3, but got instead a size of: "
+                                     "{}".format(dim.size))
+                dim = np.array([bool(d) for d in dim])
+        self._dim = dim
+
+    ###########
+    # Methods #
+    ###########
+
+    def _check_bounds(self, bounds):
+        """Check the given bounds."""
+        # check the type of the bounds
+        if not isinstance(bounds, (tuple, list, np.ndarray)):
+            raise TypeError("Expecting the given bounds to be a tuple/list/np.ndarray of float, instead got: "
+                            "{}".format(type(bounds)))
+
+        # check that the bounds have a length of 2 (i.e. lower and upper bounds)
+        if len(bounds) != 2:
+            raise ValueError("Expecting the bounds to be of length 2 (i.e. lower and upper bounds), instead got a "
+                             "length of {}".format(len(bounds)))
+
+        # if one of the bounds is None, raise error
+        if bounds[0] is None or bounds[1] is None:
+            raise ValueError("Expecting the bounds to not have None, but got: {}".format(bounds))
+
+        # reshape bounds if necessary
+        bounds = np.asarray(bounds).reshape(2, -1)
+
+        if self.dim is None:
+            if bounds.shape[1] != 3:
+                raise ValueError("Expecting the bounds to be of shape (2,3) but got instead a shape of: "
+                                 "{}".format(bounds.shape))
+        else:
+            if isinstance(self.dim, int) and bounds.shape[1] != 1:
+                raise ValueError("If you specified one dimension, we expect the shape of the bounds to be (2,1), but "
+                                 "got instead a shape of: {}".format(bounds.shape))
+            elif isinstance(self.dim, np.ndarray):
+                if bounds.shape[1] != len(self.dim[self.dim]):
+                    raise ValueError("Expecting each bound to have the same number of elements than the elements that "
+                                     "are not zero in the given 'dim' attribute")
+        return bounds
+
+    def check(self):
+        """
+        Check if the terminating condition has been fulfilled, and return True or False accordingly
+        """
+        states = self._get_states()
+
+        if self._all:  # all the dimension states
+            if self._out:  # are outside a certain bounds
+                if self._stay:  # and must stay outside these ones.
+                    if np.any(self.bounds[0] <= states <= self.bounds[1]):  # one dimension went inside
+                        self._btype = False     # failure
+                        self._over = True       # it is over
+                    else:  # they are still all outside
+                        self._btype = True      # success
+                        self._over = False      # it is not over
+                else:  # and must go inside these ones
+                    if np.all(self.bounds[0] <= states <= self.bounds[1]):  # they all went inside
+                        self._btype = True      # success
+                        self._over = True       # it is over
+                    else:  # they are some still left outside
+                        self._btype = False     # failure
+                        self._over = False      # it is not over
+            else:  # are inside a certain bounds
+                if self._stay:  # and must stay inside these ones.
+                    if not np.all(self.bounds[0] <= states <= self.bounds[1]):  # one dimension went outside
+                        self._btype = False  # failure
+                        self._over = True    # it is over
+                    else:  # they are still all inside
+                        self._btype = True  # success
+                        self._over = False  # it is not over
+                else:  # and must go outside these ones.
+                    if np.any(self.bounds[0] <= states <= self.bounds[1]):  # they are still some inside
+                        self._btype = False  # failure
+                        self._over = False   # it is not over
+                    else:  # they are all outside
+                        self._btype = True   # success
+                        self._over = True    # it is over
+
+        else:  # any of the dimension states
+            if self._out:  # is outside a certain bounds
+                if self._stay:  # and still stays outside these ones.
+                    if not np.all(self.bounds[0] <= states <= self.bounds[1]):  # at least one dim. is still outside
+                        self._btype = True   # success
+                        self._over = False   # it is not over
+                    else:  # they are all inside
+                        self._btype = False   # failure
+                        self._over = True     # it is over
+                else:  # and one must at least go inside these ones
+                    if np.any(self.bounds[0] <= states <= self.bounds[1]):  # at least one state is inside
+                        self._btype = True  # success
+                        self._over = True   # it is over
+                    else:  # they are still all outside
+                        self._btype = False  # failure
+                        self._over = False   # it is not over
+            else:  # is inside a certain bounds
+                if self._stay:  # and must stay inside these ones.
+                    if np.any(self.bounds[0] <= states <= self.bounds[1]):  # at least one state is still inside
+                        self._btype = True   # success
+                        self._over = False   # it is not over
+                    else:  # they are all outside
+                        self._btype = False  # failure
+                        self._over = True    # it is over
+                else:  # and must go outside these ones.
+                    if np.all(self.bounds[0] <= states <= self.bounds[1]):  # they are all inside
+                        self._btype = False  # failure
+                        self._over = False   # it is not over
+                    else:  # at least one went outside
+                        self._btype = True   # success
+                        self._over = True    # it is over
+
+        return self._over
+
+    def _get_states(self):
+        """Get the base states. Has to be implemented in the child class."""
+        raise NotImplementedError
+
+
+class PositionCondition(BodyCondition):
+    r"""World position terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the dimensions of the body position state are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the dimension of the body position state is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, body, bounds=(None, None), dim=None, out=False, stay=False, all=False):
+        """
+        Initialize the world position terminal condition.
+
+        Args:
+            body (Body): body instance.
+            bounds (tuple of 2 float / np.array[3]): bounds on the body position.
+            dim (None, int, int[3]): dimensions that we should consider when looking at the bounds. If None, it will
+                consider all 3 dimensions. If one dimension is provided it will only check along that dimension. If
+                a np.array of 0 and 1 is provided, it will consider the dimensions that are equal to 1. Thus, [1,0,1]
+                means to consider the bounds along the x and z axes.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the position
+                leaves the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the position leaves the bounds, it results in a success.
+            all (bool): this is only used if they are multiple dimensions. if True, all the dimensions of the state
+                are checked if they are inside or outside the bounds depending on the other parameters. if False, any
+                dimensions will be checked.
+        """
+        super(PositionCondition, self).__init__(body, bounds=bounds, dim=dim, out=out, stay=stay, all=all)
+        # check the bounds
+        self.bounds = self._check_bounds(bounds=bounds)
+
+    def _get_states(self):
+        """Return the state."""
+        position = self.body.position
+        if self.dim is None:
+            print(position)
+            return position
+
+        print(position[self.dim])
+        return position[self.dim]
+
+
+class OrientationCondition(BodyCondition):
+    r"""World orientation terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the dimensions of the body orientation (expressed as roll-pitch-yaw angles) state are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the dimension of the body orientation (expressed as roll-pitch-yaw angles) state is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, body, bounds=(None, None), dim=None, out=False, stay=False, all=False):
+        """
+        Initialize the world position terminal condition.
+
+        Args:
+            body (Body): body instance.
+            bounds (tuple of 2 float / np.array[3]): bounds on the body orientation expressed as roll-pitch-yaw angles
+                or axis-angle if the :attr:`axis` is provided.
+            dim (None, int, int[3]): dimensions that we should consider when looking at the bounds. If None, it will
+                consider all 3 dimensions. If one dimension is provided it will only check along that dimension. If
+                a np.array of 0 and 1 is provided, it will consider the dimensions that are equal to 1. Thus, [1,0,1]
+                means to consider the bounds along the x (roll) and z (yaw) axes.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the orientation
+                leaves the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the orientation leaves the bounds, it results in a success.
+            all (bool): this is only used if they are multiple dimensions. if True, all the dimensions of the state
+                are checked if they are inside or outside the bounds depending on the other parameters. if False, any
+                dimensions will be checked.
+        """
+        super(OrientationCondition, self).__init__(body, bounds=bounds, dim=dim, out=out, stay=stay, all=all)
+        # check the bounds
+        self.bounds = self._check_bounds(bounds=bounds)
+
+    def _get_states(self):
+        """Return the state."""
+        orientation = get_rpy_from_quaternion(self.body.orientation)
+        if self.dim is None:
+            return orientation
+        return orientation[self.dim]
@@ -0,0 +1,299 @@
+#!/usr/bin/env python
+"""Define some joint terminal conditions for the environment.
+"""
+
+import copy
+import numpy as np
+from abc import ABCMeta
+
+from pyrobolearn.terminal_conditions.robot_condition import RobotCondition
+
+
+__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 JointCondition(RobotCondition):
+    r"""Joint Terminal Condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the joint states are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the joint states is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+
+    States include positions, velocities, accelerations and torques.
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), out=False, stay=False, all=False):
+        """
+        Initialize the joint terminal condition.
+
+        Args:
+            robot (Robot): robot instance
+            joint_ids (int, int[N], None): joint id or list of joint ids
+            bounds (tuple of float / np.array[N]): bounds to stay in/out or reach/leave.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the joint state
+                leaves the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the joint state leaves the bounds, it results in a success.
+            all (bool): this is only used if they are multiple joints. if True, all the joints are checked such that
+                they are inside or outside the bounds depending on the other parameters. if False, any joints will be
+                checked.
+        """
+        super(JointCondition, self).__init__(robot, bounds=bounds, dim=None, out=out, stay=stay, all=all)
+
+        # get the joints of the robot
+        if joint_ids is None:
+            joint_ids = robot.get_joint_ids()
+        elif isinstance(joint_ids, int):
+            joint_ids = [joint_ids]
+        self.joints = joint_ids
+
+        # check the bounds
+        self.bounds = self._check_bounds(bounds=bounds)
+
+    def _check_bounds(self, bounds):
+        """Check the given bounds."""
+        # check the type of the bounds
+        if not isinstance(bounds, (tuple, list, np.ndarray)):
+            raise TypeError("Expecting the given bounds to be a tuple/list/np.ndarray of float, instead got: "
+                            "{}".format(type(bounds)))
+
+        # check that the bounds have a length of 2 (i.e. lower and upper bounds)
+        if len(bounds) != 2:
+            raise ValueError("Expecting the bounds to be of length 2 (i.e. lower and upper bounds), instead got a "
+                             "length of {}".format(len(bounds)))
+
+        # if one of the bounds is None, raise error
+        if bounds[0] is None or bounds[1] is None:
+            raise ValueError("Expecting the bounds to not have None, but got: {}".format(bounds))
+
+        # reshape bounds if necessary
+        bounds = np.asarray(bounds).reshape(2, -1)
+        if len(self.joints) != bounds.shape[1]:
+            if bounds.shape[1] == 1:
+                bounds = np.array([bounds[0, 0] * np.ones(len(self.joints)),
+                                   bounds[1, 0] * np.ones(len(self.joints))])
+            else:
+                raise ValueError("Expecting the number of bounds (={}) to match up with the number of joints "
+                                 "(={})".format(bounds.shape[1], len(self.joints)))
+        return bounds
+
+
+class JointPositionCondition(JointCondition):
+    r"""Joint position terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the joint positions are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the joint positions is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), out=False, stay=False, all=False):
+        """
+        Initialize the joint position terminal condition.
+
+        Args:
+            robot (Robot): robot instance
+            joint_ids (int, int[N], None): joint id or list of joint ids
+            bounds (tuple of float / np.array[N]): bounds to stay in/out or reach/leave.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the joint positions
+                leave the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the joint positions leave the bounds, it results in a success.
+            all (bool): this is only used if they are multiple joints. if True, all the joints are checked such that
+                they are inside or outside the bounds depending on the other parameters. if False, any joints will be
+                checked.
+        """
+        super(JointPositionCondition, self).__init__(robot, joint_ids=joint_ids, bounds=bounds, out=out, stay=stay,
+                                                     all=all)
+
+    def _get_states(self):
+        """Get the joint position states."""
+        return self.robot.get_joint_positions(self.joints)
+
+
+class JointVelocityCondition(JointCondition):
+    r"""Joint velocity terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the joint velocities are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the joint velocities is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), out=False, stay=False, all=False):
+        """
+        Initialize the joint velocity terminal condition.
+
+        Args:
+            robot (Robot): robot instance
+            joint_ids (int, int[N], None): joint id or list of joint ids
+            bounds (tuple of float / np.array[N]): bounds to stay in/out or reach/leave.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the joint velocities
+                leave the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the joint velocities leave the bounds, it results in a success.
+            all (bool): this is only used if they are multiple joints. if True, all the joints are checked such that
+                they are inside or outside the bounds depending on the other parameters. if False, any joints will be
+                checked.
+        """
+        super(JointVelocityCondition, self).__init__(robot, joint_ids=joint_ids, bounds=bounds, out=out, stay=stay,
+                                                     all=all)
+
+    def _get_states(self):
+        """Get the joint position states."""
+        return self.robot.get_joint_velocities(self.joints)
+
+
+class JointAccelerationCondition(JointCondition):
+    r"""Joint acceleration terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the joint accelerations are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the joint accelerations is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), out=False, stay=False, all=False):
+        """
+        Initialize the joint acceleration terminal condition.
+
+        Args:
+            robot (Robot): robot instance
+            joint_ids (int, int[N], None): joint id or list of joint ids
+            bounds (tuple of float / np.array[N]): bounds to stay in/out or reach/leave.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the joint
+                accelerations leave the bounds it results in a failure. if :attr:`stay` is False, it must get outside
+                these bounds; if the joint accelerations leave the bounds, it results in a success.
+            all (bool): this is only used if they are multiple joints. if True, all the joints are checked such that
+                they are inside or outside the bounds depending on the other parameters. if False, any joints will be
+                checked.
+        """
+        super(JointAccelerationCondition, self).__init__(robot, joint_ids=joint_ids, bounds=bounds, out=out, stay=stay,
+                                                         all=all)
+
+    def _get_states(self):
+        """Get the joint position states."""
+        return self.robot.get_joint_accelerations(self.joints)
+
+
+class JointTorqueCondition(JointCondition):
+    r"""Joint torque terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the joint torques are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the joint torques is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), out=False, stay=False, all=False):
+        """
+        Initialize the joint torque terminal condition.
+
+        Args:
+            robot (Robot): robot instance
+            joint_ids (int, int[N], None): joint id or list of joint ids
+            bounds (tuple of float / np.array[N]): bounds to stay in/out or reach/leave.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the joint torques
+                leave the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the joint positions leave the bounds, it results in a success.
+            all (bool): this is only used if they are multiple joints. if True, all the joints are checked such that
+                they are inside or outside the bounds depending on the other parameters. if False, any joints will be
+                checked.
+        """
+        super(JointTorqueCondition, self).__init__(robot, joint_ids=joint_ids, bounds=bounds, out=out, stay=stay,
+                                                   all=all)
+
+    def _get_states(self):
+        """Get the joint position states."""
+        return self.robot.get_joint_torques(self.joints)
@@ -0,0 +1,230 @@
+#!/usr/bin/env python
+"""Define some link terminal conditions for the environment.
+"""
+
+import copy
+import numpy as np
+from abc import ABCMeta
+
+from pyrobolearn.terminal_conditions.robot_condition import RobotCondition
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion
+
+
+__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 LinkCondition(RobotCondition):
+    r"""Link Terminal Condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the dimensions of the link state are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the dimension of the link state is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+
+    link states include its position, orientation, or velocity for instance.
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self, robot, link_id, wrt_link_id=None, bounds=(None, None), dim=None, out=False, stay=False, all=all):
+        """
+        Initialize the link terminal condition.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id.
+            wrt_link_id (None, int): link id wrt which the link state is based on. if None, the state will be with
+                respect to the world frame. If -1, it is wrt the base frame.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the link state
+                leaves the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the link state leaves the bounds, it results in a success.
+            all (bool): this is only used if they are multiple dimensions. if True, all the dimensions of the state
+                are checked if they are inside or outside the bounds depending on the other parameters. if False, any
+                dimensions will be checked.
+        """
+        super(LinkCondition, self).__init__(robot, bounds=bounds, dim=dim, out=out, stay=stay, all=all)
+
+        # set link
+        if not isinstance(link_id, int):
+            raise TypeError("Expecting the given 'link_id' to be an int, but instead got: {}".format(type(link_id)))
+        self.link = link_id
+
+        # set wrt_link_id
+        if wrt_link_id is not None and not isinstance(wrt_link_id, int):
+            raise TypeError("Expecting the given 'wrt_link_id' to be an int, but instead got: "
+                            "{}".format(type(wrt_link_id)))
+        self.wrt_link = wrt_link_id
+
+        # check bounds
+        self.bounds = self._check_bounds(bounds)
+
+    def _check_bounds(self, bounds):
+        """Check the given bounds."""
+        # check the type of the bounds
+        if not isinstance(bounds, (tuple, list, np.ndarray)):
+            raise TypeError("Expecting the given bounds to be a tuple/list/np.ndarray of float, instead got: "
+                            "{}".format(type(bounds)))
+
+        # check that the bounds have a length of 2 (i.e. lower and upper bounds)
+        if len(bounds) != 2:
+            raise ValueError("Expecting the bounds to be of length 2 (i.e. lower and upper bounds), instead got a "
+                             "length of {}".format(len(bounds)))
+
+        # if one of the bounds is None, raise error
+        if bounds[0] is None or bounds[1] is None:
+            raise ValueError("Expecting the bounds to not have None, but got: {}".format(bounds))
+
+        # reshape bounds if necessary
+        bounds = np.asarray(bounds).reshape(2, -1)
+        return bounds
+
+
+class LinkPositionCondition(LinkCondition):
+    r"""Link position terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the dimensions of the link position state are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the dimension of the link position state is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+    """
+
+    def __init__(self, robot, link_id=None, wrt_link_id=None, bounds=(None, None), dim=None, out=False, stay=False,
+                 all=False):
+        """
+        Initialize the link position terminal condition.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int, int[N], None): link id or list of link ids.
+            wrt_link_id (None, int): link id wrt which the position is based on. if None, the position will be with
+                respect to the world frame. If -1, it is wrt the base frame.
+            bounds (tuple of 2 np.array[3] / np.array[N,3], np.array[2,3], np.array[2,N,3]): bounds to stay in/out or
+                reach/leave.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the link position
+                leaves the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the link position leaves the bounds, it results in a success.
+        """
+        super(LinkPositionCondition, self).__init__(robot, link_id=link_id, wrt_link_id=wrt_link_id, bounds=bounds,
+                                                    dim=dim, out=out, stay=stay, all=all)
+
+    def _get_states(self):
+        """Return the link position state."""
+        # get the link position
+        if self.wrt_link is None:
+            position = self.robot.get_link_world_positions(link_ids=self.link, flatten=True)
+        else:
+            position = self.robot.get_link_positions(link_ids=self.link, wrt_link_id=self.wrt_link, flatten=True)
+
+        if self.dim is None:
+            return position
+        return position[self.dim]
+
+
+class LinkOrientationCondition(LinkCondition):
+    r"""Link orientation terminal condition
+
+    This terminal condition describes 8 cases:
+
+    1. all the dimensions of the link orientation (expressed as roll-pitch-yaw angles) state are:
+        1. in a certain bounds and must stay between these bounds. Once one gets out, the terminal condition is over,
+           and results in a failure. (all=True, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once they all get out, the terminal condition is over,
+           and results in a success. (all=True, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once they all get in, the terminal condition is over, and results
+           in a success. (all=True, out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once one gets in, the terminal condition is over,
+           and results in a failure. (all=True, out=True, stay=True)
+    2. any of the dimension of the link orientation (expressed as roll-pitch-yaw angles) state is:
+        1. in a certain bounds and must stay between these bounds. Once they all get out, the terminal condition is
+           over, and results in a failure. (all=False, out=False, stay=True)
+        2. in a certain bounds and must get out of these bounds. Once one gets out, the terminal condition is over,
+           and results in a success. (all=False, out=False, stay=False)
+        3. outside a certain bounds and must get in. Once one gets in, the terminal condition is over, and results in
+           a success. (all=False ,out=True, stay=False)
+        4. outside a certain bounds and must stay outside these ones. Once they all get in, the terminal condition is
+           over, and results in a failure. (all=False, out=True, stay=True)
+
+    Warnings: the orientation is expressed as roll-pitch-yaw angles.
+    """
+
+    def __init__(self, robot, link_id, wrt_link_id=None, bounds=(None, None), dim=None, out=False, stay=False,
+                 all=False):
+        """
+        Initialize the link orientation terminal condition.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int, None): link id or list of link ids.
+            wrt_link_id (None, int): link id wrt which the orientation is based on. if None, the orientation will be
+                with respect to the world frame. If -1, it is wrt the base frame.
+            bounds (tuple of 2 float / np.array[3], np.array[2], np.array[2,3]): bounds to stay in/out or reach/leave.
+                the orientation is expressed as roll-pitch-yaw angles.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the link
+                orientation leaves the bounds it results in a failure. if :attr:`stay` is False, it must get outside
+                these bounds; if the link orientation leaves the bounds, it results in a success.
+        """
+        super(LinkOrientationCondition, self).__init__(robot, link_id=link_id, wrt_link_id=wrt_link_id, bounds=bounds,
+                                                       dim=dim, out=out, stay=stay, all=all)
+
+    def _get_states(self):
+        """Return the link orientation state."""
+        # get the link orientation
+        if self.wrt_link is None:
+            orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=True)
+        else:
+            orientation = self.robot.get_link_orientations(link_ids=self.link, wrt_link_id=self.wrt_link, flatten=True)
+
+        # convert from quaternion to roll-pitch-yaw angles
+        orientation = get_rpy_from_quaternion(orientation)
+
+        # return the proper orientation
+        if self.dim is None:
+            return orientation
+        return orientation[self.dim]
+
+
+# class LinkVelocityCondition(LinkCondition):
+#     r"""Link velocity terminal condition
+#     """
+#     raise NotImplementedError
@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+"""Define some robot terminal conditions for the environment.
+"""
+
+from abc import ABCMeta
+
+from pyrobolearn.robots.robot import Robot
+from pyrobolearn.terminal_conditions.body_conditions import BodyCondition
+
+
+__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 RobotCondition(BodyCondition):
+    r"""Robot Terminal Condition
+
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self, robot, bounds=(None, None), dim=None, out=False, stay=False, all=False):
+        """
+        Initialize the robot terminal condition.
+
+        Args:
+            robot (Robot): robot instance
+            dim (None, int, int[3]): dimensions that we should consider when looking at the bounds. If None, it will
+                consider all 3 dimensions. If one dimension is provided it will only check along that dimension. If
+                a np.array of 0 and 1 is provided, it will consider the dimensions that are equal to 1. Thus, [1,0,1]
+                means to consider the bounds along the x and z axes.
+            out (bool): if True, we are outside the provided bounds. If False, we are inside the provided bounds.
+            stay (bool): if True, it must stay in the bounds defined by in_bounds or out_bounds; if the state
+                leave the bounds it results in a failure. if :attr:`stay` is False, it must get outside these bounds;
+                if the state leaves the bounds, it results in a success.
+            all (bool): this is only used if they are multiple dimensions. if True, all the dimensions of the state
+                are checked if they are inside or outside the bounds depending on the other parameters. if False, any
+                dimensions will be checked.
+        """
+        super(RobotCondition, self).__init__(robot, bounds=bounds, dim=dim, out=out, stay=stay, all=all)
+        self.robot = robot
+
+    @property
+    def robot(self):
+        """Return the robot instance."""
+        return self._robot
+
+    @robot.setter
+    def robot(self, robot):
+        """Set the robot instance."""
+        if not isinstance(robot, Robot):
+            raise TypeError("Expecting the given 'robot' to be an instance of `Robot`, instead got: "
+                            "{}".format(type(robot)))
+        self._robot = robot
@@ -35,55 +35,128 @@ class TerminalCondition(object):
    """
    # TODO: we should be able to combine different conditions using 'OR' and 'AND'

+    def __init__(self, btype=None, name=None):
+        """
+        Initialize the terminal condition.
+
+        Args:
+            btype (bool, str, None): if the terminal condition represents a failure or success condition. If None, it
+                represents a neutral terminal condition (which is neither a failure or success condition, but just
+                means the episode is over). If string, it has to be among {"success", "failure", "neutral"}.
+            name (str): name of the final condition
+        """
+        self.btype = btype
+        self._over = False
+        self._achieved = False
+        self.name = name
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def btype(self):
+        """Return if the terminal condition is a neutral (None), a failure (False) or a success (True) condition."""
+        return self._btype
+
+    @btype.setter
+    def btype(self, value):
+        """Set the success variable."""
+        if value is not None and not isinstance(value, bool) and not isinstance(value, str):
+            raise TypeError("Expecting the given 'btype' variable to be a boolean, string, or None, but got instead: "
+                            "{}".format(type(value)))
+        if isinstance(value, str):
+            if value == 'neutral':
+                value = None
+            elif value == 'failure':
+                value = False
+            elif value == 'success':
+                value = True
+            else:
+                raise ValueError("Expecting the given string to be among ['success', 'failure', 'neutral'], but "
+                                 "instead got: {}".format(value))
+        self._btype = value
+
+    ###########
+    # Methods #
+    ###########
+
+    def is_over(self):
+        """Return if the condition is over or not."""
+        return self._over
+
+    def succeeded(self):
+        """Return if the condition succeeded or not."""
+        return self._achieved
+
+    def type(self):
+        """Return if it a success, failure, or neutral terminal condition."""
+        return self._btype
+
+    def type_str(self):
+        """Return the string representing the type of terminal condition."""
+        if self._btype is None:
+            return "neutral"
+        if self._btype:
+            return "success"
+        return "failure"
+
+    def reset(self):
+        """
+        Reset the terminal condition.
+        """
+        self._over = False
+
    def check(self):
        """
        Check if the terminating condition has been fulfilled, and return True or False accordingly
        """
-        return False
+        return self._over
+
+    #############
+    # Operators #
+    #############

    def __str__(self):
+        """Return a string describing the terminal condition."""
        return self.__class__.__name__

-    def __call__(self, *args, **kwargs):
+    def __call__(self):
+        """Check the terminal condition."""
        return self.check()

    def __bool__(self):
+        """Return a bool based on the terminating condition."""
        return self.check()

    __nonzero__ = __bool__


-class FailedCondition(TerminalCondition):
-    r"""Failed Terminal Condition
-
-    This determines when a policy or multiple ones have failed to perform a certain task.
-    """
-    pass
+# class FailedCondition(TerminalCondition):
+#     r"""Failed Terminal Condition
+#
+#     This determines when a policy or multiple ones have failed to perform a certain task.
+#     """
+#     pass
+#
+#
+# class NeutralCondition(TerminalCondition):
+#     r"""Neutral Terminal Condition
+#
+#     This determines when an environment has ended.
+#     """
+#     pass
+#
+#
+# class SucceededCondition(TerminalCondition):
+#     r"""Succeeded Terminal Condition
+#
+#     This determines when a policy or multiple ones have succeeded to perform a certain task.
+#     """
+#     pass


-class SucceededCondition(TerminalCondition):
-    r"""Succeeded Terminal Condition
-
-    This determines when a policy or multiple ones have succeeded to perform a certain task.
-    """
-    pass
-
-
-class GymTerminalCondition(TerminalCondition):
-    r"""OpenAI Gym Terminal Condition
-
-    Returns if the OpenAI Gym environment has terminated. This does not provide any information if the environment
-    terminated because the policy succeeded or failed to perform the task.
-    """
-
-    def __init__(self, done=False):
-        self.done = done
-
-    def check(self):
-        return self.done
-
-
-class HasFallen(FailedCondition):
+class HasFallenCondition(TerminalCondition):
    r"""Has Fallen Condition

    Check if the given robot has fallen, by checking if its base is below a certain threshold.
@@ -103,6 +176,7 @@ class HasFallen(FailedCondition):
                robot has fallen. Normally, the initial robot base up vector points upward. By default, it is 30
                degrees (=pi/6 rad).
        """
+        super(HasFallenCondition, self).__init__(btype='failure')
        self.robot = robot
        self.height_threshold = height_threshold if height_threshold is not None else robot.base_height/3.
        self.angle_threshold = angle_threshold
@@ -125,6 +199,7 @@ class HasFallen(FailedCondition):
        return height_condition or angle_condition

    def __str__(self):
+        """Return a string describing the terminal condition."""
        description = '{} (\n\tbase_height={} ?<? height_threshold={}, \n\tangle_up_vector={} ?>? angle_threshold={}' \
                      '\n)'.format(self.__class__.__name__, self._compute_height(), self.height_threshold,
                                   self._compute_angle(), self.angle_threshold)
@@ -150,16 +225,18 @@ class HasFallen(FailedCondition):
        return terminal


-class HasReached(SucceededCondition):
+class HasReachedCondition(TerminalCondition):
    r"""Has Reached Condition

    Check if the robot or a part of it has reached a certain position, configuration, or state for a certain amount
    of time/steps.
    """
-    pass
+
+    def __init__(self):
+        super(HasReachedCondition, self).__init__(btype='success')


-class LinkInSpecifiedDirection(HasReached):
+class LinkInSpecifiedDirection(HasReachedCondition):
    r"""Check if the specified link is in certain direction for a certain amount of time steps.

    Specifically, it checks if the position of the link with respect to the world or another link is in a certain
@@ -168,6 +245,7 @@ class LinkInSpecifiedDirection(HasReached):
    """

    def __init__(self, state, direction, domain=(0.95, 1.), total_steps=0):
+        super(LinkInSpecifiedDirection, self).__init__()
        if not isinstance(state, LinkState):
            raise TypeError("Expecting the state to be an instance of LinkState, instead got: {}".format(type(state)))
        self.state = state
@@ -209,6 +287,7 @@ class LinkInSpecifiedDirection(HasReached):
        return False

    def __str__(self):
+        """Return a string describing the terminal condition."""
        return self.__class__.__name__ + '(direction=' + str(self.direction) + ')'

    def __copy__(self):