add terminal conditions

pyrobolearn/envs/__init__.py

@@ -5,5 +5,3 @@ from .env import Env, BasicEnv
 # define wrapper for the gym environment
 from . import gym_wrapper as gym
 
-# import terminal conditions
-from .terminating_condition import *

pyrobolearn/envs/env.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-"""Define the `Env` class class which defines the world, states, and possible rewards. This is the main object
+"""Define the `Env` class which defines the world, states, and possible rewards. This is the main object
 a policy interacts with.
 
 Dependencies:
@@ -7,6 +7,7 @@ Dependencies:
 - `pyrobolearn.states`
 - `pyrobolearn.actions`
 - (`pyrobolearn.rewards`)
+- (`pyrobolearn.envs.terminal_condition`)
 """
 
 # import gym
@@ -16,6 +17,9 @@ from pyrobolearn.states import State
 from pyrobolearn.actions import Action
 from pyrobolearn.rewards import Reward
 
+from pyrobolearn.terminal_conditions import TerminalCondition
+from pyrobolearn.physics import PhysicsRandomizer
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
@@ -48,8 +52,8 @@ class Env(object):  # gym.Env):
 
     """
 
-    def __init__(self, world, states, rewards=None, terminal_condition=None, initial_state_distribution=None,
-                 extra_info=None):
+    def __init__(self, world, states, rewards=None, terminal_conditions=None, initial_state_distribution=None,
+                 physics_randomizer=None, extra_info=None):
         """
         Initialize the environment.
 
@@ -60,24 +64,27 @@ class Env(object):  # gym.Env):
             rewards (None, Reward): The rewards can be None when for instance we are in an imitation learning setting,
                                     instead of a reinforcement learning one. If None, only the state is returned by
                                     the environment.
-            terminal_condition (None, callable): A callable function or object that check if the policy has failed
+            terminal_conditions (None, callable): A callable function or object that check if the policy has failed
                                                  or succeeded the task.
             initial_state_distribution (None, callable): A callable function or object that is called at the beginning
                                                         when resetting the environment to generate the initial state
                                                         distribution.
+            physics_randomizer (None, PhysicsRandomizer, list of PhysicsRandomizer): physics randomizers. This will be
+                called each time you reset the environment.
             extra_info (None, callable): Extra info returned by the environment at each time step.
         """
         # Check and set parameters (see corresponding properties)
         self.world = world
         self.states = states
         self.rewards = rewards
-        self.terminal_condition = terminal_condition
+        self.terminal_conditions = terminal_conditions
+        self.physics_randomizers = physics_randomizer
         self.extra_info = extra_info if extra_info is not None else lambda: False
 
         self.rendering = False  # check with simulator
 
         # save the world state in memory
-        self.world.save()
+        self.initial_world_state = self.world.save()
 
     ##############
     # Properties #
@@ -129,18 +136,48 @@ class Env(object):  # gym.Env):
         self._rewards = rewards
 
     @property
-    def terminal_condition(self):
+    def terminal_conditions(self):
         """Return the terminal condition."""
-        return self._terminal_condition
+        return self._terminal_conditions
 
-    @terminal_condition.setter
-    def terminal_condition(self, condition):
+    @terminal_conditions.setter
+    def terminal_conditions(self, conditions):
         """Set the terminal condition."""
