update envs, rewards, and states

pyrobolearn/envs/env.py

@@ -12,7 +12,7 @@ Dependencies:
 
 import copy
 import pickle
-# import gym
+import gym
 
 from pyrobolearn.worlds import World, BasicWorld
 from pyrobolearn.states import State
@@ -34,7 +34,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
+class Env(gym.Env):  # TODO: make it inheriting the gym.Env
     r"""Environment class.
 
     This class defines the environment as it described in a reinforcement learning setting [1]. That is, given an
@@ -88,6 +88,10 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         self.extra_info = extra_info if extra_info is not None else lambda: False
         self.actions = actions
 
+        # state dictionary which contains at least {'policy': State, 'value': State}
+        # if not specified, it will be the same state for the policy and value function approximator
+        self._state_dict = None
+
         # check if we are rendering with the simulator
         self.is_rendering = self.simulator.is_rendering()
         self.rendering_mode = 'human'
@@ -143,6 +147,44 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         """Return the first (combined) state."""
         return self._states[0]
 
+    @property
+    def state_dict(self):
+        """Return the state dictionary which contains at least the 'policy' and 'value' keys."""
+        if self._state_dict is not None:
+            return self._state_dict
+        states = self.states
+        if len(states) == 1:
+            states = states[0]
+        return {'policy': states, 'value': states}
+
+    @state_dict.setter
+    def state_dict(self, state_dict):
+        """Set the state dictionary which should contains at least the 'policy' and 'value' keys."""
+        if state_dict is not None:
+            if not isinstance(state_dict, dict):
+                raise TypeError("Expecting the given 'state_dict' to be a dictionary, but got instead: "
+                                "{}".format(type(state_dict)))
+            for key, value in state_dict.items():
+                if isinstance(value, (list, tuple)):
+                    for v in value:
+                        if not isinstance(v, State):
+                            raise TypeError("Expecting the values in the given 'state_dict' to be an instance of "
+                                            "`State`, or a list/tuple of them, but got instead: {}".format(type(v)))
+                if not isinstance(value, State):
+                    raise TypeError("Expecting the value in the given 'state_dict' to be an instance of `State`, or "
+                                    "a list/tuple of them, but got instead: {}".format(type(value)))
+        self._state_dict = state_dict
+
+    @property
+    def state_spaces(self):
+        """Return the state space for each state."""
+        return [state.merged_space for state in self.states]
+
+    @property
+    def state_space(self):
+        """Return the state space of the first (combined) state."""
+        return self.states[0].merged_space
+
     @property
     def actions(self):
         """Return the actions."""
@@ -172,6 +214,20 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
             return None
         return self.actions[0]
 
+    @property
+    def action_spaces(self):
+        """Return the action space for each action."""
+        if self.actions is None:
+            return None
+        return [action.merged_space for action in self.actions]
+
+    @property
+    def action_space(self):
+        """Return the action space of the first (combined) action."""
+        if self.actions is None:
+            return None
+        return self.actions[0].merged_space
+
     @property
     def rewards(self):
         """Return the rewards."""
@@ -442,6 +498,40 @@ class BasicEnv(Env):
                                        physics_randomizers, extra_info, actions)
 
 
+class GymEnv(gym.Env):
+    r"""Gym Environment.
+
+    This is a thin wrapper around a PRL environment to a Gym environment. Notably, we make sure that the action is
+    defined in the environment, as in PRL the actions don't have to be specified.
+
+    Few notes with respect to PRL:
+    - in PRL Env, you don't have to provide the action space nor the action. The reason is that it is the policy that
+      should be aware of the action space.
+    - in PRL Env, the returned state data can be a list of state data if the states have different dimensions.
+    """
+
+    def __init__(self, prl_env):
+        """
+        Initialize the Gym PRL Environment.
+
+        Args:
+            prl_env (Env): pyrobolearn (PRL) environment.
+        """
+        # check environment
+        if not isinstance(prl_env, Env):
+            raise TypeError("Expecting the given 'prl_env' to be an instance of `Env`, instead got: "
+                            "{}".format(type(prl_env)))
+        self.env = prl_env
+
+        # check that the environment has actions
+        if self.env.actions is None:
+            raise RuntimeError("Expecting the environment to have actions")
+
+    def __getattr__(self, item):
+        """The Gym Env have the same methods and attributes as the PRL Env."""
+        return getattr(self.env, item)
+
+
 # Tests
 if __name__ == '__main__':
     from pyrobolearn.simulators import BulletSim

pyrobolearn/envs/gym_wrapper.py

@@ -12,7 +12,6 @@ import numpy as np
 import torch
 import gym
 # import baselines
-from gym import *
 import warnings
 warnings.simplefilter("ignore")
 

pyrobolearn/envs/locomotion/locomotion.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+"""Provide the abstract locomotion environment from which all the other locomotion environments inherit from.
+"""
+
+from pyrobolearn.envs.env import Env
+
+
+__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 LocomotionEnv(Env):
+    r"""Locomotion Environment (abstract)
+
+    This is the abstract locomotion environment from which all locomotion environments inherit from.
+    """
+
+    def __init__(self, world, states, rewards=None, terminal_conditions=None, initial_state_generators=None,
+                 physics_randomizers=None, extra_info=None, actions=None):
+        """
+        Initialize the locomotion environment.
+
+        Args:
+            world (World): world of the environment. The world contains all the objects (including robots), and has
+                access to the simulator.
+            states ((list of) State): states that are returned by the environment at each time step.
+            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_conditions (None, callable, TerminalCondition, list of TerminalCondition): A callable function or
+                object that check if the policy has failed or succeeded the task.
+            initial_state_generators (None, StateGenerator, list of StateGenerator): state generators which are used
+                when resetting the environment to generate the initial states.
+            physics_randomizers (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.
+            actions ((list of) Action): actions that are given to the environment. Note that this is not used here in
+                the current environment as it should be the policy that performs the action. This is useful when
+                creating policies after the environment (that is, the policy can uses the environment's states and
+                actions).
+        """
+        super(LocomotionEnv, self).__init__(world=world, states=states, rewards=rewards,
+                                            terminal_conditions=terminal_conditions,
+                                            initial_state_generators=initial_state_generators,
+                                            physics_randomizers=physics_randomizers, extra_info=extra_info,
+                                            actions=actions)

pyrobolearn/envs/locomotion/quadruped.py

@@ -0,0 +1,163 @@
+#!/usr/bin/env python
+"""Provide the locomotion with quadruped environment.
+
+This is based on [1] and [2] but generalized to other quadruped platforms.
+
+References:
+    - [1] PyBullet:
+      https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
+    - [2] RaisimGym: https://github.com/leggedrobotics/raisimGym/blob/master/raisim_gym/env/env/ANYmal/Environment.hpp
+"""
+
+import pyrobolearn as prl
+
+from pyrobolearn.envs.locomotion.locomotion import LocomotionEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Erwin Coumans (Pybullet)", "Jemin Hwangbo et al. (RaisimGym)", "Brian Delhaisse (PRL)"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class LocomotionQuadrupedEnv1(LocomotionEnv):
+    r"""Locomotion Quadruped Environment
+
+    This is based on the locomotion environment provided for the minitaur robot in PyBullet [1] but generalized to
+    other quadruped robotic platforms.
+
+    Here are the various environment features:
+
+    - world: basic world with gravity enabled, a basic floor and the quadruped robot.
+    - state:
+    - action:
+    - reward:
+    - initial state generator:
+    - physics randomizer:
+    - terminal condition:
+
+    References:
+        - [1] PyBullet:
+          https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/bullet/minitaur_gym_env.py
+    """
+
+    def __init__(self, simulator=None, robot='minitaur'):
+        """
+        Initialize the locomotion with quadruped environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance.
+            robot (str): robot name.
+        """
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+        robot = world.load_robot(robot)
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(LocomotionQuadrupedEnv1, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                      terminal_conditions=terminal_condition,
+                                                      initial_state_generators=initial_state_generator)
+
+
+class LocomotionQuadrupedEnv2(LocomotionEnv):
+    r"""Locomotion Quadruped Environment
+
+    This is based on the locomotion environment provided in `raisimGym` for the ANYmal robot in [1]. The Python version
+    can be found in `raisimpy` in [2].
+
+    Here are the various environment features:
+
+    - simulator: Raisim
+    - world: basic world with gravity enabled, a basic floor and the quadruped robot.
+    - state:
+        - height (1D)
+        - world frame z-axis expressed in the body frame (3D)
+        - joint angle positions (ND)
+        - joint velocities (ND)
+        - body linear velocities (3D)
+        - body angular velocities (3D)
+    - action: PD joint position targets
+    - reward: 0.3 * v_x - 2e-5 * ||\tau||^2
+        - if terminal, -10 is added to the reward.
+    - initial state generator: fixed state generator for joint positions such that they are set to the home position.
+    - terminal condition: if there is contact with a link that is not the foot.
+
+    References:
+        - [1] RaisimGym:
+          https://github.com/leggedrobotics/raisimGym/blob/master/raisim_gym/env/env/ANYmal/Environment.hpp
+        - [2] Raisimpy: https://github.com/robotlearn/raisimpy/blob/master/examples/raisimpy_gym/envs/anymal/env.py
+    """
+
+    def __init__(self, simulator=None, robot='anymal'):
+        """
+        Initialize the locomotion quadruped environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+        """
+        # check simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet()
+        elif not isinstance(simulator, prl.simulators.Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(LocomotionQuadrupedEnv2, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                      terminal_conditions=terminal_condition,
+                                                      initial_state_generators=initial_state_generator)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = LocomotionQuadrupedEnv1(sim)
+
+    # run simulation
+    for _ in count():
+        env.step(sleep_dt=1./240)

pyrobolearn/envs/locomotion/robust_quadruped.py

@@ -0,0 +1,581 @@
+#!/usr/bin/env python
+"""Provide the locomotion with quadruped environment.
+
+This is based on [1,2] but generalized to other quadruped platforms.
+
+References:
+    - [1] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+    - [2] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+"""
+
+import pyrobolearn as prl
+
+from pyrobolearn.envs.locomotion.locomotion import LocomotionEnv
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Hwangbo et al.", "Lee et al.", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class SelfRightingEnv(LocomotionEnv):
+    r"""Self-righting locomotion environment
+
+    This is based on the locomotion environment provided in [1] with the ANYmal robotic platform. As described in [1],
+    "the goal is to regain upright base pose from an arbitrary configuration and re-position joints to the sitting
+    configuration such that the robot has all feet on the ground for a safe stand-up maneuver".
+
+    - simulator: Raisim
+    - world: basic world with gravity enabled, a basic floor and the quadruped robot.
+    - state:
+        - gravity unit vector (:math:`e_g`) expressed in the base frame (3)
+        - base angular velocity in body frame (3)
+        - joint position and velocity states (2N)
+        - history of joint position error and velocity: current joint state at t (position error + velocity) and two
+          past states corresponding to t-0.01s and t-0.02s (6N)
+        - previous joint position targets a_{t-1} (N)
+        - additive noise for observation
+            - up to 0.25 rad/s to the angular velocity
+            - up to 0.5 rad/s to the joint velocities
+            - up to 0.05 rad to the joint positions
+    - action: PD joint position targets :math:`q_d = 0.5 o_t + q_t` where :math:`o_t` is the output of the policy and
+      :math:`q_t` are the current joint positions.
+    - cost: :math:`0.0005 c_{\tau} + 0.2 c_{jslim} + 0.0025 c_{ad} + 6c_o + 6c_{jp} + 6c_{bi} + 6c_{bs} + 6c_{c,in}`,
+      where:
+        - torque: :math:`c_{\tau} = || \tau ||^2` where :math:`\tau` are the joint torques.
+        - joint speed limit: :math:`c_{jslim} = \sum_{i}^{N} \max(\dot{q}_{i,lim} - |q_i|, 0)^2` where :math:`N` is
+          the number of actuated joints, :math:`q_i` is the position of the i-th joint, and :math:`\dot{q}_{i,lim}` is
+          the maximum speed of the i-th joint.
+        - action difference: :math:`c_{ad} = || a_t - a_{t-1} ||^2` where :math:`a_t` is the action vector.
+        - orientation cost: :math:`c_o = || [0,0,-1]^\top - e_g ||` where :math:`e_g` is the unit gravity vector
+          expressed in the base frame.
+        - joint position: :math:`c_{jp} = \sum_{i}^N K(d(q_i, \hat{q}_i), 2.0)` where :math:`\hat{q}_i` is the desired
+          target joint position which correspond in this case to the crouching pose, :math:`d(\cdot, \cdot)` is the
+          minimum angle difference which maps to :math:`[0,\pi]`, and
+          :math:`K(e, \alpha) = \frac{-1}{e^{\alpha e} + 2 + e^{\alpha e}}` is a kernel function that maps
+          :math:`\mathcal{R}` to :math:`[-0.25, 0[`.
+        - body impulse: :math:`c_{bi} = \sum_{n \in I_c \backslash I_{c,f}} || i_{c,n} || / (|I_c| - |I_{c,f}|)` where
+          :math:`I_c` is the index set of the contact points, :math:`I_{c,f}` is the index set of the foot contact
+          points, :math:`i_{c,n}` is the impulse of the `n`th contact.
+        - body slippage: :math:`c_{bs} = \sum_{n \in I_c} ||v_{c,n}||^2 / |I_c|` where :math:`v_{c,n}` is the velocity
+          of the contact point.
+        - self collision: :math:`c_{c,in} = |I_{c,in}|` where :math:`I_{c,in}` is the index set of the self-collision
+          points.
+    - initial state generator: drop the quadruped from 0.5m about the ground with random joint positions
+    - physics randomizer:
+        - link masses perturbed up to 10% of the original value
+        - the CoM of the base is randomly translated up to 3cm in x,y,z directions
+        - the collision geometry of the robot is approximated using collision primitives (box, cylinder, sphere) with
+          randomized shapes and positions.
+        - the coefficient of friction is sampled from :math:`U([0.8, 2.0])`.
+    - terminal condition:
+        - time limit of 6sec
+
+
+    Here are more information about the policy, value function, and algorithm used (with exploration strategy) in the
+    paper [1]:
+
+    - policy network: input, 128 (tanh) units, 128 (tanh) units, N output units
+    - value network: input, 128 (tanh) units, 128 (tanh) units, 1 output unit
+    - exploration in the continuous action space.
+    - RL algorithm: TRPO (but also tested PPO)
+        - KL divergence threshold (delta) = 0.01
+        - GAE: discount factor (gamma) = 0.993, lambda = 0.99
+        - for value function: Adam optimizer with learning rate = 0.001
+    - curriculum learning: constraining cost terms (power, torque, joint speed, action difference and orientation
+      costs) are scaled to 10% of the final value at the first iteration and are scaled up as the training proceeds.
+
+    Note that the authors report that they could train the behavior policy in ~5hours on a single desktop
+    machine (32 GB memory, Intel i7-8700K and Geforce GTX 1070) with a fully C++ code.
+
+
+    References:
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+    """
+
+    def __init__(self, simulator=None, robot='anymal'):
+        """
+        Initialize the self-righting environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+        """
+        # check simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet()
+        elif not isinstance(simulator, prl.simulators.Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+
+        # check if the robot has the crouching pose as joint configuration.
+        if not self.robot.has_joint_configuration('crouching'):
+            raise TypeError("Expecting the robot to have the 'crouching' joint configuration predefined.")
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None  # prl.terminal_conditions.TimeLimitCondition(time=6)
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(SelfRightingEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                              terminal_conditions=terminal_condition,
+                                              initial_state_generators=initial_state_generator)
+
+
+class StandingUpEnv(LocomotionEnv):
+    r"""Standing-up locomotion environment
+
+    This is based on the locomotion environment provided in [1] with the ANYmal robotic platform. As described in [1],
+    the goal is to stand-up from an up-right position such that the robot is ready for the next phase (i.e. locomotion).
+
+    - simulator: Raisim
+    - world: basic world with gravity enabled, a basic floor and the quadruped robot.
+    - state:
+        - gravity unit vector (:math:`e_g`) expressed in the base frame (3)
+        - base angular velocity in body frame (3)
+        - base linear velocity in body frame (3)
+        - joint position and velocity states (2N)
+        - history of joint position error and velocity: current joint state at t (position error + velocity) and two
+          past states corresponding to t-0.01s and t-0.02s (6N)
+        - previous joint position targets a_{t-1} (N)
+        - additive noise for observation
+            - up to 0.2 m/s to the linear velocity
+            - up to 0.25 rad/s to the angular velocity
+            - up to 0.5 rad/s to the joint velocities
+            - up to 0.05 rad to the joint positions
+    - action: PD joint position targets :math:`q_d = 0.5 o_t + q_t` where :math:`o_t` is the output of the policy and
+      :math:`q_t` are the current joint positions.
+    - cost: :math:`0.0001 c_{\tau} + 0.6 c_{jslim} + 0.001 c_{ad} + 2.5 c_o + 5 c_h + 3 c_{jp}`, where:
+        - torque: :math:`c_{\tau} = || \tau ||^2` where :math:`\tau` are the joint torques.
+        - joint speed limit: :math:`c_{jslim} = \sum_{i}^{N} \max(\dot{q}_{i,lim} - |q_i|, 0)^2` where :math:`N` is
+          the number of actuated joints, :math:`q_i` is the position of the i-th joint, and :math:`\dot{q}_{i,lim}` is
+          the maximum speed of the i-th joint.
+        - action difference: :math:`c_{ad} = || a_t - a_{t-1} ||^2` where :math:`a_t` is the action vector.
+        - orientation cost: :math:`c_o = || [0,0,-1]^\top - e_g ||` where :math:`e_g` is the unit gravity vector
+          expressed in the base frame.
+        - height: :math:`c_h = 1.0` if base height < threshold, otherwise 0.
+        - joint position: :math:`c_{jp} = \sum_{i}^N K(d(q_i, \hat{q}_i), 2.0)` where :math:`\hat{q}_i` is the desired
+          target joint position which correspond in this case to the crouching pose, :math:`d(\cdot, \cdot)` is the
+          minimum angle difference which maps to :math:`[0,\pi]`, and
+          :math:`K(e, \alpha) = \frac{-1}{e^{\alpha e} + 2 + e^{\alpha e}}` is a kernel function that maps
+          :math:`\mathcal{R}` to :math:`[-0.25, 0[`.
+    - initial state generator: drop the quadruped from 0.5m about the ground with near-upright pose.
+    - physics randomizer:
+        - link masses perturbed up to 10% of the original value
+        - the CoM of the base is randomly translated up to 3cm in x,y,z directions
+        - the collision geometry of the robot is approximated using collision primitives (box, cylinder, sphere) with
+          randomized shapes and positions.
+        - the coefficient of friction is sampled from :math:`U([0.8, 2.0])`.
+    - terminal condition:
+        - time limit of 6sec
+
+
+    Here are more information about the policy, value function, and algorithm used (with exploration strategy) in the
+    paper [1]:
+
+    - policy network: input, 128 (tanh) units, 128 (tanh) units, N output units
+    - value network: input, 128 (tanh) units, 128 (tanh) units, 1 output unit
+    - exploration in the continuous action space.
+    - RL algorithm: TRPO (but also tested PPO)
+        - KL divergence threshold (delta) = 0.01
+        - GAE: discount factor (gamma) = 0.993, lambda = 0.99
+        - for value function: Adam optimizer with learning rate = 0.001
+    - curriculum learning: constraining cost terms (power, torque, joint speed, action difference and orientation
+      costs) are scaled to 10% of the final value at the first iteration and are scaled up as the training proceeds.
+
+    Note that the authors report that they could train the behavior policy in ~5hours on a single desktop
+    machine (32 GB memory, Intel i7-8700K and Geforce GTX 1070) with a fully C++ code.
+
+
+    References:
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+    """
+
+    def __init__(self, simulator=None, robot='anymal'):
+        """
+        Initialize the standing-up environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+        """
+        # check simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet()
+        elif not isinstance(simulator, prl.simulators.Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+
+        # check if the robot has the crouching pose as joint configuration.
+        if not self.robot.has_joint_configuration('standing'):
+            raise TypeError("Expecting the robot to have the 'standing' joint configuration predefined.")
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None  # prl.terminal_conditions.TimeLimitCondition(time=6)
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(StandingUpEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                            terminal_conditions=terminal_condition,
+                                            initial_state_generators=initial_state_generator)
+
+
+class CommandedLocomotionEnv(LocomotionEnv):
+    r"""Locomotion quadruped environment
+
+    This is based on the locomotion environment provided in [1] with the ANYmal robotic platform. As described in [1],
+    "the goal is for the robot to follow a given velocity command composed of desired forward velocity, lateral
+    velocity, and yaw rate".
+
+    - simulator: Raisim
+    - world: basic world with gravity enabled, a basic floor and the quadruped robot.
+    - state:
+        - desired velocity commands (forward velocity, lateral velocity, yaw rate) (3)
+        - estimated base height (h_e) (1)
+        - gravity unit vector (:math:`e_g`) expressed in the base frame (3)
+        - base angular velocity in body frame (3)
+        - base linear velocity in body frame (3)
+        - joint position and velocity states (2N)
+        - history of joint position error and velocity: current joint state at t (position error + velocity) and two
+          past states corresponding to t-0.01s and t-0.02s (6N)
+        - previous joint position targets a_{t-1} (N)
+        - additive noise for observation
+            - up to 0.2 m/s to the linear velocity
+            - up to 0.25 rad/s to the angular velocity
+            - up to 0.5 rad/s to the joint velocities
+            - up to 0.05 rad to the joint positions
+    - action: PD joint position targets :math:`q_d = 0.5 o_t + q_n` where :math:`o_t` is the output of the policy and
+      :math:`q_n` is the standing joint configuration.
+    - cost: :math:`0.0005 c_{\tau} + 0.03 c_{jslim} + 0.5c_{ad} + 0.4c_o + 6c_\omega + 10 c_v + 0.1 c_{fc} + 2 c_{fs}`,
+      where:
+        - torque: :math:`c_{\tau} = || \tau ||^2` where :math:`\tau` are the joint torques.
+        - joint speed limit: :math:`c_{jslim} = \sum_{i}^{N} \max(\dot{q}_{i,lim} - |q_i|, 0)^2` where :math:`N` is
+          the number of actuated joints, :math:`q_i` is the position of the i-th joint, and :math:`\dot{q}_{i,lim}` is
+          the maximum speed of the i-th joint.
+        - action difference: :math:`c_{ad} = || a_t - a_{t-1} ||^2` where :math:`a_t` is the action vector.
+        - orientation cost: :math:`c_o = || [0,0,-1]^\top - e_g ||` where :math:`e_g` is the unit gravity vector
+          expressed in the base frame.
+        - angular velocity: :math:`c_\omega = K(|\omega^B_B - \hat{\omega}^B_B|, 1.0)`, where :math:`\omega^B_B` is the
+          angular velocity of the base expressed in the body frame, :math:`\hat{\omega}` is the desired angular
+          velocity, and :math:`K(e, \alpha) = \frac{-1}{e^{\alpha e} + 2 + e^{\alpha e}}` is a kernel function that
+          maps :math:`\mathcal{R}` to :math:`[-0.25, 0[`.
+        - linear velocity: :math:`c_v = K(|v^B_B - \hat{v}^B_B|, 4.0)`, where :math:`v^B_B` is the linear velocity of
+          the base expressed in the body frame and math:`\hat{v}` is the desired linear velocity.
+        - foot clearance: :math:`c_{fc} = \sum (h_{f,i} - 0.07)^2 ||v_{f,i}||,  \forall i s.t. g_i > 0, i \in I_{c,f}`,
+          where :math:`h_{f,i}` is the ze position of the `i`th foot, :math:`v_{f,i}` is the velocity of the `i`th foot,
+          :math:`g_i` is the gap function of the `i`th contact, and :math:`I_{c,f}` is the index set of the foot
+          contact points.
+        - foot slippage: :math:`c_{fs} = \sum ||v_{f,i}||,  \forall i s.t. g_i=0, i \in I_{c,f}`
+    - initial state generator:
+        - sample the desired forward velocity, lateral velocity and yaw rate from U(-1, 1) m/s, U(-0.4, 0.4) m/s and
+          U(-1.2, 1.2) rad/s respectively. Note that this depends on the joystick/game controller that is being used.
+        - the initial joint states are sampled from a MVN centered at the standing configuration.
+    - physics randomizer:
+        - link masses perturbed up to 10% of the original value
+        - the CoM of the base is randomly translated up to 3cm in x,y,z directions
+        - the collision geometry of the robot is approximated using collision primitives (box, cylinder, sphere) with
+          randomized shapes and positions.
+        - the coefficient of friction is sampled from :math:`U([0.8, 2.0])`.
+    - terminal condition:
+        - time limit of 4sec
+        - joint limit with terminal cost of 1.0
+        - falling (base touching the ground) with the cost of 1.0
+
+
+    Here are more information about the policy, value function, and algorithm used (with exploration strategy) in the
+    paper [1]:
+
+    - policy network: input, 128 (tanh) units, 256 (tanh) units, N output units
+    - value network: input, 128 (tanh) units, 256 (tanh) units, 1 output unit
+    - exploration in the continuous action space.
+    - RL algorithm: TRPO (but also tested PPO)
+        - KL divergence threshold (delta) = 0.01
+        - GAE: discount factor (gamma) = 0.995, lambda = 0.99
+        - for value function: Adam optimizer with learning rate = 0.001
+    - curriculum learning: constraining cost terms (power, torque, joint speed, action difference and orientation
+      costs) are scaled to 10% of the final value at the first iteration and are scaled up as the training proceeds.
+
+    Note that the authors report that they could train the behavior policy in ~5hours on a single desktop
+    machine (32 GB memory, Intel i7-8700K and Geforce GTX 1070) with a fully C++ code.
+
+
+    References:
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+    """
+
+    def __init__(self, simulator=None, robot='anymal'):
+        """
+        Initialize the standing-up environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+        """
+        # check simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet()
+        elif not isinstance(simulator, prl.simulators.Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+
+        # check if the robot has the crouching pose as joint configuration.
+        if not self.robot.has_joint_configuration('standing'):
+            raise TypeError("Expecting the robot to have the 'standing' joint configuration predefined.")
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None  # prl.terminal_conditions.TimeLimitCondition(time=6)
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(CommandedLocomotionEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                     terminal_conditions=terminal_condition,
+                                                     initial_state_generators=initial_state_generator)
+
+
+class BehaviorLocomotionEnv(LocomotionEnv):
+    r"""Behavior Locomotion Environment
+
+    This is based on the locomotion environment provided in [1] with the ANYmal robotic platform. As described in [1],
+    "the behavior selector has to choose an appropriate behavior such that the robot returns to a nominal operating
+    state (i.e. states where it can locomote) every time it loses balance." 
+    
+    Practically, this environment uses the following previously defined environments `SelfRightingEnv`,
+    `StandingUpEnv`, and `CommandedLocomotionEnv`.
+
+    - simulator: Raisim
+    - world: basic world with gravity enabled, a basic floor and the quadruped robot.
+    - state:
+        - previous discrete action (represented as a real one-hot vector) (3)
+        - desired velocity commands (forward velocity, lateral velocity, yaw rate) (3)
+        - estimated base height (h_e) (1)
+        - gravity unit vector (:math:`e_g`) expressed in the base frame (3)
+        - base angular velocity in body frame (3)
+        - base linear velocity in body frame (3)
+        - joint position and velocity states (2N)
+        - history of joint position error and velocity: current joint state at t (position error + velocity) and two
+          past states corresponding to t-0.01s and t-0.02s (6N)
+        - previous joint position targets a_{t-1} (N)
+        - additive noise for observation
+            - up to 0.2 m/s to the linear velocity
+            - up to 0.25 rad/s to the angular velocity
+            - up to 0.5 rad/s to the joint velocities
+            - up to 0.05 rad to the joint positions
+    - action: discrete action :math:`a \in \{0, 1, 2\}` represented as a real 3D vector :math:`[p_0, p_1, p_2]` (i.e.
+      the vector outputted by the policy).
+    - cost: :math:`0.001 c_{pw} + 0.05 c_{\tau} + 0.05 c_{jslim} + 0.05 c_{ad} + 0.5c_o + 10 c_\omega + 10 c_v + 3c_h`,
+      where:
+        - power: math:`c_{pw} = \sum_i^N \max(\dot{q}_i \tau_i, 0)`, where :math:`N` is the number of actuated joints,
+          :math:`\dot{q}_i` and :math:`\tau_i` are the velocity and torque (respectively) of the `i`th joint.
+        - torque: :math:`c_{\tau} = || \tau ||^2`, where :math:`\tau` are the joint torques.
+        - joint speed limit: :math:`c_{jslim} = \sum_{i}^{N} \max(\dot{q}_{i,lim} - |q_i|, 0)^2`, where :math:`N` is
+          the number of actuated joints, :math:`q_i` is the position of the i-th joint, and :math:`\dot{q}_{i,lim}` is
+          the maximum speed of the i-th joint.
+        - action difference: :math:`c_{ad} = || a_t - a_{t-1} ||^2`, where :math:`a_t` is the action vector.
+        - orientation cost: :math:`c_o = || [0,0,-1]^\top - e_g ||`, where :math:`e_g` is the unit gravity vector
+          expressed in the base frame.
+        - angular velocity: :math:`c_\omega = K(|\omega^B_B - \hat{\omega}^B_B|, 1.0)`, where :math:`\omega^B_B` is the
+          angular velocity of the base expressed in the body frame, :math:`\hat{\omega}` is the desired angular
+          velocity, and :math:`K(e, \alpha) = \frac{-1}{e^{\alpha e} + 2 + e^{\alpha e}}` is a kernel function that
+          maps :math:`\mathcal{R}` to :math:`[-0.25, 0[`.
+        - linear velocity: :math:`c_v = K(|v^B_B - \hat{v}^B_B|, 4.0)`, where :math:`v^B_B` is the linear velocity of
+          the base expressed in the body frame and math:`\hat{v}` is the desired linear velocity.
+        - height: :math:`c_h = 1.0` if base height < threshold, otherwise 0, where the threshold depends on the average
+          base height of the robot (or its maximum possible height).
+    - initial state generator:
+        - sample from the initial state distributions of a randomly selected behavior {self-righting, standing-up,
+          locomotion}.
+    - physics randomizer:
+        - link masses perturbed up to 10% of the original value
+        - the CoM of the base is randomly translated up to 3cm in x,y,z directions
+        - the collision geometry of the robot is approximated using collision primitives (box, cylinder, sphere) with
+          randomized shapes and positions.
+        - the coefficient of friction is sampled from :math:`U([0.8, 2.0])`.
+    - terminal condition:
+        - time limit of 12sec
+
+
+    Here are more information about the policy, value function, and algorithm used (with exploration strategy) in the
+    paper [1]:
+
+    - policy network: input, 128 (tanh) units, 3 output units (softmax)
+    - value network: input, 128 (tanh) units, 1 output unit
+    - exploration in the discrete action space.
+    - RL algorithm: TRPO (but also tested PPO)
+        - KL divergence threshold (delta) = 0.01
+        - GAE: discount factor (gamma) = 0.99, lambda = 0.99
+        - for value function: Adam optimizer with learning rate = 0.001
+
+    Note that the authors report that they could train the behavior policy in ~5hours on a single desktop
+    machine (32 GB memory, Intel i7-8700K and Geforce GTX 1070) with a fully C++ code.
+
+
+    References:
+        - [1] "Robust Recovery Controller for a Quadrupedal Robot using Deep Reinforcement Learning", Lee et al., 2019
+        - [2] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+    """
+
+    def __init__(self, simulator=None, robot='anymal'):
+        """
+        Initialize the locomotion with quadruped environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+        """
+        # check simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet()
+        elif not isinstance(simulator, prl.simulators.Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(BehaviorLocomotionEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                    terminal_conditions=terminal_condition,
+                                                    initial_state_generators=initial_state_generator)
+
+
+class AgileLocomotionEnv(LocomotionEnv):
+    r"""Agile locomotion environment.
+
+    This is based on the locomotion environment provided in [1] with the ANYmal robotic platform, where they introduce
+    the actuator net.
+
+    References:
+        - [1] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+    """
+
+    def __init__(self, simulator=None, robot='anymal'):
+        """
+        Initialize the locomotion with quadruped environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+        """
+        # check simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet()
+        elif not isinstance(simulator, prl.simulators.Simulator):
+            raise TypeError("Expecting the given 'simulator' to be an instance of `Simulator`, but got instead: "
+                            "{}".format(type(simulator)))
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+
+        # create state
+        state = None
+
+        # create action
+        action = None
+
+        # create reward
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        # create environment using composition
+        super(AgileLocomotionEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                 terminal_conditions=terminal_condition,
+                                                 initial_state_generators=initial_state_generator)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # # create environment
+    # env = RobustLocomotionQuadrupedEnv(sim)
+    #
+    # # run simulation
+    # for _ in count():
+    #     env.step(sleep_dt=1. / 240)

pyrobolearn/envs/manipulation/dexterity.py

@@ -0,0 +1,194 @@
+#!/usr/bin/env python
+"""Provide the manipulation dexterity environment defined in [1].
+
+Reference:
+    - [1] "Learning Dexterous In-Hand Manipulation", OpenAI et al., 2018 (https://arxiv.org/abs/1808.00177)
+"""
+
+import pyrobolearn as prl
+from pyrobolearn.envs.env import Env
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI (Paper)", "Brian Delhaisse (PRL code)"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class DexterityEnv(Env):
+    r"""Manipulation Dexterity Environment
+
+    This is based on the environment presented in [1] by OpenAI. The following
+
+    Here are the various environment features:
+
+    - simulator: MuJoCo
+    - world: basic world with gravity enabled, a basic floor, the robotic hand(s), and the cube (with letters drew on
+    it).
+        - robotic hand: shadowhand (by default), softhand, allegrohand, schunk_hand
+    - states:
+        - for value network:
+            - fingertip positions (5*3D)
+            - object position (3D)
+            - object orientation (4D=quaternion)
+            - target orientation (4D=quaternion)
+            - relative target orientation (4D=quaternion)
+            - hand joint angles (24D)
+            - hand joint velocities (24D)
+            - object velocity (3D)
+            - object angular velocity (3D)
+        - for policy:
+            - fingertip positions (5*3D)
+            - object position (3D)
+            - relative target orientation (4D=quaternion)
+    - actions: desired joint angles of the hand relative to the current ones. The actions are discretized into 11 bins.
+    - reward function:
+        - `r_t = d_t - d_{t+1}`, where `d_t` and `d_{t+1}` are the rotation angles between the desired and current
+          object orientations before and after the transition, respectively.
+        - 5 if the goal is achieved
+        - -20 if the object drop
+    - terminal condition:
+        - the goal is achieved
+        - the object drop
+    - domain randomization
+        - Gaussian noise to policy observations
+            - cor
+        - physics randomization:
+            - object dimensions: U([0.95, 1.05])
+            - object and robot link masses: U([0.5, 1.5])
+            - surface friction coefficients: U([0.7, 1.3])
+            - robot joint damping coefficients: U([0.3, 3.0])
+            - actuator force gains (P term): \log U([0.75, 1.5])
+            - additive joint limits noise: N(0, 0.15) rad
+            - additive gravity vector noise (each coordinate): N(0, 0.4) m/s^2
+        - visual appearance randomization
+            - camera positions
+            - camera intrinsics
+            - lighting conditions
+            - pose of the hand and object
+            - materials and textures for all objects in the scene (including the hand)
+
+    Here are more information about the policy, value function, and algorithm used (with exploration strategy) in the
+    paper [1]:
+
+    - policy network: fully-connected neural network composed of a normalization layer, dense ReLU (1024), LSTM (512)
+    - value network: fully-connected neural network composed of a normalization layer, dense ReLU (1024), LSTM (512)
+    - vision pose estimation network:
+        - Input: 3 RGB image of size 200x200x3
+        - Conv2D: 32 filters, 5x5 kernel size, stride 1, no padding
+        - Conv2D: 32 filters, 3x3 kernel size, stride 1, no padding
+        - Max pooling: 3x3 kernel size, stride 3
+        - ResNet: 1 block, 16 filters, 3x3 kernel size, stride 3
+        - ResNet: 2 blocks, 32 filters, 3x3 kernel size, stride 3
+        - ResNet: 2 blocks, 64 filters, 3x3 kernel size, stride 3
+        - ResNet: 2 blocks, 64 filters, 3x3 kernel size, stride 3
+        - Spatial Softmax
+        - Flatten
+        - Concatenate
+        - Fully-connected: 128 units
+        - Fully-connected: output dimensions (3 for position and 4 for orientation (quaternion))
+    - exploration: in the action space using a categorical distribution with 11 bins for each action coordinate
+    - RL algorithm: PPO
+        - clip parameter = 0.2
+        - entropy regularization coefficient = 0.01
+        - GAE: discount factor (gamma) = 0.998, lambda = 0.95
+        - optimizer: Adam with learning rate = 3e-4
+        - batch size: 80k chunks x 10 transitions = 800k transitions
+        - minibatch size: 25.6k transitions
+        - number of minibatches per step: 60
+    - SL algorithm for the vision network
+        - optimizer: Adam with learning rate = 5e-4 (halved every 20,000 batches)
+        - minibatch size: 64x3 = 192 RGB images
+        - weight decay regularization: 0.001
+        - number of training batches: 400,000
+
+    Reference:
+        - [1] "Learning Dexterous In-Hand Manipulation", OpenAI et al., 2018 (https://arxiv.org/abs/1808.00177)
+    """
+
+    def __init__(self, simulator, hand='shadowhand', num_hands=1, with_camera=False, verbose=False):
+        """
+        Initialize the manipulation dexterity environment.
+
+        Args:
+            simulator (Simulator): simulator instance.
+            hand (str):
+            num_hands (int):
+            verbose (bool): if True, it will print information when creating the environment
+            with_camera (bool): if True, it will add the cameras that are presented in the paper at the same positions.
+        """
+
+        # create world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robotic hand
+        if not isinstance(hand, str):
+           raise TypeError("Expecting a string specifying which hand we want to load in the world, but instead got: "
+                           "{}".format(type(hand)))
+        if hand[-4:] != 'hand':  # 'shadowhand', 'softhand', 'allegrohand', 'schunkhand'
+            raise ValueError("Expecting the given 'hand' to be ['shadowhand', 'softhand', 'allegrohand', "
+                             "'schunk_hand'], but instead got: {}".format(hand))
+        self.robot = world.load_robot(hand, position=(-0.2, 0, 0.5), orientation=(-0.5, 0.5, -0.5, 0.5), left=False)
+
+        if verbose:
+            self.robot.print_info()
+
+        # load cube in hand
+        path = prl.world_mesh_path + 'manipulation/cube_with_letters/cube.obj'
+        self.cube = world.load_mesh(path, position=[0.1, 0, 0.57], scale=(.05, .05, .05), flags=0, return_body=True)
+
+        # load cameras if needed
+        if with_camera:
+            pass
+
+        # create states
+        states = prl.states
+
+        state_dict = dict()
+        state_dict['value'] = None
+        state_dict['policy'] = None
+        state_dict['vision'] = None
+        self.state_dict = state_dict
+
+        # create discrete actions
+        actions = prl.actions.JointPositionChangeAction(robot, joint_ids=robot.joints, discrete_values=None)
+
+        # create terminal condition
+        drop_condition = None
+
+        terminal_conditions = [drop_condition, ]
+
+        # create reward
+        rewards = None
+
+        # create initial state generator
+        initial_state_generators = None
+
+        # create physics randomizer
+        physics_randomizers = None
+
+        # create environment using composition
+        super(DexterityEnv, self).__init__(world=world, states=states, rewards=rewards,
+                                           terminal_conditions=terminal_conditions,
+                                           initial_state_generators=initial_state_generators,
+                                           physics_randomizers=physics_randomizers, actions=actions)
+
+
+# Test
+if __name__ == '__main__':
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = DexterityEnv(sim, hand='shadowhand', verbose=True)
+
+    # run simulation
+    env.reset()
+    for _ in count():
+        env.step(sleep_dt=1. / 240)

pyrobolearn/envs/vec_env.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+"""Provide the vectorized PRL environment.
+"""
+
+import numpy as np
+import gym
+from stable_baselines.common.vec_env import VecEnv  # , VecNormalize
+import warnings
+warnings.simplefilter("ignore")
+
+from pyrobolearn.envs.env import Env
+
+
+__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 VecPRLEnv(VecEnv):
+    r"""Vectorized PRL environment.
+    """
+
+    def __init__(self, env, num_envs=1):
+        """
+        Initialize the Vectorized environment.
+
+        Args:
+            env (Env): PRL environment.
+            num_envs (int): number of environments.
+        """
+
+        # check the environment type
+        if not isinstance(env, Env):
+            raise TypeError("Expecting the given `env` to be an instance of `Env`, but got instead: "
+                            "{}".format(type(env)))
+        self.env = env
+
+        self.num_envs = num_envs if isinstance(num_envs, int) else 1
+
+        observation_space = env.observation_space
+        action_space = env.action_space
+        if action_space is None:
+            raise ValueError("The action space has not been defined for the given environment.")
+
+        super(VecPRLEnv, self).__init__(num_envs=num_envs, observation_space=observation_space,
+                                        action_space=action_space)
+
+    def reset(self):
+        """
+        Reset all the environments and return an array of observations, or a tuple of observation arrays.
+
+        If step_async is still doing work, that work will be cancelled and step_wait() should not be called
+        until step_async() is invoked again.
+
+        Returns:
+            list[int, np.array[int]], list[float, np.array[float]]: observation
+        """
+        pass
+
+    def step_async(self, actions):
+        """
+        Tell all the environments to start taking a step
+        with the given actions.
+        Call step_wait() to get the results of the step.
+
+        You should not call this if a step_async run is
+        already pending.
+        """
+        pass
+
+    @abstractmethod
+    def step_wait(self):
+        """
+        Wait for the step taken with step_async().
+
+        :return: ([int] or [float], [float], [bool], dict) observation, reward, done, information
+        """
+        pass
+
+    @abstractmethod
+    def close(self):
+        """
+        Clean up the environment's resources.
+        """
+        pass
+
+    @abstractmethod
+    def get_attr(self, attr_name, indices=None):
+        """
+        Return attribute from vectorized environment.
+
+        :param attr_name: (str) The name of the attribute whose value to return
+        :param indices: (list,int) Indices of envs to get attribute from
+        :return: (list) List of values of 'attr_name' in all environments
+        """
+        pass
+
+    @abstractmethod
+    def set_attr(self, attr_name, value, indices=None):
+        """
+        Set attribute inside vectorized environments.
+
+        :param attr_name: (str) The name of attribute to assign new value
+        :param value: (obj) Value to assign to `attr_name`
+        :param indices: (list,int) Indices of envs to assign value
+        :return: (NoneType)
+        """
+        pass
+
+    @abstractmethod
+    def env_method(self, method_name, *method_args, indices=None, **method_kwargs):
+        """
+        Call instance methods of vectorized environments.
+
+        :param method_name: (str) The name of the environment method to invoke.
+        :param indices: (list,int) Indices of envs whose method to call
+        :param method_args: (tuple) Any positional arguments to provide in the call
+        :param method_kwargs: (dict) Any keyword arguments to provide in the call
+        :return: (list) List of items returned by the environment's method call
+        """
+        pass
+
+    def step(self, actions):
+        """
+        Step the environments with the given action
+
+        :param actions: ([int] or [float]) the action
+        :return: ([int] or [float], [float], [bool], dict) observation, reward, done, information
+        """
+        self.step_async(actions)
+        return self.step_wait()
+
+    def get_images(self):
+        """
+        Return RGB images from each environment
+        """
+        raise NotImplementedError
+
+    def render(self, *args, **kwargs):
+        """
+        Gym environment rendering
+
+        :param mode: (str) the rendering type
+        """
+        logger.warn('Render not defined for %s' % self)

pyrobolearn/rewards/geometric_rewards.py

@@ -0,0 +1,294 @@
+#!/usr/bin/env python
+"""Define the costs used on link states / actions.
+"""
+
+from abc import ABCMeta
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.rewards.reward import Reward
+
+
+__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 GeometricalReward(Reward):
+    r"""Geometrical reward.
+
+    The geometrical reward uses 3D geometric shapes to describe a reward function. This can notably be used for reward
+    shaping to guide the agent in the 3D space.
+    """
+
+    def __init__(self):
+        super(GeometricalReward, self).__init__()
+
+        self._visual = None
+
+    def _compute(self):
+        """Compute and return the reward value."""
+        pass
+
+    def draw(self):
+        """Draw the visual shape in the simulator."""
+        pass
+
+
+class SphericalReward(GeometricalReward):
+    r"""Spherical reward.
+
+    """
+
+    def __init__(self, bodies, link_ids=-1, attached_body=None, attached_link_id=-1, position=None, orientation=None,
+                 radius=1, theta=(0, 2*np.pi), phi=(0, np.pi), radius_reward_range=None, theta_reward_ranges=None,
+                 height_reward_range=None, interpolation='linear', simulator=None):
+        """
+        Initialize the spherical reward.
+
+        Args:
+            bodies (int, Body, list[int], list[Body]): the bodies or body unique ids that we should check if they are
+                inside the sphere.
+            link_ids (int, list[int]): the link id associated to each body. By default, it is -1 for the base.
+            attached_body (Body, int, None): the body instance or unique id to which the spherical reward is
+                attached to. If None, it will be attached to the world frame.
+            attached_link_id (int, None): the link id to which the reward is attached to. By default, it is -1 for the
+                base.
+            position (np.array/list/tuple[float[3]], None): local position of the spherical reward. If None, it will
+                be the origin (0,0,0).
+            orientation (np.array/list/tuple[float[4]], None): local orientation (expressed as a quaternion [x,y,z,w])
+                of the spherical reward. If None, it will be the unit quaternion [0,0,0,1].
+            radius (float, tuple[float[2]]): radius of the sphere. If two radii are provided, the first one is the
+                inner radius and the second one is the outer radius of the sphere.
+            theta (tuple[float[2]]): the lower and upper bounds of the theta angle.
+            phi (tuple[float[2]]): the lower and upper bounds of the phi angle.
+            radius_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(r,v)` where the first item represents the radius value
+                `r` and the second item represents the associated reward value `v`.
+            theta_reward_ranges (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(t,v)` where the first item represents the theta angle
+                value `t` and the second item represents the associated reward value `v`.
+            height_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(h,v)` where the first item represents the height value
+                `h` and the second item represents the associated reward value `v`.
+            interpolation (str): the interpolation method to use for the given reward ranges. Currently, you can select
+                between 'linear' or 'step'.
+            simulator (Simulator, None): if the given bodies are all unique ids, the simulator instance has to be
+                provided.
+        """
+        pass
+
+    def _compute(self):
+        """Compute and return the reward value."""
+        pass
+
+    def draw(self):
+        """Draw the visual shape in the simulator."""
+        # if self._visual is None:
+        #     visual_shape = self.sim.create_visual_shape(self.sim.GEOM_SPHERE, radius=radius, rgba_color=color)
+        #     sphere = self.sim.create_body(visual_shape_id=visual_shape, mass=0., position=position)
+        pass
+
+
+class RectangularReward(GeometricalReward):
+    r"""Rectangular reward
+
+    """
+
+    def __init__(self, bodies, link_ids=-1, attached_body=None, attached_link_id=-1, position=None, orientation=None,
+                 a=1, b=1, c=1, radius_reward_range=None, theta_reward_ranges=None,
+                 height_reward_range=None, interpolation='linear', simulator=None):
+        """
+        Initialize the rectangular reward.
+
+        Args:
+            bodies (int, Body, list[int], list[Body]): the bodies or body unique ids that we should check if they are
+                inside the rectangle.
+            link_ids (int, list[int]): the link id associated to each body. By default, it is -1 for the base.
+            attached_body (Body, int, None): the body instance or unique id to which the rectangular reward is
+                attached to. If None, it will be attached to the world frame.
+            attached_link_id (int, None): the link id to which the reward is attached to. By default, it is -1 for the
+                base.
+            position (np.array/list/tuple[float[3]], None): local position of the rectangular reward. If None, it will
+                be the origin (0,0,0).
+            orientation (np.array/list/tuple[float[4]], None): local orientation (expressed as a quaternion [x,y,z,w])
+                of the rectangular reward. If None, it will be the unit quaternion [0,0,0,1].
+            a (float, tuple[float[2]]): radius of the rectangle. If two radii are provided, the first one is the
+                inner radius and the second one is the outer radius of the rectangle.
+            b (tuple[float[2]]): the lower and upper bound of the
+            c (float): the height/length of the rectangle.
+            radius_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(r,v)` where the first item represents the radius value
+                `r` and the second item represents the associated reward value `v`.
+            theta_reward_ranges (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(t,v)` where the first item represents the theta angle
+                value `t` and the second item represents the associated reward value `v`.
+            height_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(h,v)` where the first item represents the height value
+                `h` and the second item represents the associated reward value `v`.
+            interpolation (str): the interpolation method to use for the given reward ranges. Currently, you can select
+                between 'linear' or 'step'.
+            simulator (Simulator, None): if the given bodies are all unique ids, the simulator instance has to be
+                provided.
+        """
+        pass
+
+    def _compute(self):
+        """Compute and return the reward value."""
+        pass
+
+    def draw(self):
+        """Draw the visual shape in the simulator."""
+        pass
+
+
+class EllipsoidalReward(GeometricalReward):
+    r"""Ellipsoidal reward
+
+    """
+
+    def __init__(self, bodies, link_ids=-1, attached_body=None, attached_link_id=-1, position=None, orientation=None,
+                 a=1, b=1, c=1, radius_reward_range=None, theta_reward_ranges=None,
+                 height_reward_range=None, interpolation='linear', simulator=None):
+        """
+        Initialize the ellipsoidal reward.
+
+        Args:
+            bodies (int, Body, list[int], list[Body]): the bodies or body unique ids that we should check if they are
+                inside the ellipsoid.
+            link_ids (int, list[int]): the link id associated to each body. By default, it is -1 for the base.
+            attached_body (Body, int, None): the body instance or unique id to which the ellipsoidal reward is
+                attached to. If None, it will be attached to the world frame.
+            attached_link_id (int, None): the link id to which the reward is attached to. By default, it is -1 for the
+                base.
+            position (np.array/list/tuple[float[3]], None): local position of the ellipsoidal reward. If None, it will
+                be the origin (0,0,0).
+            orientation (np.array/list/tuple[float[4]], None): local orientation (expressed as a quaternion [x,y,z,w])
+                of the ellipsoidal reward. If None, it will be the unit quaternion [0,0,0,1].
+            a (float, tuple[float[2]]): length of the first semi-axis. If tuple, it is the lower and upper bounds of
+                the length of the first semi-axis.
+            b (float, tuple[float[2]]): length of the second semi-axis. If tuple, it is the lower and upper bounds of
+                the length of the second semi-axis.
+            c (float, tuple[float[2]]): length of the third semi-axis. If tuple, it is the lower and upper bounds of
+                the length of the third semi-axis.
+            radius_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(r,v)` where the first item represents the radius value
+                `r` and the second item represents the associated reward value `v`.
+            theta_reward_ranges (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(t,v)` where the first item represents the theta angle
+                value `t` and the second item represents the associated reward value `v`.
+            height_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(h,v)` where the first item represents the height value
+                `h` and the second item represents the associated reward value `v`.
+            interpolation (str): the interpolation method to use for the given reward ranges. Currently, you can select
+                between 'linear' or 'step'.
+            simulator (Simulator, None): if the given bodies are all unique ids, the simulator instance has to be
+                provided.
+        """
+        pass
+
+    def _compute(self):
+        """Compute and return the reward value."""
+        pass
+
+    def draw(self):
+        """Draw the visual shape in the simulator."""
+        pass
+
+
+class CylindricalReward(GeometricalReward):
+    r"""Cylindrical reward
+    """
+
+    def __init__(self, bodies, link_ids=-1, attached_body=None, attached_link_id=-1, position=None, orientation=None,
+                 radius=1, theta=(0, 2*np.pi), height=1, radius_reward_range=None, theta_reward_ranges=None,
+                 height_reward_range=None, interpolation='linear', simulator=None):
+        """
+        Initialize the cylindrical reward.
+
+        Args:
+            bodies (int, Body, list[int], list[Body]): the bodies or body unique ids that we should check if they are
+                inside the cylinder.
+            link_ids (int, list[int]): the link id associated to each body. By default, it is -1 for the base.
+            attached_body (Body, int, None): the body instance or unique id to which the cylindrical reward is
+                attached to. If None, it will be attached to the world frame.
+            attached_link_id (int, None): the link id to which the reward is attached to. By default, it is -1 for the
+                base.
+            position (np.array/list/tuple[float[3]], None): local position of the cylindrical reward. If None, it will
+                be the origin (0,0,0).
+            orientation (np.array/list/tuple[float[4]], None): local orientation (expressed as a quaternion [x,y,z,w])
+                of the cylindrical reward. If None, it will be the unit quaternion [0,0,0,1].
+            radius (float, tuple[float[2]]): radius of the cylinder. If two radii are provided, the first one is the
+                inner radius and the second one is the outer radius of the cylinder.
+            theta (tuple[float[2]]): the lower and upper bounds of the theta angle.
+            height (float): the height/length of the cylinder.
+            radius_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(r,v)` where the first item represents the radius value
+                `r` and the second item represents the associated reward value `v`.
+            theta_reward_ranges (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(t,v)` where the first item represents the theta angle
+                value `t` and the second item represents the associated reward value `v`.
+            height_reward_range (list[tuple[float[2]]], list/tuple[float[2]]): If a list / tuple of 2 floats, it is
+                the lower and upper bounds of the reward range. If a list of tuple of 2 floats, for each item
+                in the list, it must be a tuple of length 2 `(h,v)` where the first item represents the height value
+                `h` and the second item represents the associated reward value `v`.
+            interpolation (str): the interpolation method to use for the given reward ranges. Currently, you can select
+                between 'linear' or 'step'.
+            simulator (Simulator, None): if the given bodies are all unique ids, the simulator instance has to be
+                provided.
+        """
+        pass
+
+    def _compute(self):
+        """Compute and return the reward value."""
+        pass
+
+    def draw(self):
+        """Draw the visual shape in the simulator."""
+        pass
+
+
+class CompositeGeometricalReward(GeometricalReward):
+    r"""Composite geometrical reward
+
+    This class is useful if you have overlapping geometrical rewards and you wish to prioritize some shapes over others
+    when computing the reward function.
+    """
+
+    def __init__(self, rewards, priorities):
+        r"""
+        Initialize the composite geometrical reward.
+
+        Args:
+            rewards (list[GeometricalReward]): list of geometrical rewards.
+            priorities (list[int]): list of priorities, where each priority is associated with the given geometrical
+                reward. The length of this list must match the number of given rewards. Two rewards can have the same
+                priorities, and if they overlap their returned reward values are added.
+        """
+        super(CompositeGeometricalReward, self).__init__()
+        self.rewards = rewards
+        self.priorities = priorities
+
+    def _compute(self):
+        """Compute and return the reward value."""
+        pass
+
+    def draw(self):
+        """Draw the visual shape in the simulator."""
+        pass

pyrobolearn/rewards/terminal_rewards.py

@@ -26,19 +26,19 @@ class TerminalReward(Reward):
     value once the goal has been achieved (e.g. games).
     """
 
-    def __init__(self, terminal_condition, subreward, final_reward):
+    def __init__(self, terminal_conditions, subreward, final_reward):
         r"""
         Terminal reward.
 
         Args:
-            terminal_condition (TerminalCondition): terminal condition.
+            terminal_conditions (TerminalCondition, list of TerminalCondition): terminal condition(s).
             subreward (Reward, float, int): sub reward that is called until the terminal condition is not fulfilled.
             final_reward (Reward, float, int): final reward that is called when the terminal condition has been reached.
         """
         super(TerminalReward, self).__init__()
 
         # set the attributes
-        self.terminal_condition = terminal_condition
+        self.terminal_conditions = terminal_conditions
         self.subreward = subreward
         self.final_reward = final_reward
 
@@ -47,17 +47,26 @@ class TerminalReward(Reward):
     ##############
 
     @property
-    def terminal_condition(self):
-        """Return the terminal condition instance."""
-        return self._terminal_condition
+    def terminal_conditions(self):
+        """Return the terminal condition instances."""
+        return self._terminal_conditions
 
-    @terminal_condition.setter
-    def terminal_condition(self, condition):
-        """Set the terminal condition instance."""
-        if not isinstance(condition, TerminalCondition):
-            raise TypeError("Expecting the given 'terminal_condition' to be an instance of `TerminalCondition`, "
-                            "instead got: {}".format(type(condition)))
-        self._terminal_condition = condition
+    @terminal_conditions.setter
+    def terminal_conditions(self, conditions):
+        """Set the terminal condition instances."""
+        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 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:
+            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 subreward(self):
@@ -95,7 +104,7 @@ class TerminalReward(Reward):
 
     def _compute(self):
         """Compute the terminal reward."""
-        done = self.terminal_condition()
+        done = any([condition() for condition in self.terminal_conditions])
         if done:
             return self.final_reward()
         return self.subreward()

pyrobolearn/robots/anymal.py

@@ -80,18 +80,23 @@ class ANYmal(QuadrupedRobot):
 
         # init configuration
         self.reset_joint_states(q=[0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8])
+        # self.reset_joint_states(q=[0.052, 1.66, -2.8, -0.052, 1.66, -2.8, 0.052, -1.66, 2.8, -0.052, -1.66, 2.8])
 
         # some values are taken from "raisimGym/raisim_gym/env/env/ANYmal/Environment.hpp"
         self.base_height = 0.54
         self.avg_height = 0.44
 
+        self._joint_configuration = {'home': np.array([0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03,
+                                                       -0.4, 0.8]),
+                                     'standing': 'home',
+                                     'init': 'home',
+                                     'crouching': np.array([0.052, 1.66, -2.8, -0.052, 1.66, -2.8, 0.052, -1.66, 2.8,
+                                                            -0.052, -1.66, 2.8]),
+                                     'lying': 'crouching'}
+
     def get_home_joint_positions(self):
         """Return the joint positions for the home position."""
-        return np.array([0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8])
-
-    def get_joint_configurations(self, name=None):
-        if name == 'home' or name == 'init':
-            return np.array([0.03, 0.4, -0.8, -0.03, 0.4, -0.8, 0.03, -0.4, 0.8, -0.03, -0.4, 0.8])
+        return self._joint_configuration['home']
 
 
 # Test
@@ -118,5 +123,7 @@ if __name__ == "__main__":
     # run simulator
     for _ in count():
         # robot.update_joint_slider()
+        robot.step()
         print("BASE HEIGHT: {}".format(robot.get_base_position()[2]))
+        print(robot.get_joint_positions())
         world.step(sleep_dt=1./240)

pyrobolearn/robots/legged_robot.py

@@ -769,7 +769,7 @@ class LeggedRobot(Robot):
         Update all visuals.
         """
         # update robot visuals
-        super(LeggedRobot, self).update_visual()
+        super(LeggedRobot, self).update_visuals()
 
         # update support polygon
 

pyrobolearn/robots/robot.py

@@ -176,6 +176,9 @@ class Robot(ControllableBody):
         self.sensors = {}  # dict of sensors {SensorClass: [sensorInstance]}
         self.actuators = {}  # dict of actuators {ActuatorClass: [actuatorInstance]}
 
+        # joint configurations
+        self._joint_configuration = {}  # {str: np.array[float[N]]} or {str: tuple(list[int], np.array[float[N]])}
+
     #############
     # Operators #
     #############
@@ -1465,13 +1468,32 @@ class Robot(ControllableBody):
 
         Returns:
             if name is None:
-                list:
-                    str: name of each joint configuration.
+                list[str]: name of each joint configuration.
             else:
-                np.array[float[M]]: joint ids to move.
+                list[int[M]]: joint ids to move.
                 np.array[float[M]]: joint positions.
         """
-        pass
+        if name is None:
+            return list(self._joint_configuration.keys())
+        if name in self._joint_configuration:
+            item = self._joint_configuration[name]
+            if isinstance(item, str):  # the item is an alias
+                return self._joint_configuration[item]
+            return item
+
+    def has_joint_configuration(self, name):
+        """
+        Check if the robot has the specified joint configuration.
+
+        This method has to be implemented in the child class.
+
+        Args:
+            name (str): name of the joint configuration to move the robot to.
+
+        Returns:
+            bool: True if the robot has the specified joint configuration.
+        """
+        return name in self._joint_configuration
 
     ##################################
     # Links (task/operational space) #

pyrobolearn/simulators/bullet.py

@@ -1834,7 +1834,7 @@ class Bullet(Simulator):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1863,7 +1863,7 @@ class Bullet(Simulator):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1892,7 +1892,7 @@ class Bullet(Simulator):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1918,7 +1918,7 @@ class Bullet(Simulator):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1945,7 +1945,7 @@ class Bullet(Simulator):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:

pyrobolearn/simulators/simulator.py

@@ -1305,7 +1305,7 @@ class Simulator(object):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1324,7 +1324,7 @@ class Simulator(object):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1343,7 +1343,7 @@ class Simulator(object):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1359,7 +1359,7 @@ class Simulator(object):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1376,7 +1376,7 @@ class Simulator(object):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:
@@ -1396,7 +1396,7 @@ class Simulator(object):
 
         Args:
             body_id (int): unique body id.
-            link_ids (list[int]): list of link indices.
+            link_ids (int, list[int]): link index, or list of link indices.
 
         Returns:
             if 1 link:

pyrobolearn/states/merged_space.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+"""Define the merged space.
+
+This is a space that merges the various spaces together based on each dimension.
+"""
+
+import copy
+import numpy as np
+import gym
+
+
+__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 MergedSpace(gym.spaces.Space):
+    r"""Merged Space.
+
+
+    """
+
+    def __init__(self, spaces):
+        self.spaces = spaces
+        super(MergedSpace, self).__init__(shape=None, dtype=None)
+
+    @property
+    def spaces(self):
+        return self._spaces
+
+    @spaces.setter
+    def spaces(self, spaces):
+        if isinstance(spaces, gym.spaces.Space):
+            spaces = [spaces]
+        if not isinstance(spaces, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given spaces to be a list/tuple/np.ndarray of `gym.spaces.Space`, but "
+                            "got instead: {}".format(type(spaces)))
+        for i, space in enumerate(spaces):
+            if not isinstance(space, gym.spaces.Space):
+                raise TypeError("Expecting the {}th item to be an instance of `gym.spaces.Space`, but got instead: "
+                                "{}".format(i, type(space)))
+        self._spaces = spaces
+
+    def sample(self):
+        """
+        Uniformly randomly sample a random element of this space.
+        """
+        raise NotImplementedError
+
+    def seed(self, seed):
+        """Set the seed for this space's pseudo-random number generator."""
+        if seed is not None:
+            for space in self.spaces:
+                space.seed(seed)
+
+    def contains(self, x):
+        """
+        Return boolean specifying if x is a valid member of this space.
+        """
+        raise NotImplementedError

pyrobolearn/states/state.py

@@ -60,9 +60,8 @@ class State(object):
         policy = NNPolicy(states, actions)
 
     References:
-        [1] "Wikipedia: Composition over Inheritance", https://en.wikipedia.org/wiki/Composition_over_inheritance
-        [2] "OpenAI gym": https://gym.openai.com/   and    https://github.com/openai/gym
-
+        - [1] "Wikipedia: Composition over Inheritance", https://en.wikipedia.org/wiki/Composition_over_inheritance
+        - [2] "OpenAI gym": https://gym.openai.com/   and    https://github.com/openai/gym
     """
 
     def __init__(self, states=(), data=None, space=None, window_size=1, axis=None, ticks=1, name=None):
@@ -385,14 +384,22 @@ class State(object):
         """
         return torch.cat([data.reshape(-1) for data in self.merged_torch_data])
 
+    @property
+    def spaces(self):
+        if self.has_space():
+            return [self._space]
+        return [state._space for state in self._states]
+
     @property
     def space(self):
         """
         Get the corresponding space.
         """
         if self.has_space():
-            return [self._space]
-        return [state._space for state in self._states]
+            # return [self._space]
+            return gym.spaces.Tuple([self._space])
+        # return [state._space for state in self._states]
+        return gym.spaces.Tuple([state._space for state in self._states])
 
     @space.setter
     def space(self, space):
@@ -402,6 +409,13 @@ class State(object):
         if self.has_data() and not self.has_space() and isinstance(space, (gym.spaces.Box, gym.spaces.Discrete)):
             self._space = space
 
+    @property
+    def merged_space(self):
+        """
+        Get the corresponding merged space.
+        """
+        return False
+
     @property
     def name(self):
         """
@@ -743,8 +757,8 @@ class State(object):
         """
         if self.is_combined_states():
             return [state.sample() for state in self._states]
-        if self._distribution is None:
-            return
+        if self._distribution is None:  # uniform distribution
+            return self._space.sample()
         else:
             pass
         raise NotImplementedError
@@ -786,8 +800,8 @@ class State(object):
         it will be min(dimension, axis).
 
         Examples:
-            s0 = JntPositionState(robot)
-            s1 = JntVelocityState(robot)
+            s0 = JointPositionState(robot)
+            s1 = JointVelocityState(robot)
             s = s0 & s1
             print(s)
             print(s.shape)

pyrobolearn/storages/er.py

@@ -2,8 +2,8 @@
 """Provides the experience replay (ER) storage.
 
 References:
-    [1] "Reinforcement Learning for robots using neural networks", Lin, 1993
-    [2] "Playing Atari with Deep Reinforcement Learning", Mnih et al., 2013
+    - [1] "Reinforcement Learning for robots using neural networks", Lin, 1993
+    - [2] "Playing Atari with Deep Reinforcement Learning", Mnih et al., 2013
 """
 
 import random
@@ -37,8 +37,8 @@ __status__ = "Development"
 #     The following code is inspired by [3] but modified such that it uses a PyTorch list storage.
 #
 #     References:
-#         [1] "Reinforcement Learning for robots using neural networks", Lin, 1993
-#         [2] "Playing Atari with Deep Reinforcement Learning", Mnih et al., 2013
+#         - [1] "Reinforcement Learning for robots using neural networks", Lin, 1993
+#         - [2] "Playing Atari with Deep Reinforcement Learning", Mnih et al., 2013
 #     """
 #
 #     def __init__(self, capacity=10000, device=None, dtype=torch.float):
@@ -172,8 +172,8 @@ class ExperienceReplay(DictStorage):  # ExperienceReplayStorage(DictStorage):
     The following code is inspired by [3] but modified such that it uses a PyTorch list storage.
 
     References:
-        [1] "Reinforcement Learning for robots using neural networks", Lin, 1993
-        [2] "Playing Atari with Deep Reinforcement Learning", Mnih et al., 2013
+        - [1] "Reinforcement Learning for robots using neural networks", Lin, 1993
+        - [2] "Playing Atari with Deep Reinforcement Learning", Mnih et al., 2013
     """
 
     def __init__(self, state_shapes, action_shapes, capacity=10000, *args, **kwargs):

pyrobolearn/storages/her.py

@@ -8,7 +8,7 @@ References:
 from pyrobolearn.storages.er import ExperienceReplay
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -22,9 +22,9 @@ class HindsightExperienceReplay(ExperienceReplay):
 
     One of the main challenges in RL is to shape the reward function such that the agent can successfully learned to
     perform the specified task. This often requires expert knowledge to engineer this reward function.
-    To address this, the authors from [1] proposes to use a hindsight experience replay, which enables learning from
-    sparse and binary rewards, and can be combined with any off-policy RL algorithms. This notably improves the
-    sample efficiency.
+    To address this, the authors from [1] propose to use a hindsight experience replay storage unit, which enables
+    learning from sparse and binary rewards, and can be combined with any off-policy RL algorithms. This notably
+    improves the sample efficiency.
 
     In this setting, one or several goals have to be defined. They are concatenated with the state and feed to the
     policy and value approximators. Additionally, they are included in the transition tuple sampled from the
@@ -64,9 +64,19 @@ class HindsightExperienceReplay(ExperienceReplay):
 
 
     References:
-        [1] "Hindsight Experience Replay", Andrychowicz et al., 2017
+        - [1] "Hindsight Experience Replay", Andrychowicz et al., 2017
     """
-    pass
+
+    def __init__(self, state_shapes, action_shapes, capacity=10000, *args, **kwargs):
+        """
+        Initialize the experience replay storage.
+
+        Args:
+            state_shapes (list[tuple[int]], tuple[int]): each tuple represents the shape of an observation/state.
+            action_shapes (list[tuple[int]], tuple[int]): each tuple represents the shape of an action.
+            capacity (int): maximum size of the experience replay storage.
+        """
+        super(HindsightExperienceReplay, self).__init__(state_shapes, action_shapes, capacity, *args, **kwargs)
 
 
 # alias

pyrobolearn/storages/per.py

@@ -9,13 +9,13 @@ normalized for stability reasons) are used.
 In summary, PER can be seen as a stochastic prioritization ER which uses importance sampling.
 
 References:
-    [1] "Prioritized Experience Replay", Schaul, 2015
+    - [1] "Prioritized Experience Replay", Schaul, 2015
 """
 
 from pyrobolearn.storages.storage import PriorityQueueStorage
 
 __author__ = "Brian Delhaisse"
-__copyright__ = "Copyright 2018, PyRoboLearn"
+__copyright__ = "Copyright 2019, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
 __license__ = "GNU GPLv3"
 __version__ = "1.0.0"
@@ -34,9 +34,11 @@ class PrioritizedExperienceReplay(PriorityQueueStorage):
 
     In summary, PER can be seen as a stochastic prioritization ER which uses importance sampling.
 
-    There are 2 stochastic prioritization schemes used in [1].
+    There are 2 stochastic prioritization schemes used in [1]:
+
     - proportional prioritization: :math:`p_i = |\delta_i| + \epsilon`, where :math:`\epsilon` is a small positive
     constant to avoid the transition to have a probability of 0.
+
     - rank-based prioritization: math:`p_i = \frac{1}{rank(i)}`, where rank(i) is the rank of transition i (that is
     they are i other keys in the priority queue that are smaller than the current key i) when the replay memory is
     sorted according to :math:`|\delta_i|`
@@ -68,9 +70,11 @@ class PrioritizedExperienceReplay(PriorityQueueStorage):
 
 
     References:
-        [1] "Prioritized Experience Replay", Schaul, 2015
+        - [1] "Prioritized Experience Replay", Schaul, 2015
     """
-    pass
+
+    def __init__(self):
+        super(PrioritizedExperienceReplay, self).__init__()
 
 
 # alias