-        if condition is None:
-            condition = lambda: False
-        if not callable(condition):
-            raise TypeError("Expecting the terminal condition to be callable.")
-        self._terminal_condition = condition
+        if conditions is None:
+            conditions = [TerminalCondition()]
+        elif isinstance(conditions, TerminalCondition):
+            conditions = [conditions]
+        elif isinstance(conditions, (list, tuple)):
+            for idx, condition in enumerate(conditions):
+                if not callable(conditions):
+                    raise TypeError("Expecting the {} item in the given terminal conditions to be an instance of "
+                                    "`TerminalCondition`, instead got: {}".format(idx, type(condition)))
+        else:
+            raise TypeError("Expecting the terminal conditions to be an instance of `TerminalCondition`, or a list of "
+                            "`TerminalCondition`, but instead got: {}".format(type(conditions)))
+        self._terminal_conditions = conditions
+
+    @property
+    def physics_randomizers(self):
+        """Return the list of physics randomizers used each time we reset the environment."""
+        return self._physics_randomizers
+
+    @physics_randomizers.setter
+    def physics_randomizers(self, randomizers):
+        """Set the physics randomizers."""
+        if randomizers is None:
+            randomizers = []
+        elif isinstance(randomizers, PhysicsRandomizer):
+            randomizers = [randomizers]
+        elif isinstance(randomizers, (list, tuple)):
+            for randomizer in randomizers:
+                if not isinstance(randomizer, PhysicsRandomizer):
+                    raise TypeError("Expecting the randomizer to be an instance of `PhysicsRandomizer`, instead got "
+                                    "{}".format(randomizer))
+        else:
+            raise TypeError("Expecting the given randomizers to be None, a `PhysicsRandomizer`, or a list of them; "
+                            "instead got: {}".format(type(randomizers)))
+        self._physics_randomizers = randomizers
 
     ###########
     # Methods #
@@ -156,6 +193,10 @@ class Env(object):  # gym.Env):
         # reset world
         self.world.reset()
 
+        # randomize the environment
+        for randomizer in self.physics_randomizers:
+            randomizer.randomize()
+
         # reset states and return first states/observations
         return self.states.reset()
 
@@ -184,10 +225,13 @@ class Env(object):  # gym.Env):
         # apply each policy's action in the environment
         # for action in actions:
         #    action()
-        if actions is not None:
+        # TODO: calling the actions should be done inside the policy(ies), and not in the environments. The policy
+        #  decided when to execute an action. Think about when there are multiple policies, when using multiprocessing,
+        #  or when the environment runs in real-time.
+        if actions is not None and isinstance(actions, Action):
             actions()
 
-        # perform a step forward in the simulation
+        # perform a step forward in the simulation which computes all the dynamics
         self.world.step()
 
         # compute reward
@@ -196,7 +240,7 @@ class Env(object):  # gym.Env):
 
         # compute terminating condition
         # done = [reward.is_done() for reward in self.rewards]
-        done = self.terminal_condition()
+        done = any([condition() for condition in self.terminal_conditions])
 
         # get next state/obs for each policy
         # states = [state() for state in self.states]
@@ -214,6 +258,12 @@ class Env(object):  # gym.Env):
 
         # Bullet: do nothing
         # if isinstance(self.sim, Bullet): pass
+        # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 1)
+        pass
+
+    def hide(self):
+        # hide the GUI
+        # self.sim.configureDebugVisualizer(self.sim.COV_ENABLE_RENDERING, 0)
         pass
 
     def close(self):
@@ -226,7 +276,13 @@ class Env(object):  # gym.Env):
         Args:
             seed (int): seed for the random generator used in the simulator.
         """
-        self.sim.setSeed(seed)
+        # set the simulator seed
+        self.simulator.seed(seed)
+
+        # set the seed for the physics randomizer
+        for randomizer in self.physics_randomizers:
+            randomizer.seed(seed)
+
 
 
 class BasicEnv(Env):

pyrobolearn/envs/gym_wrapper.py

@@ -9,6 +9,7 @@ on the OpenAI gym library.
 import inspect
 import functools
 import numpy as np
+import torch
 import gym
 from gym import *
 import warnings
@@ -106,6 +107,11 @@ class GymEnvWrapper(gym.Env):
         """perform a step in the environment and set the data for the GymState and GymAction"""
         if isinstance(actions, Action):
             actions = actions.data[0]
+        elif isinstance(actions, torch.Tensor):
+            if actions.requires_grad:
+                actions = actions.detach().numpy()
+            else:
+                actions = actions.numpy()
         if isinstance(self.action_space, gym.spaces.Discrete) and isinstance(actions, np.ndarray):
             actions = actions[0]
         observations, reward, done, info = self.env.step(actions)

pyrobolearn/processors/__init__.py

@@ -1,6 +1,9 @@
 
 # import processors
-from processor import *
+from processor import Processor
+
+# import basic processors (center, normalize, standardize, etc.)
+from basic_processors import *
 
 # import linear processors
-from linear_processor import
+from linear_processor import LinearProcessor

pyrobolearn/processors/basic_processors.py

@@ -0,0 +1,277 @@
+#!/usr/bin/env python
+"""Provide basic processor classes that center, normalize, or standardize the given input.
+
+Processors are functions that are applied to the inputs (respectively outputs) of an approximator/learning model
+before (respectively after) being processed by it. Processors might have parameters but they do not have
+trainable/optimizable parameters; the parameters are fixed and given at the beginning.
+"""
+
+import torch
+
+from pyrobolearn.processors.processor import Processor, convert_numpy
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+# TODO: update the methods such that x can be a matrix of data points (2d tensor), and not only one data point
+#  (1d tensor)
+
+class ShiftProcessor(Processor):
+    r"""Shift Processor
+
+    Shift the data by the given amount; that is, it returned: :math:`\hat{x} = x + z` where :math:`z` is the
+    specified amount to shift the original input :math:`x`.
+    """
+
+    def __init__(self, z):
+        """
+        Initialize the Shift Processor.
+
+        Args:
+            z (int, float, np.array, torch.Tensor): amount to be shifted.
+        """
+        super(ShiftProcessor, self).__init__()
+        if isinstance(z, (int, float)):
+            z = [z]
+        self.z = torch.tensor(z, dtype=torch.float)
+
+    @convert_numpy
+    def compute(self, x):
+        return x - self.z
+
+
+class RunningCenterProcessor(Processor):
+    r"""Running Center Processor
+
+    Center the data by using the mean which is updated each time a new data point is given.
+    """
+
+    def __init__(self):
+        super(RunningCenterProcessor, self).__init__()
+        self.mean = torch.zeros(1)
+        self.N = 0
+
+    def reset(self):
+        self.mean = torch.zeros(1)
+        self.N = 0
+
+    @convert_numpy
+    def compute(self, x):
+        # update the mean
+        self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * x
+        self.N += 1
+
+        # center the data with new mean
+        return x - self.mean
+
+
+class StandardizerProcessor(Processor):
+    r"""Standardizer Processor
+
+    Processor that standardizes the given data; the returned data is centered around 0 with a standard deviation of 1.
+    That is, it returned :math:`\hat{x} = \frac{x - \mu}{\sigma}`, where :math:`\mu` is the mean, and :math:`\sigma`
+    is the standard deviation.
+    """
+
+    def __init__(self, mean=0., std_dev=1., epsilon=1.e-4):
+        """
+        Initialize the Standardizer Processor.
+
+        Args:
+            mean (int, float, np.array, torch.Tensor): mean
+            std_dev (int, float, np.array, torch.Tensor): standard deviation
+            epsilon (float): small number to be added to the denominator for stability in case the std dev = 0
+        """
+        super(StandardizerProcessor, self).__init__()
+        if isinstance(mean, (int, float)):
+            mean = [mean]
+        if isinstance(std_dev, (int, float)):
+            std_dev = [std_dev]
+        self.mean = torch.tensor(mean, dtype=torch.float)
+        self.std = torch.tensor(std_dev, dtype=torch.float)
+        self.eps = epsilon
+
+    @convert_numpy
+    def compute(self, x):
+        return (x - self.mean) / (self.std + self.eps)
+
+
+class RunningStandardizerProcessor(Processor):
+    r"""Running Standardizer Processor
+
+    Processor that standardizes the given data; the returned data is centered around 0 with a standard deviation of 1.
+    That is, it returned :math:`\hat{x} = \frac{x - \mu}{\sigma}`, where :math:`\mu` is the mean, and :math:`\sigma`
+    is the standard deviation. The mean and the standard deviation (or variance) are updated at each time a new data
+    point is given.
+    """
+
+    def __init__(self, epsilon=1.e-4):
+        """
+        Initialize the Running Standardizer Processor.
+
+        Args:
+            epsilon (float): small number to be added to the denominator for stability in case the std dev = 0
+        """
+        super(RunningStandardizerProcessor, self).__init__()
+        self.mean = torch.zeros(1)
+        self.var = torch.ones(1)
+        self.N = 0
+        self.eps = epsilon
+
+    def reset(self):
+        self.mean = torch.zeros(1)
+        self.var = torch.ones(1)
+        self.N = 0
+
+    @convert_numpy
+    def compute(self, x):
+        # update the mean
+        old_mean = torch.clone(self.mean)
+        self.mean = self.N / (self.N + 1.) * self.mean + 1. / (self.N + 1) * x
+
+        # update the var / stddev
+        self.var = self.N / (self.N + 1) * self.var + 1. / (self.N + 1) * (x - old_mean) * (x - self.mean)
+        std = torch.sqrt(self.var)
+
+        # update total number of data points
+        self.N += 1
+
+        # standardize the data
+        return (x - self.mean) / (std + self.eps)
+
+
+class NormalizerProcessor(Processor):
+    r"""Normalizer Processor
+
+    Processor that normalizes the given data; the returned data will be between 0 and 1.
+    That is, it returned :math:`\hat{x} = \frac{x - x_{min}}{x_{max} - x_{min}}`, where
+    :math:`x \in [x_{min}, x_{max}]`.
+    """
+
+    def __init__(self, xmin, xmax):
+        """
+        Initialize the Normalizer Processor.
+
+        Args:
+            xmin (int, float, np.array, torch.Tensor): minimum bound
+            xmax (int, float, np.array, torch.Tensor): maximum bound
+        """
+        super(NormalizerProcessor, self).__init__()
+        if isinstance(xmin, (int, float)):
+            xmin = [xmin]
+        if isinstance(xmax, (int, float)):
+            xmax = [xmax]
+        self.xmin = torch.tensor(xmin, dtype=torch.float)
+        self.xmax = torch.tensor(xmax, dtype=torch.float)
+        if torch.allclose(self.xmin, self.xmax):
+            raise ValueError("The given arguments 'xmin' and 'xmax' are the same.")
+
+    @convert_numpy
+    def compute(self, x):
+        return (x - self.xmin) / (self.xmax - self.xmin)
+
+
+class RunningNormalizerProcessor(Processor):
+    r"""Running Normalizer Processor
+
+    Processor that normalizes the given data; the returned data will be between 0 and 1.
+    That is, it returned :math:`\hat{x} = \frac{x - x_{min}}{x_{max} - x_{min}}`, where
+    :math:`x \in [x_{min}, x_{max}]`. The :math:`x_{min}` and `x_{max}` will be updated each time a new data point
+    is given.
+
+    Warnings: it will return zero at the beginning as x = x_min = x_max.
+    """
+
+    def __init__(self):
+        super(RunningNormalizerProcessor, self).__init__()
+        self.xmin = torch.zeros(1)
+        self.xmax = torch.ones(1)
+
+    def reset(self):
+        self.xmin = torch.zeros(1)
+        self.xmax = torch.ones(1)
+
+    @convert_numpy
+    def compute(self, x):
+        # compute new xmin and xmax given new data point
+        self.xmin = torch.min(x, self.xmin)
+        self.xmax = torch.max(x, self.xmax)
+
+        # if xmax and xmin are not different, make the denominator to be 1
+        idx = (self.xmax == self.xmin)
+        self.xmax[idx] = self.xmin[idx] + 1.
+
+        # normalize
+        return (x - self.xmin) / (self.xmax - self.xmin)
+
+
+class ClipProcessor(Processor):
+    r"""Clip Processor
+
+    Processor that clips the given data; the returned data will be between [low, high], where `low` and `high` are
+    respectively the specified lower and higher bound.
+    """
+
+    def __init__(self, low=-10., high=10.):
+        """
+        Initialize the Clip processor.
+
+        Args:
+            low (int, float, np.array, torch.Tensor): lower bound
+            high (int, float, np.array, torch.Tensor): higher bound
+        """
+        super(ClipProcessor, self).__init__()
+        if isinstance(low, (int, float)):
+            low = [low]
+        if isinstance(high, (int, float)):
+            high = [high]
+        self.low = torch.tensor(low, dtype=torch.float)
+        self.high = torch.tensor(high, dtype=torch.float)
+
+    @convert_numpy
+    def compute(self, x):
+        return torch.min(torch.max(x, self.low), self.high)
+
+
+class ScaleProcessor(Processor):
+    r"""Scale processor
+
+    Processor that scales the input x which is between [x1, x2] to the output y which is between [y1, y2].
+    This is for instance useful after a tanh layer in a neural network which outputs a value between -1 and 1, and
+    that value has to be rescaled to a bigger (absolute) value.
+    """
+
+    def __init__(self, x1, x2, y1, y2):
+        """
+        Initialize the scale processor.
+
+        Args:
+            x1 (int, float, np.array, torch.Tensor): lower bound of input
+            x2 (int, float, np.array, torch.Tensor): upper bound of input
+            y1 (int, float, np.array, torch.Tensor): lower bound of output
+            y2 (int, float, np.array, torch.Tensor): upper bound of output
+        """
+
+        super(ScaleProcessor, self).__init__()
+
+        def convert(x):
+            if isinstance(x, (int, float)):
+                return [x]
+            return x
+
+        self.x1 = torch.tensor(convert(x1), dtype=torch.float)
+        self.x2 = torch.tensor(convert(x2), dtype=torch.float)
+        self.y1 = torch.tensor(convert(y1), dtype=torch.float)
+        self.y2 = torch.tensor(convert(y2), dtype=torch.float)
+        self.ratio = (self.y2 - self.y1) / (self.x2 - self.x1)
+
+    @convert_numpy
+    def compute(self, x):
+        return self.y1 + (x - self.x1) * self.ratio

pyrobolearn/processors/linear_processor.py

@@ -2,10 +2,18 @@
 """Define the Linear Processor class.
 """
 
-import numpy as np
 import torch
 
-from processor import Processor
+from pyrobolearn.processors.processor import Processor, convert_numpy
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
 
 
 class LinearProcessor(Processor):
@@ -17,12 +25,12 @@ class LinearProcessor(Processor):
 
     def __init__(self, a, b):
         super(LinearProcessor, self).__init__()
-        self.a = torch.Tensor(a)
-        self.b = torch.Tensor(b)
+        self.a = torch.tensor(a, dtype=torch.float)
+        self.b = torch.tensor(b, dtype=torch.float)
 
+    def reset(self):
+        pass
+
+    @convert_numpy
     def compute(self, x):
-        if isinstance(x, np.ndarray):
-            x = torch.from_numpy(x).float()
-            y = self.a * x + self.b
-            return y.numpy()
         return self.a * x + self.b

pyrobolearn/processors/processor.py

@@ -1,14 +1,51 @@
 #!/usr/bin/env python
 """Define the Processor class.
 
-Processors are rules that are applied to the inputs and outputs of a learning model before being processed by the
-model or after. Processors might have parameters but they do not have trainable/optimizable parameters; the parameters
-are fixed and given at the beginning.
+Processors are functions that are applied to the inputs (respectively outputs) of an approximator/learning model
+before (respectively after) being processed by it. Processors might have parameters but they do not have
+trainable/optimizable parameters; the parameters are fixed and given at the beginning.
 """
 
 import numpy as np
 import torch
 
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+# define decorator that converts the given numpy array to a torch tensor and return it back to a numpy array if
+# specified
+def convert_numpy(f):
+    def wrapper(self, x, to_numpy=False):
+        """Process the given argument.
+
+        Args:
+            x (np.array, torch.Tensor): input data.
+            to_numpy (bool): If True, it will convert the processed data into a numpy array.
+        """
+
+        # convert to torch Tensor if numpy array
+        if isinstance(x, np.ndarray):
+            x = torch.from_numpy(x).float()
+
+        # call inner function on the given argument
+        x = f(self, x)
+
+        # reconvert to numpy array if specified, and return it
+        if to_numpy:
+            return x.numpy()
+
+        # return torch Tensor
+        return x
+
+    return wrapper
+
 
 class Processor(object):
     r"""Processor
@@ -21,70 +58,12 @@ class Processor(object):
     def __init__(self):
         pass
 
+    def reset(self):
+        pass
+
+    @convert_numpy
     def compute(self, x):
         pass
 
-    def __call__(self, x):
-        return self.compute(x)
-
-
-class CenterProcessor(Processor):
-    r"""Center Processor
-
-    Center the data by the given mean; that is, it returned: :math:`\hat{x} = x - \mu` where :math:`\mu` is the mean.
-    """
-
-    def __init__(self, mean):
-        super(CenterProcessor, self).__init__()
-        self.mean = torch.Tensor(mean)
-
-    def compute(self, x):
-        if isinstance(x, np.ndarray):
-            x = torch.from_numpy(x).float()
-            x -= self.mean
-            return x.numpy()
-        return x - self.mean
-
-
-class StandardizerProcessor(Processor):
-    r"""Standardizer Processor
-
-    Processor that standardize the given data; the returned data is centered around 0 with a standard deviation of 1.
-    That is, it returned :math:`\hat{x} = \frac{x - \mu}{\sigma}`, where :math:`\mu` is the mean, and :math:`\sigma`
-    is the standard deviation.
-    """
-
-    def __init__(self, mean, std):
-        super(StandardizerProcessor, self).__init__()
-        self.mean = torch.Tensor(mean)
-        self.std = torch.Tensor(std)
-
-    def compute(self, x):
-        if isinstance(x, np.ndarray):
-            x = torch.from_numpy(x).float()
-            x = (x - self.mean) / (self.std + 1.e-13)
-            return x.numpy()
-        return (x - self.mean) / (self.std + 1.e-13)
-
-
-class NormalizerProcessor(Processor):
-    r"""Normalizer Processor
-
-    Processor that normalize the given data; the returned data will be between 0 and 1.
-    That is, it returned :math:`\hat{x} = \frac{x - x_{min}}{x_{max} - x_{min}}`, where
-    :math:`x \in [x_{min}, x_{max}]`.
-    """
-
-    def __init__(self, xmin, xmax):
-        super(NormalizerProcessor, self).__init__()
-        self.xmin = torch.Tensor(xmin)
-        self.xmax = torch.Tensor(xmax)
-        if torch.allclose(self.xmin, self.xmax):
-            raise ValueError("The given arguments 'xmin' and 'xmax' are the same.")
-
-    def compute(self, x):
-        if isinstance(x, np.ndarray):
-            x = torch.from_numpy(x).float()
-            x = (x - self.xmin) / (self.xmax - self.xmin)
-            return x.numpy()
-        return (x - self.xmin) / (self.xmax - self.xmin)
+    def __call__(self, x, to_numpy=False):
+        return self.compute(x, to_numpy=to_numpy)

pyrobolearn/terminal_conditions/README.md

@@ -0,0 +1,12 @@
+## Terminal conditions
+
+This folder contains terminal conditions that are used to end an environment. This for instance can be used to notify that the task was successfully carried out or it resulted in a failure.
+
+More concretely, assume you have a locomotion task where a robot is supposed to move from a point A to a point B. Two terminal conditions can be defined in this case, one which is triggered when the robot has fallen (failure), and one where the robot arrived at point B (success).
+
+With respect to `gym.envs`: currently, gym environments only return a boolean to notify if the environment is over or not, which is quite restrictive. Did the agent succeeded to perform the task or did it failed? What particular condition causes the environment to end? Also, some environments share the same terminal conditions but they are copied-pasted in the code under the `step` method, resulting in code duplication. Instead, as we did for the `world`, `rewards`, `states`, and other modules, we define the terminal conditions outside the environment, and give them as arguments to the environment constructor. We thus favor [composition over inheritance](https://en.wikipedia.org/wiki/Composition_over_inheritance) resulting in better reusability and flexibility.
+
+## What to look/check next?
+
+Check the `envs` folder.
+

pyrobolearn/terminal_conditions/__init__.py

@@ -0,0 +1,3 @@
+
+# import terminal conditions
+from .terminal_condition import *

pyrobolearn/terminal_conditions/terminal_condition.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-"""Define some common terminating condition for the environment.
+"""Define some common terminal conditions for the environment.
 """
 
 import numpy as np
@@ -7,6 +7,9 @@ import numpy as np
 from pyrobolearn.robots import Robot
 from pyrobolearn.states import LinkState
 
+from pyrobolearn.utils.orientation import *
+
+
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
@@ -17,10 +20,10 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class TerminatingCondition(object):
-    r"""Terminating Condition
+class TerminalCondition(object):
+    r"""Terminal Condition
 
-    This class provides the basic layout for the `TerminatingCondition` class and its child classes.
+    This class provides the basic layout for the `TerminalCondition` class and its child classes.
     This one can be used to check when an environment has fulfilled certain conditions and can be terminated.
 
     This class can be further subdivided into two categories: failed and succeeded conditions.
@@ -49,24 +52,24 @@ class TerminatingCondition(object):
     __nonzero__ = __bool__
 
 
-class FailedCondition(TerminatingCondition):
-    r"""Failed Terminating Condition
+class FailedCondition(TerminalCondition):
+    r"""Failed Terminal Condition
 
     This determines when a policy or multiple ones have failed to perform a certain task.
     """
     pass
 
 
-class SucceededCondition(TerminatingCondition):
-    r"""Succeeded Terminating Condition
+class SucceededCondition(TerminalCondition):
+    r"""Succeeded Terminal Condition
 
     This determines when a policy or multiple ones have succeeded to perform a certain task.
     """
     pass
 
 
-class GymTerminatingCondition(TerminatingCondition):
-    r"""OpenAI Gym Terminating Condition
+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.
@@ -85,15 +88,46 @@ class HasFallen(FailedCondition):
     Check if the given robot has fallen, by checking if its base is below a certain threshold.
     """
 
-    def __init__(self, robot, threshold=None):
+    def __init__(self, robot, height_threshold=None, angle_threshold=np.pi/6):
+        """
+        Check if the robot has fallen.
+
+        Args:
+            robot (Robot): robot instance.
+            height_threshold (float, None): height threshold. If the base height of the robot is below this threshold,
+                it will be considered that the robot has fallen. If None, it will use the original robot base height
+                and divided by 3 as the threshold.
+            angle_threshold (float): angle threshold in radians. If the angle between the initial robot base up
+                vector, and the current base up vector is bigger than the threshold, it will be considered that the
+                robot has fallen. Normally, the initial robot base up vector points upward. By default, it is 30
+                degrees (=pi/6 rad).
+        """
         self.robot = robot
-        self.threshold = threshold if threshold is not None else robot.height/4.
+        self.height_threshold = height_threshold if height_threshold is not None else robot.base_height/3.
+        self.angle_threshold = angle_threshold
+
+    def _compute_angle(self):
+        """Compute angle between the initial base up vector and current base up vector."""
+        up_vector = get_matrix_from_quaternion(self.robot.get_base_orientation(False))[:, 2]
+        angle = np.arccos(np.dot(self.robot.base_up_vector, up_vector))
+        return angle
+
+    def _compute_height(self):
+        """Compute the current height."""
+        return self.robot.get_base_position()[2]
 
     def check(self):
-        return self.robot.getBasePosition()[2] < self.threshold
+        height = self._compute_height()
+        height_condition = height < self.height_threshold
+        angle = self._compute_angle()
+        angle_condition = angle > self.angle_threshold
+        return height_condition or angle_condition
 
     def __repr__(self):
-        return self.__class__.__name__ + '(threshold=' + str(self.threshold) + ')'
+        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)
+        return description
 
 
 class HasReached(SucceededCondition):