Merge pull request #2 from robotlearn/master

Update

CITATION.rst

@@ -0,0 +1,16 @@
+Citation
+--------
+
+.. code-block:: latex
+
+    @misc{delhaisse2019pyrobolearn,
+        author = {Delhaisse, Brian and Xin, Songyan and Rozo, Leonel, and Caldwell, Darwin},
+        title = {PyRoboLearn: A Python Framework for Robot Learning Practitioners},
+        publisher = {GitHub},
+        journal = {GitHub repository},
+        howpublished = {\url{https://github.com/robotlearn/pyrobolearn}},
+        year=2019,
+    }
+
+
+If you use a specific learning model, algorithm, robot, controller, and so on, please cite the corresponding paper. The reference(s) can usually be found in the class documentation (at the end), and sometimes in the README file in the corresponding folder.

README.rst

@@ -10,75 +10,21 @@ This framework revolves mainly around 7 axes: simulators, worlds, robots, interf
 Requirements
 ------------
 
-The framework has been tested with Python 2.7 and Ubuntu 16.04 and 18.04. We also tested parts of it with Python 3.5 on Ubuntu 16.04 and so far so good, but there might be some errors that escaped my attention.
+The framework has been tested with Python 2.7, 3.5 and 3.6, on Ubuntu 16.04 and 18.04. The installation on other OS is
+experimental.
 
 
 Installation
 ------------
 
-Docker
-~~~~~~
+There are two ways to install the framework:
 
-At the moment the docker is a self contained Ubuntu image with all the libraries installed. When launched we have access to a Python3.6 interpreter and we can import pyrobolearn directly.
-In the future, ROS may be splitted in another container and linked to this one.
-
-1. Install Docker and nvidia-docker
-
-.. code-block:: bash
-
-	sudo apt-get update
-	sudo apt install apt-transport-https ca-certificates curl software-properties-common
-	curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add -
-	sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable # you should replace bionic by your version
-	sudo apt update
-	sudo apt install docker-ce
-	sudo systemctl status docker # check that docker is active
-
-2. Build the image
-
-.. code-block:: bash
-
-	docker build -t pyrobolearn .
+1. using a virtual environment and pip
+2. using a Docker
 
 
-3. Launch
-
-
-You can now start the python interpreter with every library already installed
-
-.. code-block:: bash
-
-	docker run -p 11311:11311 -v $PWD/dev:/pyrobolearn/dev/:rw -ti pyrobolearn python3
-
-
-To open an interactive terminal in the docker image use:
-
-.. code-block:: bash
-
-	docker run -p 11311:11311 -v $PWD/dev:/pyrobolearn/dev/:rw -ti pyrobolearn /bin/bash
-
-
-4. nvidia-docker
-if the GPU is not recognized in the interpreter, you can install nvidia-docker
-
-.. code-block:: bash
-	
-	curl -sL https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add -
-	distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
-	curl -sL https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
-	sudo apt-get update
-	sudo apt-get install nvidia-docker2
-	sudo pkill -SIGHUP dockerd
-
-And use:
-
-.. code-block:: bash
-
-	nvidia-docker run -p 11311:11311 -v $PWD/dev:/pyrobolearn/dev/:rw -ti pyrobolearn
-
-
-Ubuntu
-~~~~~~
+Virtualenv & Pip
+~~~~~~~~~~~~~~~~
 
 1. First download the ``pip`` Python package manager and create a virtual environment for Python as described in the following link: https://packaging.python.org/guides/installing-using-pip-and-virtualenv/
 On Ubuntu, you can install ``pip`` and ``virtualenv`` by typing in the terminal: 
@@ -143,23 +89,115 @@ Depending on your computer configuration and the python version you use, you mig
 	sudo apt install python3-tk  # if python 3.5
 
 
+Docker
+~~~~~~
+
+At the moment the docker is a self contained Ubuntu image with all the libraries installed. When launched we have access to a Python3.6 interpreter and we can import pyrobolearn directly.
+In the future, ROS may be splitted in another container and linked to this one.
+
+1. Install Docker and nvidia-docker
+
+.. code-block:: bash
+
+	sudo apt-get update
+	sudo apt install apt-transport-https ca-certificates curl software-properties-common
+	curl -fsSL https://download.docker.com/linux/ubuntu/gpg | sudo apt-key add -
+	sudo add-apt-repository "deb [arch=amd64] https://download.docker.com/linux/ubuntu bionic stable # you should replace bionic by your version
+	sudo apt update
+	sudo apt install docker-ce
+	sudo systemctl status docker # check that docker is active
+
+2. Build the image
+
+.. code-block:: bash
+
+	docker build -t pyrobolearn .
+
+
+3. Launch
+
+
+You can now start the python interpreter with every library already installed
+
+.. code-block:: bash
+
+	docker run -p 11311:11311 -v $PWD/dev:/pyrobolearn/dev/:rw -ti pyrobolearn python3
+
+
+To open an interactive terminal in the docker image use:
+
+.. code-block:: bash
+
+	docker run -p 11311:11311 -v $PWD/dev:/pyrobolearn/dev/:rw -ti pyrobolearn /bin/bash
+
+
+4. nvidia-docker
+if the GPU is not recognized in the interpreter, you can install nvidia-docker
+
+.. code-block:: bash
+	
+	curl -sL https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add -
+	distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
+	curl -sL https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
+	sudo apt-get update
+	sudo apt-get install nvidia-docker2
+	sudo pkill -SIGHUP dockerd
+
+And use:
+
+.. code-block:: bash
+
+	nvidia-docker run -p 11311:11311 -v $PWD/dev:/pyrobolearn/dev/:rw -ti pyrobolearn
+
+
+Other Operating Systems
+~~~~~~~~~~~~~~~~~~~~~~~
+
+Note that some interfaces (like game controllers, depth camera, etc) might not be available on other OS, however the 
+main robotic framework should work.
+
+1. Windows: You will have to install first PyBullet and NLopt beforehand.
+
+For nlopt, install first ``conda``, then type:
+
+.. code-block:: bash
+
+	conda install -c conda-forge nlopt
+
+If Pybullet doesn't install on Windows (using visual studio), you might have to copy ``rc.exe`` and ``rc.dll`` from
+
+``C:\Program Files (x86)\Windows Kits\10\bin\<xx.x.xxxx.x>\x64``
+
+to
+
+``C:\Program Files (x86)\Windows Kits\10\bin\x86``
+
+And add the last folder to the Windows environment path (Go to ``System Properties`` > ``Advanced`` > ``Environment Variables`` > ``Path`` 
+> ``Edit``).
+
+Finally, remove the nlopt package from the ``requirements.txt``. The rest of the installation should be straightforward.
+
+
+2. Mac OSX: We managed to install the PyRoboLearn framework on MacOSX (Mojave) by following the procedures explained in the section 
+"Virtualenv & Pip". You can replace the ``sudo apt install`` by ``brew install`` (after installing `Homebrew <https://brew.sh/>`_).
+
+
 How to use it?
 --------------
 
 Check the ``README.rst`` file in the ``examples`` folder.
 
+
+License
+-------
+
+PyRoboLearn is currently released under the `GNU GPLv3 <https://choosealicense.com/licenses/gpl-3.0/>`_ license.
+
+
 Citation
 --------
 
-.. code-block:: latex
-
-	@misc{delhaisse2019pyrobolearn,
-    	author = {Delhaisse, Brian and Xin, Songyan and Rozo, Leonel, and Caldwell, Darwin},
-    	title = {PyRoboLearn: A Python Framework for Robot Learning Practitioners},
-    	howpublished = {\url{https://github.com/robotlearn/pyrobolearn}},
-    	year=2019,
-	}
-
+For how to cite this repository, please refer to the ``CITATION.rst`` file.
 
 If you use a specific learning model, algorithm, robot, controller, and so on, please cite the corresponding paper. The reference(s) can usually be found in the class documentation (at the end), and sometimes in the README file in the corresponding folder.
 
@@ -169,6 +207,8 @@ Acknowledgements
 
 Currently, we mainly use the PyBullet simulator.
 
-- *PyBullet, a Python module for physics simulation for games, robotics and machine learning*, Erwin Coumans and Yunfei Bai, 2016-2019
-- references for each robot, model, and others can be found in the corresponding class documentation
-- Locomotion controllers were provided by Songyan Xin (see ``pyrobolearn/controllers/locomotion``)
+- *PyBullet, a Python module for physics simulation for games, robotics and machine learning*, Erwin Coumans and
+  Yunfei Bai, 2016-2019
+- References for each robot, model, and others can be found in the corresponding class documentation
+- Locomotion controllers were provided by Songyan Xin
+- We thanks Daniele Bonatto for providing the Docker file, and test the installation on Windows.

docs/source/models.rst

@@ -13,7 +13,7 @@ Learning models can be divided into 2 categories:
 	- Polynomial models
 	- Deep Neural Networks (DNNs)
 	- Gaussian processes (GPs)
-- Trajectory based learning models:
+- Trajectory based learning models
 	- Dynamic Movement Primitives (DMPs)
 	- Central Pattern Generators (CPGs)
 	- Gaussian Mixture Models and Gaussian Mixture Regression (GMMs/GMRs)

docs/source/priorities.rst

@@ -1,3 +0,0 @@
-Priority Tasks
-==============
-

docs/source/pyrobolearn.rst

@@ -1,33 +1,15 @@
 PyRoboLearn
 ===========
 
-PyRoboLearn is a Python framework in robot learning for education and research. PyRoboLearn is meant to be a free and open-source tool.
+PyRoboLearn is a Python framework in robot learning for education and research. PyRoboLearn (PRL) is meant to be a free and open-source tool, and is currently released under the GNU GPLv3 license.
 
-Goal
-
-
-Problem formulation
--------------------
-
-General idea.
-
-- lack of benchmarks
-- lack of flexibility and modularity
-- lack of generalization
-- high coupling
-
-For instance:
-
-Full example.
-
-
-Main idea of PyRoboLearn and solution to above problem.
+We were motivated to create this framework notably because of the lack of benchmarks, flexibility and modularity, generalization, and the hight coupling between different modules. These problems influenced our design decisions (see below).
 
 
 Hardware/Software requirements
 ------------------------------
 
-The PyRoboLearn framework has been tested on Ubuntu 16.04 and 18.04, with Python 2.7, 3.5 and 3.6.
+The PyRoboLearn framework has been tested on Ubuntu 16.04 and 18.04, with Python 2.7, 3.5 and 3.6. It has also been successfully installed on Windows 10 and MacOSX, but not all features such as interfaces as well as simulators are available with these operating systems.
 
 
 Design Decisions
@@ -38,15 +20,153 @@ While designing PRL, we focused on the five following features:
 - modularity: design a module (i.e. class) for each different concept
 - abstraction: add a layer of abstraction for combination of low-level modules
 - reusability: easy to reuse the different modules and to combine them
-- low coupling between the different modules
+- low coupling/dependency between the different modules such that they can be used in other codes
 - flexibility: this is mainly achieved by favoring composition over inheritance.
 
 
-The Python language has been selected.
+Because of its fast learning curve, the massive community and available libraries, as well as its less easiness to use, the Python language was selected to code PRL. However, note that most of the libraries used in PRL are coded in C/C++ with Python wrappers such that the code is still running pretty fast.
 
 
+Overview
+--------
+
+Here is a general overview of the framework:
+
 .. figure:: ../UML/pyrobolearn_uml.png
 	:alt: UML diagram of PyRoboLearn
 	:align: center
 
-	UML diagram of PyRoboLearn
+	UML diagram of PyRoboLearn
+
+we now provide a brief overview of each submodule and its intended use:
+
+- ``simulators``: this contains the abstract ``Simulator`` interface from which all the simulators should inherit from.
+  This interface allows to decouple the rest of the code in PRL with the simulator being used. Some simulators might
+  have some features that other simulators don't have, in that case, an error is raised or an approximation is made.
+  For instance, ``PyBullet`` don't provide joint accelerations, but a simulator like ``MuJoCo`` does. As such, it is
+  checked in the ``Robot`` class if the simulator provide these accelerations, if not, it is approximated using finite
+  difference. Currently, only ``Bullet`` is fully-supported. Interfaces for ``MuJoCo``, ``Dart``, and ``Raisim`` are 
+  ongoing.
+
+  - ``middlewares``: this will contain the middleware classes that inherit from the ``Middleware`` class, such as
+    ``ROS`` and others. You will be able to provide it to the simulator and the simulator will use it to publish or
+    receive packages.
+
+- ``robots``: this contains the various robots (manipulators, grippers, legged robots, wheeled robots, flying robots,
+  etc) that can be used in PRL. They all inherit from ``Robot`` which itself inherit from ``Body`` (which is the most
+  abstract class). The ``Body`` has direct access to the simulator interface. Robots have also access to:
+
+  - ``sensors``: this contains the various sensors used by robots. They all inherit from the ``Sensor`` class. They
+    use the simulator interface to get their values.
+  - ``actuators``: this contains the various actuators used by robots. They all inherit from the ``Actuator`` class.
+    They might use the simulator interface to perform action through it.
+
+- ``worlds``: this contains the main ``World`` class which is inherited by all the other worlds. The ``World`` class
+  has direct access to the simulator interface (like ``Body``), and users should interact with it to load the various
+  bodies and robots in the world, or change the physical properties (friction, restitution, etc) of the world. Through
+  that class, you can also attach two bodies together and generate terrains.
+- ``utils``: this contains the various util methods like ``transformations`` (from one orientation representation to
+  another one, and functions that can be applied on quaternions), converters (that convert from one data type to
+  another), ``interpolators``, ``feedback laws``, and others that are used by other parts of the framework.
+
+  - ``data_structures``: this contains data structures such as ordered sets and the different type of queues.
+  - ``plotting``: this contains real-time plotting tools than can plot the joint positions, velocities, accelerations,
+    torques, or link frames in real-time. This is used in combination with the ``Simulator``.
+  - ``parsers``: this contains mainly parsers for some datasets, and robot/world file formats (such as URDF, SDF, Skel, 
+    MJCF, etc).
+
+- ``tools``: this contains the *interfaces* and *bridges*. 
+
+  - *Interfaces* allows to receive or send the data from/to various I/O interfaces (such as mouse, keyboard, 3D space 
+    mouse, game controllers, webcam, depth cameras, sensors like LeapMotion, and others). They all inherit from the 
+    abstract ``Interface`` class which has thread supports. If threads are not used, the user has to call the ``step`` 
+    method such that it reads the next value (i.e. these are not event-driven, i.e. you control when you want to get/set 
+    the data). Interfaces are independent from the other components in the PRL framework (with maybe at the exception 
+    of some ``utils`` methods), and as such can be used in other software. 
+  - *Bridges* makes the connection between an interface and another component in PRL (like a robot or body in the world, 
+    or the world camera). Fundamentally, they accept as input an interface and the component, and the user details what 
+    should be done in that class. This allows to decouple the interface from the application part; e.g. the same game 
+    controller interface could be used to move a wheeled robot or quadcopter robot by providing two bridges (one for 
+    wheeled robots, and one for quadcopter robots). All the bridges inherit from the abstract ``Bridge`` class, and as 
+    with interface a ``step`` method can be called.
+
+- ``states``: this contains the various states which all inherit from the ``State`` abstract class. States can easily
+  be composed together such that you could specify which states you would like to have. For instance, if you want
+  the joint positions, velocities, and the base position and orientation states, you can add them to form one common
+  state. Calling the state will compute their values, and they will save these in the ``data`` attribute. They
+  basically act as useful containers. States are notably provided as inputs to controllers, policies, and rewards among
+  others, and are outputted by the environments.
+- ``actions``: this contains the various actions which all inherit from the ``Action`` abstract class. They are given
+  notably to the policy during the initialization, which sets the action data. Calling an action will perform an action
+  in the simulator (e.g. move the robot joints using position control) or through an interface (e.g. say something
+  through the computer speakers).
+- ``rewards``: this contains the various *rewards* and *costs*. They all inherit from the ``Reward`` abstract class,
+  and various arithmetic operations can be performed on them. They accept as possible arguments the ``State`` and
+  ``Action``. Each time you call them, they check the data contained in the given states and actions and compute
+  the corresponding reward value. This allows the user to reuse different reward functions and easily combine them
+  without worrying how to get or compute the reward value. Note that as for states and actions, rewards can have a
+  particular range which specifies their domain. This is useful if we would like to know if a reward function is
+  strictly positive or not (e.g. the PoWER RL algorithm only accepts strictly positive rewards which it can check by
+  looking at the reward's range).
+- ``envs``: this contains the various environments. They all inherit from the ``Env`` class which accepts as arguments
+  at least the world, the state, and possibly a reward (if we are in the reinforcement learning case). These arguments
+  can be provided at runtime making it easy to (re)use other modules, and render the framework very flexible (see
+  `Composition over inheritance <https://en.wikipedia.org/wiki/Composition_over_inheritance>`_). Few robotic
+  environments are also provided in this class.
+
+  - ``states/generators``: this contains ``state generators`` which generates ``states`` for the environment. You can
+    for instance generate the position / orientation of a body, or its joints. They can be provided to the environment
+    and are called each time you reset the environment.
+  - ``physics``: this contains ``physics randomizers`` which can randomize the physical properties of the joints
+    (e.g. joint damping), links (e.g. mass), and the world (e.g. friction). They can be provided to the environment,
+    and are called each time the environment is reset.
+  - ``terminal_conditions``: this contains terminal conditions which detect if an episode is over or not. They can
+    in addition specify if the environment ended with a success or failure. You can provide them to the environment
+    which check them at each time step.
+
+- ``models``: this contains the various learning models, which have parameters or hyperparameters to optimize given
+  some data. These models can be categorized into two different types: movement primitives and general function
+  approximators. The models are independent from the rest of the framework (except maybe few ``utils`` functions).
+  Some models were implemented from scratch while others were wrapped.
+- ``approximators``: this contains the various approximators which is basically a wrapper around the above models (only
+  the ones that are function approximators and not movement primitives), and accepts as inputs states, actions and
+  general arrays/tensors. They all inherit from the ``Approximator`` class and represents an abstraction above the
+  model classes. Because they can accept states and actions, this makes them dependent on these submodules in PRL.
+  Approximators are notably used to model policies (which maps states to actions), value function approximators (which
+  maps states to a scalar, or states and actions to a scalar, or states to a scalar for each discrete action), and
+  dynamic transition functions (which maps states and actions to the next states). These are described next.
+- ``policies``: this contains the various policies that can be used in PRL. They all inherit from the ``Policy`` class,
+  and use internally approximators, or models (if movement primitives). They can operate at different rates, and are
+  provided with a state and action instance at the initialization.
+- ``values``: this contains the various value function approximators that can be used in PRL. They accepts as inputs
+  the states and possibly the actions (if Q-value function approximator). They are mostly used by reinforcement
+  learning algorithms).
+- ``dynamics``: this contains the various dynamic function approximators. This is mostly used by model-based 
+  reinforcement learning algorithms. This is currently not fully-implemented/operational.
+
+- ``tasks``: this contains the various learning tasks/paradigms. They all inherit from the ``Task`` class and accepts 
+  at least as inputs the policy(ies) and environment. They act as a container for these two's, and calling the 
+  ``step`` method will perform one full cycle in the agent-environment interaction loop. Subsequently, you can also 
+  call ``run`` to run several loop for the specified number of steps. Tasks can notably be provided to algorithms 
+  (especially RL algorithms).
+
+- ``distribution``: this contains few distributions that are used by exloration strategies (see next bullet point).
+- ``exploration``: this contains the various exploration strategies that can be used by the policy; parameter and
+  action exploration. They all inherit from the ``Exploration`` class and accepts as inputs the policy that they wrap
+  around.
+- ``storages``: this contains the various data storages/containers (such as experience replay storage and batches)
+  that are used during the learning process.
+- ``losses``: this contains the various losses that are used by the various algorithms. As for the rewards, you can
+  perform arithmetic operations on them and combine them in different ways.
+- ``optimizers``: this contains the various optimizers that can be used. We provide a common interface and wrap popular
+  optimizers. Currently, some optimizers are not fully-operational.
+- ``returns``: this provides the various returns and estimators that are used in RL.
+- ``algos``: this contains the various learning algorithms on how to acquire the data and train the various models
+  (policies, values, dynamics, etc).
+- ``metrics``: this contains the various metrics that are used in different learning paradigms (imitation, reinforcement, 
+  transfer, etc). They are not currently all implemented. You can combine different metrics together and plot them by 
+  just calling the ``plot`` method.
+
+Other folders include:
+
+- ``filters``: this contains various filters (KF, EKF, UKF, HF, etc).

docs/source/readme.rst

@@ -1,15 +1,2 @@
 
 .. include:: ../../README.rst
-
-Citation
-========
-
-.. code-block:: latex
-
-   @misc{delhaisse2019pyrobolearn,
-    author = {Delhaisse, Brian and Xin, Songyan and Rozo, Leonel, and Caldwell, Darwin},
-    title = {PyRoboLearn: A Python Framework for Robot Learning Practitioners},
-    howpublished = {\url{https://github.com/robotlearn/pyrobolearn}},
-    year=2019,
-   }
-

docs/source/robots.rst

@@ -236,6 +236,8 @@ To illustrate how to create your own robot, let's assume you want to create a hu
 Sensors and Actuators
 ---------------------
 
+Both sensors and actuators are attached to joints or links, and interact with the simulator interface. They notably both accept a ``noise`` distribution, the number of ``ticks`` (i.e. the number of steps to wait/sleep before the acquisition of the next sensor value), the ``latency`` (currently fixed).
+
 * Sensors
 * Actuators
 

docs/source/simulators.rst

@@ -1,11 +1,11 @@
 Simulators
 ==========
 
-The simulator is the starting point in the **PyRoboLearn** (PRL) framework. To avoid a tight coupling with a particular simulator, a ``Simulator`` interface class (from which all the other simulators inherit from) has been implemented. Other .
+The simulator is the starting point in the **PyRoboLearn** (PRL) framework. To avoid a tight coupling with a particular simulator, a ``Simulator`` interface class (from which all the other simulators inherit from) has been implemented. The ``Simulator`` interface notably provides a common interface such that it provides features in the same way without regard to the simulator. For instance, a quaternion might be returned as [x,y,z,w] with one simulator, while with another it could be returned as [w,x,y,z]. This is addressed by the ``Simulator`` interfaces which uses only one convention, and convert if necessary the quaternion returned by the inner simulator. 
 
-We provide the ``Bullet`` interface, which uses the ``pybullet`` library.
+Currently, the fully supported simulator is ``pybullet`` through the ``Bullet`` interface. Other simulators such as ``MuJoCo``, ``Dart`` and ``Raisim`` are partially implemented but their full integration is still in progress. 
 
-The general idea is that you would be able to change the simulator if you wish without having to modify any other lines of code. See example below.
+The general idea is that you would be able to change the simulator if you wish without having to modify any other lines of code. See example below. 
 
 
 Design
@@ -86,12 +86,14 @@ FAQs and Troubleshootings
 Future works
 ------------
 
-My main objectives for future works are the implementation of:
-
+Currently, I am working on supporting the following simulators:
 - the Mujoco interface; I originally did not start with it as it is closed-source and requires a License. However, it is used a lot in research and thus it could be interesting to have it as well.
+- the DART interface; there is a minimal implementation of it in PRL where I was mostly playing around with it.
+- the RaiSim interface; the ``raisimpy`` python wrapper has been implemented, and a minimal implementation is provided.
+
+My main objectives for future works are the implementation of:
 - the Gazebo-ROS interface; a part has already been implemented but it is far from over.
 - the Isaac interface if Nvidia provided a nice Python API.
 
 Possible other future works might include the implementation of:
-- the DART interface; there is a minimal implementation of it when I was playing around with it
 - the opensim interface; this interface is for musculoskeletal models but this can be interesting when testing algorithms/models.

examples/README.rst

@@ -11,11 +11,14 @@ You can check the following folders on:
 - ``simulators``: how to use a particular simulator. Currently, the Bullet simulator is the one fully operational.
 - ``worlds``: how to create a world in the simulator, load various objects inside and interact with them, use the camera, and load or generate terrains.
 - ``robots``: how to load a specific robot (biped, quadruped, wheeled, etc) into the world.
+- ``plotting``: how to use real-time plotting tools (to plot the joint values or the link frames).
 - ``interfaces``: the various interfaces (game controllers, webcam, etc) and bridges that you can use.
 - ``kinematics``: how to use forward and inverse kinematics as well as position and velocity control.
 - ``dynamics``: how to use forward and inverse dynamics as well as force control.
 - ``manipulability``: how to use the velocity and dynamic manipulability ellipsoids.
-- ``states``: how to query the states / observations.
 - ``models``: the different learning models that you can use.
+- ``states``: how to query the states / observations.
+- ``rewards``: how to use the reward functions.
+- ``environments``: provide a full example on how to create an environment from scratch in PRL.
 - ``imitation``: how to use imitation learning with the framework.
-- ``gym/cartpole``: policies that are trained with different algorithms on the gym Cartpole environment.
+- ``reinforcement``: how to use reinforcement learning with the framework.

examples/dynamics/control/attractor_point.py

@@ -22,6 +22,7 @@ robot = prl.robots.RRBot(sim)
 robot.disable_motor()           # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
 robot.print_info()
 robot.change_transparency()
+world.load_robot(robot)
 
 # define variables
 link_id = robot.get_link_ids('hokuyo_link')         # the link we are interested to

examples/dynamics/control/gravity_compensation.py

@@ -22,6 +22,7 @@ robot = prl.robots.RRBot(sim)
 robot.disable_motor()       # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
 robot.print_info()
 robot.change_transparency()
+world.load_robot(robot)
 
 
 # run simulator

examples/dynamics/control/no_forces.py

@@ -22,6 +22,7 @@ robot = prl.robots.RRBot(sim)
 robot.disable_motor()       # disable motors
 robot.print_info()
 robot.change_transparency()
+world.load_robot(robot)
 
 
 # run simulator

examples/environments/README.rst

@@ -84,8 +84,7 @@ Examples
 
 Here are few examples that you can find in this folder that better demonstrate how to use the environment:
 
-1. ``basics.py``: show the flexibility of how to build an environment and use it.
-2. ``manipulator.py``: show how to define an environment where the goal is to reach a target object using a manipulator.
+1. ``inverted_pendulum.py``: create the inverted pendulum environment from scratch in PRL. This example combines the various concepts that we have seen until now (simulator, world, robot, state, action, reward, physics randomizer, initial state generator, etc). At the end, the environment is launched in a similar way as in OpenAI Gym.
 
 References:
 

examples/environments/inverted_pendulum.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+"""In this file, we create from scratch the inverted pendulum swing-up environment defined in OpenAI Gym.
+
+This is based on the control problem proposed in OpenAI Gym [1]:
+"The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the problem,
+the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
+
+References:
+    - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+
+
+# create simulator
+sim = prl.simulators.Bullet()
+
+
+# create basic world with the pendulum
+world = prl.worlds.BasicWorld(sim)
+robot = world.load_robot('pendulum')
+robot.disable_motor()  # such that it swings freely
+robot.print_info()
+
+
+# create state: [cos(q_1), sin(q_1), \dot{q}_1]
+trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+velocity_state = prl.states.JointVelocityState(robot=robot)
+state = trig_position_state + velocity_state
+print("\nObservation: {}".format(state))
+
+
+# create action: \tau_1
+action = prl.actions.JointTorqueAction(robot, bounds=(-2., 2.))
+print("\nAction: {}".format(action))
+
+
+# create reward/cost: ||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2
+position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
+                                              target_state=np.zeros(len(robot.joints)),
+                                              update_state=True)
+velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
+torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot), update_state=True)
+reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+print("Reward: {}".format(reward))
+
+
+# create initial state generator: generate the state each time we reset the environment
+def reset_robot(robot):  # function to disable the motors every time we reset the joint state
+    def reset():
+        robot.disable_motor()
+    return reset
+
+
+init_state = prl.states.JointPositionState(robot)
+low, high = np.array([-np.pi] * len(robot.joints)), np.array([np.pi] * len(robot.joints))
+initial_state_generator = prl.states.generators.UniformStateGenerator(state=init_state, low=low, high=high,
+                                                                      fct=reset_robot(robot))
+
+
+# create physics randomizer: randomize the mass each time we reset the environment
+masses = robot.get_link_masses(link_ids=robot.joints)
+masses = (masses - masses/10., masses + masses/10.)
+physics_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
+
+
+# create the environment using composition
+env = prl.envs.Env(world=world, states=state, rewards=reward, actions=action,
+                   initial_state_generators=initial_state_generator, physics_randomizers=physics_randomizer,
+                   terminal_conditions=None)
+
+
+# run simulation
+env.reset()
+for t in prl.count():
+
+    if (t % 800) == 0:  # reset to see what initial_state_generator and physics randomizer do
+        env.reset()
+        print("New link mass: {}".format(robot.get_link_masses(link_ids=robot.joints)))
+
+    states, rewards, done, info = env.step(sleep_dt=1./240)
+    print("Reward: {}".format(rewards))

examples/imitation/trajectory_reproduction_kuka_dmp.py

@@ -8,7 +8,7 @@ import numpy as np
 import matplotlib.pyplot as plt
 
 # Import robots and world
-from pyrobolearn.simulators import BulletSim
+from pyrobolearn.simulators import Bullet
 from pyrobolearn.worlds import BasicWorld
 from pyrobolearn.robots import KukaIIWA
 
@@ -29,7 +29,7 @@ num_basis = 20
 rate = 30
 
 # Create simulator
-sim = BulletSim()
+sim = Bullet()
 
 # create world
 world = BasicWorld(sim)

examples/kinematics/inverse/moving_sphere.py

@@ -19,6 +19,7 @@ world = BasicWorld(sim)
 # create robot
 robot = KukaIIWA(sim)
 robot.print_info()
+world.load_robot(robot)
 
 # define useful variables for IK
 dt = 1./240
@@ -61,7 +62,7 @@ for t in count():
     else:
         J = robot.get_linear_jacobian(link_id, q=q)[:, qIdx]
 
-    # Pseudo-inverse
+    # Pseudo-inverse: \hat{J} = J^T (JJ^T + k^2 I)^{-1}
     Jp = robot.get_damped_least_squares_inverse(J, damping)
 
     # evaluate damped-least-squares IK

examples/models/gmm.py

@@ -0,0 +1,66 @@
+#!/usr/bin/env python
+"""Provide some examples using GMMs.
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+from sklearn.mixture import GaussianMixture
+
+from pyrobolearn.models.gmm import Gaussian, GMM, plot_gmm, plot_gmm_sklearn
+
+
+# create manually a GMM
+dim, num_components = 2, 5
+gmm = GMM(gaussians=[Gaussian(mean=np.random.uniform(-1., 1., size=dim),
+                              covariance=0.1*np.identity(dim)) for _ in range(num_components)])
+gmm_sklearn = GaussianMixture(n_components=num_components)
+
+
+# plot initial GMM
+plot_gmm(gmm, title='Initial GMM')
+plt.show()
+
+
+# create data: Generate random sample following a sine curve
+# Ref: https://scikit-learn.org/stable/auto_examples/mixture/plot_gmm_sin.html#sphx-glr-auto-examples-mixture-\
+# plot-gmm-sin-py
+n_samples = 100
+np.random.seed(0)
+X = np.zeros((n_samples, 2))
+step = 4. * np.pi / n_samples
+
+for i in range(X.shape[0]):
+    x = i * step - 6.
+    X[i, 0] = x + np.random.normal(0, 0.1)
+    X[i, 1] = 3. * (np.sin(x) + np.random.normal(0, .2))
+
+xlim, ylim = [-8, 8], [-8, 8]
+
+# plot data
+plt.title('Training data')
+plt.scatter(X[:, 0], X[:, 1])
+plt.show()
+
+
+# init GMM
+init_method = 'k-means'  # 'random', 'k-means', 'uniform', 'sklearn', 'curvature'
+gmm.init(X, method=init_method)
+fig, ax = plt.subplots(1, 1)
+plot_gmm(gmm, X=X, ax=ax, title='GMM after ' + init_method.capitalize(), xlim=xlim, ylim=ylim)
+plt.show()
+
+
+# fit a GMM using EM
+result = gmm.fit(X, init=None)
+gmm_sklearn.fit(X)
+
+# plot EM optimization
+plt.plot(result['losses'])
+plt.title('EM per iteration')
+plt.show()
+
+# plot trained GMM
+fig, ax = plt.subplots(1, 2)
+plot_gmm(gmm, X=X, label=True, ax=ax[0], title='Our Trained GMM', option=1, xlim=xlim, ylim=ylim)
+plot_gmm_sklearn(gmm_sklearn, X, label=True, ax=ax[1], title="Sklearn's Trained GMM", xlim=xlim, ylim=ylim)
+plt.show()

examples/models/promp.py

@@ -0,0 +1,56 @@
+#!/usr/bin/env python
+"""Provide some examples using ProMPs.
+"""
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+from pyrobolearn.models.promp.promp import DiscreteProMP, plot_state, plot_proba_state, plot_weighted_basis
+
+
+# create data and plot it
+N = 8
+t = np.linspace(0., 1., 100)
+# eps = 0.1
+# y = np.array([np.sin(2*np.pi*t) + eps * np.random.rand(len(t)) for _ in range(N)])      # shape: NxT
+# dy = np.array([2*np.pi*np.cos(2*np.pi*t) + eps * np.random.rand(len(t)) for _ in range(N)])     # shape: NxT
+phi = np.random.uniform(low=-1., high=1., size=N)
+y = np.array([np.sin(2 * np.pi * t + phi[i]) for i in range(int(N/2))])  # shape: NxT
+y1 = np.array([np.cos(2 * np.pi * t + phi[i]) for i in range(int(N/2))])
+y = np.vstack((y, y1))
+dy = np.array([2 * np.pi * np.cos(2 * np.pi * t + phi[i]) for i in range(int(N/2))])  # shape: NxT
+dy1 = np.array([2 * np.pi * np.sin(2 * np.pi * t + phi[i]) for i in range(int(N/2))])
+dy = np.vstack((dy, dy1))
+Y = np.dstack((y, dy))  # N,T,2D  --> why not N,2D,T
+plot_state(Y, title='Training data')
+plt.show()
+
+# create discrete and rhythmic ProMP
+promp = DiscreteProMP(num_dofs=1, num_basis=10, basis_width=1./20)
+
+# plot the basis function activations
+plt.plot(promp.Phi(t)[:, :, 0].T)
+plt.title('basis functions')
+plt.show()
+
+# plot ProMPs
+y_pred = promp.rollout()
+fig, ax = plt.subplots(1, 2)
+plot_state(y_pred[None], ax=ax, title='ProMP prediction before learning', linewidth=2.)    # shape: N,T,2D
+plot_weighted_basis(t, promp, ax=ax)
+plt.show()
+
+# learn from demonstrations
+promp.imitate(Y)
+y_pred = promp.rollout()
+fig, ax = plt.subplots(1, 2)
+plot_state(y_pred[None], ax=ax, title='ProMP prediction after learning', linewidth=3.)   # N,T,2D
+plot_weighted_basis(t, promp, ax=ax)
+plt.show()
+
+method = 'marginal'
+means, covariances = promp.rollout_proba(method=method, return_gaussian=False)
+fig, ax = plt.subplots(1, 2)
+# plot_state(Y, ax=ax, title='Training data')
+plot_proba_state(means, covariances, ax=ax, title='ProMP prediction after learning', linewidth=3.)
+plt.show()

examples/plotting/README.rst

@@ -0,0 +1,14 @@
+Plotting examples
+=================
+
+In this folder, you will find examples where we use the plotting tools provided in PRL.
+
+Warnings: Currently, you have to close the figure before closing the simulator. If you close the simulator first,
+you might still have the process responsible to draw the figure running.
+
+- ``joints.py``: plot in real-time the joint positions (in blue), velocities (in green), accelerations (in red),
+  and/or torques (in purple).
+- ``link_frames.py``: plot in real-time the frames of the specified links.
+
+To test these classes, you can run the corresponding python file, and move the manipulator with the mouse and check the
+real-time plots.

examples/plotting/joints.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python
+"""Provide the joint real-time plotting example.
+
+Try to move the Kuka manipulator with your mouse, and check the joint values. Note that it can take few seconds to
+load the plot.
+
+Warnings: don't forget to close FIRST the figure, THEN the simulator otherwise you will have the plotting process still
+running.
+"""
+
+from itertools import count
+import pyrobolearn as prl
+
+
+# create the simulator
+sim = prl.simulators.Bullet()
+
+# create the world
+world = prl.worlds.BasicWorld(sim)
+
+# load the robot
+robot = world.load_robot('kuka_iiwa')
+
+# create the joint real-time plotting tool
+plot = prl.utils.plotting.JointRealTimePlot(robot, joint_ids=None, position=True, velocity=False,
+                                            acceleration=False, torque=False, ticks=24)
+
+# run the simulation
+for t in count():
+    # update the plot
+    plot.update()
+
+    # perform a step in the world
+    world.step(sim.dt)

examples/plotting/link_frames.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+"""Provide the link frame real-time plotting example.
+
+Try to move the Kuka manipulator and / or the box with your mouse. Note that it can take few seconds to load the plot.
+
+Warnings: don't forget to close FIRST the figure, THEN the simulator otherwise you will have the plotting process still
+running.
+"""
+
+import pyrobolearn as prl
+
+
+# create the simulator
+sim = prl.simulators.Bullet()
+
+# create the world
+world = prl.worlds.BasicWorld(sim)
+
+# load the robot and a box
+robot = world.load_robot('kuka_iiwa')
+box = world.load_box([0.7, 0., 0.2], dimensions=(0.2, 0.2, 0.2), color=(0.2, 0.2, 0.8, 1.), return_body=True)
+
+# create the link frame real-time plotting tool
+plot = prl.utils.plotting.LinkFrameRealTimePlot(bodies=[robot, box], link_ids=None, ticks=24)
+
+# run the simulation
+for t in prl.count():
+    # update the plot
+    plot.update()
+
+    # perform a step in the world
+    world.step(sim.dt)

examples/reinforcement/README.md

@@ -0,0 +1,9 @@
+## Reinforcement learning task
+
+In this folder, you can run reinforcement learning tasks.
+
+- In the `gym` subfolder, you can run `gym` environments using the models and algorithms available from the PRL 
+  frameworks.
+- In the `baselines` subfolder, you can `PRL` environments using the neural networks models and algorithms provided by 
+  the `stable_baselines` library.
+- Other example files provide PRL environments along with models and algorithms provided by PRL.

examples/reinforcement/baselines/README.rst

@@ -0,0 +1,14 @@
+Baselines
+---------
+
+This folder contains examples when using PRL environments and algorithms defined in the ``stable_baselines`` Python
+library.
+
+Few notes with respect to that:
+
+1. ``stable_baselines`` uses the ``TensorFlow`` backend, and a ``DummyVecEnv`` has to be provided to the algorithms.
+2. Normally, in PRL, the actions can be defined outside the environments and it is the policy that is responsible to
+   apply the action in the world. However, in ``OpenAI gym``, it is the environment that has the ``action_space`` and
+   apply the ``action``. To accommodate with that, the action can also be defined and provided to the PRL environment.
+3. When using PRL with ``stable_baselines``, make sure that each states have the same dimensions; i.e. we can not
+   return a 1D vector state with a 2D matrix state at the same time (at least, not currently).

examples/reinforcement/baselines/acrobot_gym.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Example on how to use the 'Acrobot' OpenAI Gym environments in PRL using the `stable_baselines` library.
+"""
+
+from stable_baselines.common.policies import MlpPolicy
+from stable_baselines.common.vec_env import DummyVecEnv
+from stable_baselines import PPO2
+
+from pyrobolearn.envs import gym  # this is a thin wrapper around the gym library
+
+# create env, state, and action from gym
+env = gym.make('Acrobot-v1')
+state, action = env.state, env.action
+print("State and action space: {} and {}".format(state.space, action.space))
+
+# The algorithms require a vectorized environment to run
+env = DummyVecEnv([lambda: env])
+
+model = PPO2(MlpPolicy, env, verbose=1)
+model.learn(total_timesteps=10000)
+
+obs = env.reset()
+for i in range(1000):
+    action, _states = model.predict(obs)
+    obs, rewards, dones, info = env.step(action)
+    env.render()

examples/reinforcement/baselines/acrobot_prl.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+"""Example on how to use the PRL 'Acrobot' environment using the `stable_baselines` library.
+"""
+
+from stable_baselines.common.policies import MlpPolicy
+from stable_baselines.common.vec_env import DummyVecEnv
+from stable_baselines import PPO2
+
+import gym
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.acrobot import AcrobotEnv
+
+# create env, state, and action from gym
+sim = prl.simulators.Bullet(render=True)
+env = AcrobotEnv(sim)
+print("State and action space: {} and {}".format(env.state.space, env.action.space))
+print("State and action merged space: {} and {}".format(env.state.merged_space, env.action.merged_space))
+
+# The algorithms require a vectorized environment to run
+env = DummyVecEnv([lambda: env])
+
+model = PPO2(MlpPolicy, env, verbose=1)
+model.learn(total_timesteps=10000)
+
+obs = env.reset()
+# env.render()
+for i in range(1000):
+    action, _states = model.predict(obs)
+    obs, rewards, dones, info = env.step(action)
+    # env.render()

examples/reinforcement/baselines/cartpole_gym.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Example on how to use the 'Cartpole' OpenAI Gym environments in PRL using the `stable_baselines` library.
+"""
+
+from stable_baselines.common.policies import MlpPolicy
+from stable_baselines.common.vec_env import DummyVecEnv
+from stable_baselines import PPO2
+
+from pyrobolearn.envs import gym  # this is a thin wrapper around the gym library
+
+# create env, state, and action from gym
+env = gym.make('CartPole-v1')
+state, action = env.state, env.action
+print("State and action space: {} and {}".format(state.space, action.space))
+
+# The algorithms require a vectorized environment to run
+env = DummyVecEnv([lambda: env])
+
+model = PPO2(MlpPolicy, env, verbose=1)
+model.learn(total_timesteps=10000)
+
+obs = env.reset()
+for i in range(1000):
+    action, _states = model.predict(obs)
+    obs, rewards, dones, info = env.step(action)
+    env.render()

examples/reinforcement/baselines/pendulum_gym.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Example on how to use the 'Pendulum' OpenAI Gym environments in PRL using the `stable_baselines` library.
+"""
+
+from stable_baselines.common.policies import MlpPolicy
+from stable_baselines.common.vec_env import DummyVecEnv
+from stable_baselines import PPO2
+
+from pyrobolearn.envs import gym  # this is a thin wrapper around the gym library
+
+# create env, state, and action from gym
+env = gym.make('Pendulum-v0')
+state, action = env.state, env.action
+print("State and action space: {} and {}".format(state.space, action.space))
+
+# The algorithms require a vectorized environment to run
+env = DummyVecEnv([lambda: env])
+
+model = PPO2(MlpPolicy, env, verbose=1)
+model.learn(total_timesteps=10000)
+
+obs = env.reset()
+for i in range(1000):
+    action, _states = model.predict(obs)
+    obs, rewards, dones, info = env.step(action)
+    env.render()

examples/reinforcement/baselines/pendulum_prl.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+"""Example on how to use the PRL 'Acrobot' environment using the `stable_baselines` library.
+"""
+
+from stable_baselines.common.policies import MlpPolicy
+from stable_baselines.common.vec_env import DummyVecEnv
+from stable_baselines import PPO2
+
+import gym
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.pendulum import InvertedPendulumSwingUpEnv
+
+# create env, state, and action from gym
+sim = prl.simulators.Bullet(render=True)
+env = InvertedPendulumSwingUpEnv(sim)
+print("State and action space: {} and {}".format(env.state.space, env.action.space))
+print("State and action merged space: {} and {}".format(env.state.merged_space, env.action.merged_space))
+
+# The algorithms require a vectorized environment to run
+env = DummyVecEnv([lambda: env])
+
+model = PPO2(MlpPolicy, env, verbose=1)
+model.learn(total_timesteps=10000)
+
+obs = env.reset()
+# env.render()
+for i in range(1000):
+    action, _states = model.predict(obs)
+    obs, rewards, dones, info = env.step(action)
+    # env.render()

examples/robots/README.md

@@ -1,22 +0,0 @@
-## Robot examples
-
-More than 60 robots (of various types) are available through `pyrobolearn`.
-
-Here are the few examples that you can find in this folder:
-1. `load_robot.py <robot_name>`: load the given robot in the world.
-2. `visualize_robot.py <robot_name>`: test different visualization tools that can be used on the robot to show its 
-joint axis, bounding boxes, and others.
-3. `robot_with_sliders.py <robot_name>`: load the given robot in the world and allow you to manipulate the robot's 
-joints with sliders.
-4. `distribute_epucks.py`: distribute several e-pucks in the world and make them move forward.
-5. `quadcopter_controller.py`: move a quadcopter in the air using an Xbox or Playstation game controller.
-6. `robots/<robot>.py`: load the given robot in the simulator by directly instantiating it. Some of these files do 
-more than just loading the robot.
-
-Notes: to turn the camera in the simulator, keep pressing the `ctrl` key and the left button on the mouse, and 
-move this last one.
-
-
-#### What to check next?
-
-Check the `pyrobolearn/examples/interfaces` or `pyrobolearn/examples/kinematics` folder.

examples/robots/README.rst

@@ -0,0 +1,28 @@
+Robot examples
+==============
+
+More than 60 robots (of various types) are available through ``pyrobolearn``.
+
+Here are the few examples that you can find in this folder:
+
+1. ``load_robot.py <robot_name>``: load the given robot in the world.
+2. ``visualize_robot.py <robot_name>``: test different visualization tools that can be used on the robot to show its
+   joint axis, bounding boxes, and others.
+3. ``robot_with_sliders.py <robot_name>``: load the given robot in the world and allow you to manipulate the robot's
+   joints with sliders.
+4. ``distribute_epucks.py``: distribute several e-pucks in the world and make them move forward.
+5. ``quadcopter_controller.py``: move a quadcopter in the air using an Xbox or Playstation game controller.
+6. ``robots/<robot>.py``: load the given robot in the simulator by directly instantiating it. Some of these files do
+   more than just loading the robot.
+7. ``attach_gripper_to_manipulator``: attach the specified gripper / hand to the Kuka manipulator robot. In this file,
+   you can check the various grippers you can use.
+8. ``attach_manipulator_to_quadruped``: attach the Kuka Youbot manipulator to the HyQ2Max quadruped robot.
+
+Notes: to turn the camera in the simulator, keep pressing the ``ctrl`` key and the left button on the mouse, and
+move the mouse.
+
+
+What to check next?
+~~~~~~~~~~~~~~~~~~~
+
+Check the ``pyrobolearn/examples/interfaces`` or ``pyrobolearn/examples/kinematics`` folder.

examples/robots/attach_gripper_to_manipulator.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+"""Attach a gripper/hand to the Kuka manipulator.
+
+In this file, you can attach different grippers / hands to the kuka robot. You can move the robot with the mouse.
+"""
+
+import argparse
+
+import pyrobolearn as prl
+
+
+# create parser to select the gripper/hand
+parser = argparse.ArgumentParser()
+parser.add_argument('-g', '--gripper', help='the gripper/hand to attach to the kuka robot', type=str,
+                    choices=['softhand', 'allegrohand', 'wam_gripper', 'youbot_gripper', 'pr2_gripper', 'jaco_gripper',
+                             'fetch_gripper', 'franka_gripper', 'baxter_gripper', 'schunk_hand', 'shadowhand'],
+                    default='softhand')
+args = parser.parse_args()
+
+
+# create simulator
+sim = prl.simulators.Bullet()
+
+# create basic world with floor and gravity
+world = prl.worlds.BasicWorld(sim)
+
+# load kuka robot
+robot = world.load_robot('kuka_iiwa')
+
+# load hand/gripper
+hand = world.load_robot(args.gripper, position=(0., 0., 1.5), fixed_base=False)
+
+# compute parent frame position (this will be removed later and integrated in PRL)
+parent_frame_position = [0., 0., 0.]
+if args.gripper == 'shadowhand':
+    parent_frame_position = [0., 0., 0.1]
+elif args.gripper == 'allegrohand':
+    parent_frame_position = [0., 0., 0.06]
+elif args.gripper == 'wam_gripper':
+    parent_frame_position = [0., 0., 0.01]
+elif args.gripper == 'youbot_gripper':
+    parent_frame_position = [0., 0., 0.03]
+elif args.gripper == 'pr2_gripper':
+    parent_frame_position = [0., 0., 0.002]
+elif args.gripper == 'jaco_gripper':
+    parent_frame_position = [0., 0., 0.06]
+elif args.gripper == 'fetch_gripper':
+    parent_frame_position = [0., 0., 0.07]
+elif args.gripper == 'franka_gripper':
+    parent_frame_position = [0., 0., 0.02]
+elif args.gripper == 'baxter_gripper':
+    parent_frame_position = [0., 0., -0.03]
+elif args.gripper == 'schunk_hand':
+    parent_frame_position = [0., 0., 0.002]
+
+# attach hand/gripper to robot
+world.attach(body1=robot, body2=hand, link1=robot.end_effectors[0], link2=-1, joint_axis=[0., 0., 0.],
+             parent_frame_position=parent_frame_position, child_frame_position=[0., 0., 0.])
+
+# set the hand joint positions
+hand.set_joint_positions([0.] * hand.num_actuated_joints)
+
+# run simulation
+for t in prl.count():
+    sim.step(sim.dt)

examples/robots/attach_manipulator_to_quadruped.py

@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+"""Attach the Kuka Youbot manipulator to the HyQ2Max quadruped.
+"""
+
+import pyrobolearn as prl
+
+
+# create simulator
+sim = prl.simulators.Bullet()
+
+# create world
+world = prl.worlds.BasicWorld(sim)
+
+# create quadruped and manipulator
+quadruped = world.load_robot('hyq2max')
+manipulator = world.load_robot('kuka_youbot_arm', position=(0., 0., 1.), fixed_base=False)
+
+# attach manipulator to the back of the robot
+world.attach(body1=quadruped, body2=manipulator, link1=-1, link2=-1, joint_axis=[0., 0., 0.],
+             parent_frame_position=[0.25, 0., 0.2], child_frame_position=[0., 0., 0.])
+
+# run simulation
+for t in prl.count():
+    sim.step(sim.dt)

examples/robots/robots/cassie.py

@@ -25,5 +25,5 @@ robot.print_info()
 # run simulator
 for _ in count():
     # robot.update_joint_slider()
-    robot.move_joint_home_positions()
+    robot.move_home_joint_positions()
     world.step(sleep_dt=1./240)

examples/robots/robots/laikago.py

@@ -25,5 +25,5 @@ robot.print_info()
 # run simulator
 for _ in count():
     # robot.update_joint_slider()
-    robot.move_joint_home_positions()
+    robot.move_home_joint_positions()
     world.step(sleep_dt=1./240)

examples/simulators/README.md

@@ -22,6 +22,9 @@ joint position values that were returned by the Bullet simulator on the correspo
 values from the ROS topics and change them in the simulator. This works with the `bullet_ros_publisher.py` code 
 presented above. By moving the robot with your mouse in the publisher version, you will see the robot in this 
 subscriber version moves in accordance with. This can be useful if you have access to the real platform as well.
+4. `simulators.py`: example which loads few primitive shapes and the ANYmal robot using a simulator among `Bullet`, 
+`Mujoco`, `Raisim`, and `Dart`. Only the line `sim = <SimulatorName>(render=True)` needs to be changed. Note that 
+the integration of these other simulators is ongoing.
 
 Later, a `ROS`/`ROS_RBDL` "simulator" (without passing by a real simulator like `Bullet`) will allow you to make 
 your code works on a real platform using ROS without changing any other lines of code. This is one of the big 

examples/simulators/bullet_ros_publisher.py

@@ -47,14 +47,13 @@ import pyrobolearn as prl
 
 
 # create simulator (ros core will automatically be launched if it has not already been launched)
-sim = prl.simulators.BulletROS(subscribe=False, publish=True, teleoperate=True)
+sim = prl.simulators.BulletROS(publish=True, teleoperate=True)
 
 # load world
 world = prl.worlds.BasicWorld(sim)
 
-# load rrbot
-robot = prl.robots.RRBot(sim)
-
+# load robot
+robot = world.load_robot('wam')
 
 # run simulation
 for t in count():
@@ -64,3 +63,35 @@ for t in count():
 
     # perform a step in the simulator (and sleep for `sim.dt`)
     world.step(sim.dt)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

examples/simulators/bullet_ros_subscriber.py

@@ -15,14 +15,13 @@ import pyrobolearn as prl
 
 
 # create simulator (ros core will automatically be launched if it has not already been launched)
-sim = prl.simulators.BulletROS(subscribe=True, publish=False)
+sim = prl.simulators.BulletROS(subscribe=True)
 
 # load world
 world = prl.worlds.BasicWorld(sim)
 
-# load rrbot
-robot = prl.robots.RRBot(sim)
-
+# load robot
+robot = world.load_robot('wam')
 
 # run simulation
 for t in count():
@@ -31,3 +30,39 @@ for t in count():
 
     # perform a step in the world, and sleep for `sim.dt`
     world.step(sim.dt)
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

examples/simulators/simulators.py

@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+"""Simulator tests.
+
+Example on how to load different things with the simulators. This example is still in an experimental phase. For
+now, only Bullet is fully-supported. We are working on the other ones, especially the Mujoco simulator.
+- Bullet: OK
+- Raisim: OK  (todo: for collision bodies, it only accepts OBJ files)
+- MuJoCo: OK  (todo: control still missing)
+- DART: OK, but capsules don't have collision shapes... (todo: fix some URDFs)
+- VREP: Not implemented yet + problem when importing PyRep with pybullet. Also, need to figure out how to call the
+  'loadURDF' plugin.
+- Isaac: not available yet.
+"""
+
+import os
+from itertools import count
+
+from pyrobolearn.simulators.bullet import Bullet
+from pyrobolearn.simulators.raisim import Raisim
+from pyrobolearn.simulators.dart import Dart
+from pyrobolearn.simulators.mujoco import Mujoco
+# from pyrobolearn.simulators.vrep import VREP  # Problem when importing PyRep with Pybullet
+# from pyrobolearn.simulators.isaac import Isaac  # Not available yet
+
+
+sim = Bullet(render=True)
+# sim = Raisim(render=True)
+# sim = Dart(render=True)
+# sim = Mujoco(render=True)
+# sim = VREP(render=True)
+# sim = Isaac(render=True)
+print("Gravity: {}".format(sim.get_gravity()))
+
+# load floor
+floor = sim.load_floor(dimension=20)
+
+# create box
+box = sim.create_primitive_object(sim.GEOM_BOX, position=(0, 0, 2), mass=1, rgba_color=(1, 0, 0, 1))
+sphere = sim.create_primitive_object(sim.GEOM_SPHERE, position=(2, 2, 2), mass=1, rgba_color=(0, 1, 0, 1))
+cylinder = sim.create_primitive_object(sim.GEOM_CYLINDER, position=(0, 2, 2), mass=1)
+capsule = sim.create_primitive_object(sim.GEOM_CAPSULE, position=(0, -2, 2), mass=1, rgba_color=(0, 0, 1, 1),
+                                      radius=0.5, height=0.5)
+
+# load robot
+urdf_path = os.path.dirname(os.path.abspath(__file__)) + '/../../pyrobolearn/robots/urdfs/'
+# path = urdf_path + 'rrbot/rrbot.urdf'
+# path = urdf_path + 'jaco/jaco.urdf'
+# path = urdf_path + 'kuka/kuka_iiwa/iiwa14.urdf'
+# path = urdf_path + 'hyq2max/hyq2max.urdf'
+path = urdf_path + 'anymal/anymal.urdf'
+# path = urdf_path + 'centauro/centauro_stick.urdf'
+
+robot = sim.load_urdf(path, position=(3, -3, 2), use_fixed_base=False)
+
+# perform step
+for t in count():
+    sim.step(sleep_time=sim.dt)

pyrobolearn/README.md

@@ -1,4 +0,0 @@
-## PyRoboLearn
-
-In each folder, you will find a readme file describing what the code in the folder is for.
-

pyrobolearn/README.rst

@@ -0,0 +1,135 @@
+PyRoboLearn (PRL)
+=================
+
+In each folder, you will find a ``README`` file describing the purpose of the corresponding submodule.
+
+Here we provide a brief overview of each submodule and its intended use:
+
+- ``simulators``: this contains the abstract ``Simulator`` interface from which all the simulators should inherit from.
+  This interface allows to decouple the rest of the code in PRL with the simulator being used. Some simulators might
+  have some features that other simulators don't have, in that case, an error is raised or an approximation is made.
+  For instance, ``PyBullet`` don't provide joint accelerations, but a simulator like ``MuJoCo`` does. As such, it is
+  checked in the ``Robot`` class if the simulator provide these accelerations, if not, it is approximated using finite
+  difference. Currently, only ``Bullet`` is fully-supported. Interfaces for ``MuJoCo`` and ``Dart`` are ongoing.
+
+  - ``middlewares``: this will contain the middleware classes that inherit from the ``Middleware`` class, such as
+    ``ROS`` and others. You will be able to provide it to the simulator and the simulator will use it to publish or
+    receive packages.
+
+- ``robots``: this contains the various robots (manipulators, grippers, legged robots, wheeled robots, flying robots,
+  etc) that can be used in PRL. They all inherit from ``Robot`` which itself inherit from ``Body`` (which is the most
+  abstract class). The ``Body`` has direct access to the simulator interface. Robots have also access to:
+
+  - ``sensors``: this contains the various sensors used by robots. They all inherit from the ``Sensor`` class. They
+    use the simulator to get their values.
+  - ``actuators``: this contains the various actuators used by robots. They all inherit from the ``Actuator`` class.
+    They might use the simulator to perform action through it.
+
+- ``worlds``: this contains the main ``World`` class which is inherited by all the other worlds. The ``World`` class
+  has direct access to the simulator interface (like ``Body``), and users should interact with it to load the various
+  bodies and robots in the world, or change the physical properties (friction, restitution, etc) of the world. Through
+  that class, you can also attach two bodies together and generate terrains.
+- ``utils``: this contains the various util methods like ``transformations`` (from one orientation representation to
+  another one, and functions that can be applied on quaternions), converters (that convert from one data type to
+  another), ``interpolators``, ``feedback laws``, and others that are used by other parts of the framework.
+
+  - ``data_structures``: this contains data structures such as ordered sets and the different type of queues.
+  - ``plotting``: this contains real-time plotting tools than can plot the joint positions, velocities, accelerations,
+    torques, or link frames in real-time. This is used in combination with the ``Simulator``.
+  - ``parsers``: this contains mainly parsers for some datasets, and robot/world file formats (such as URDF, SDF, Skel, 
+    MJCF, etc).
+
+- ``tools``: this contains the *interfaces* and *bridges*. 
+
+  - *Interfaces* allows to receive or send the data from/to various I/O interfaces (such as mouse, keyboard, 3D space 
+    mouse, game controllers, webcam, depth cameras, sensors like LeapMotion, and others). They all inherit from the 
+    abstract ``Interface`` class which has thread supports. If threads are not used, the user has to call the ``step`` 
+    method such that it reads the next value (i.e. these are not event-driven, i.e. you control when you want to get/set 
+    the data). Interfaces are independent from the other components in the PRL framework (with maybe at the exception 
+    of some ``utils`` methods), and as such can be used in other software. 
+  - *Bridges* makes the connection between an interface and another component in PRL (like a robot or body in the world, 
+    or the world camera). Fundamentally, they accept as input an interface and the component, and the user details what 
+    should be done in that class. This allows to decouple the interface from the application part; e.g. the same game 
+    controller interface could be used to move a wheeled robot or quadcopter robot by providing two bridges (one for 
+    wheeled robots, and one for quadcopter robots). All the bridges inherit from the abstract ``Bridge`` class, and as 
+    with interface a ``step`` method can be called.
+
+- ``states``: this contains the various states which all inherit from the ``State`` abstract class. States can easily
+  be composed together such that you could specify which states you would like to have. For instance, if you want
+  the joint positions, velocities, and the base position and orientation states, you can add them to form one common
+  state. Calling the state will compute their values, and they will save these in the ``data`` attribute. They
+  basically act as useful containers. States are notably provided as inputs to controllers, policies, and rewards among
+  others, and are outputted by the environments.
+- ``actions``: this contains the various actions which all inherit from the ``Action`` abstract class. They are given
+  notably to the policy during the initialization, which sets the action data. Calling an action will perform an action
+  in the simulator (e.g. move the robot joints using position control) or through an interface (e.g. say something
+  through the computer speakers).
+- ``rewards``: this contains the various *rewards* and *costs*. They all inherit from the ``Reward`` abstract class,
+  and various arithmetic operations can be performed on them. They accept as possible arguments the ``State`` and
+  ``Action``. Each time you call them, they check the data contained in the given states and actions and compute
+  the corresponding reward value. This allows the user to reuse different reward functions and easily combine them
+  without worrying how to get or compute the reward value. Note that as for states and actions, rewards can have a
+  particular range which specifies their domain. This is useful if we would like to know if a reward function is
+  strictly positive or not (e.g. the PoWER RL algorithm only accepts strictly positive rewards which it can check by
+  looking at the reward's range).
+- ``envs``: this contains the various environments. They all inherit from the ``Env`` class which accepts as arguments
+  at least the world, the state, and possibly a reward (if we are in the reinforcement learning case). These arguments
+  can be provided at runtime making it easy to (re)use other modules, and render the framework very flexible (see
+  `Composition over inheritance <https://en.wikipedia.org/wiki/Composition_over_inheritance>`_). Few robotic
+  environments are also provided in this class.
+
+  - ``states/generators``: this contains ``state generators`` which generates ``states`` for the environment. You can
+    for instance generate the position / orientation of a body, or its joints. They can be provided to the environment
+    and are called each time you reset the environment.
+  - ``physics``: this contains ``physics randomizers`` which can randomize the physical properties of the joints
+    (e.g. joint damping), links (e.g. mass), and the world (e.g. friction). They can be provided to the environment,
+    and are called each time the environment is reset.
+  - ``terminal_conditions``: this contains terminal conditions which detect if an episode is over or not. They can
+    in addition specify if the environment ended with a success or failure. You can provide them to the environment
+    which check them at each time step.
+
+- ``models``: this contains the various learning models, which have parameters or hyperparameters to optimize given
+  some data. These models can be categorized into two different types: movement primitives and general function
+  approximators. The models are independent from the rest of the framework (except maybe few ``utils`` functions).
+  Some models were implemented from scratch while others were wrapped.
+- ``approximators``: this contains the various approximators which is basically a wrapper around the above models (only
+  the ones that are function approximators and not movement primitives), and accepts as inputs states, actions and
+  general arrays/tensors. They all inherit from the ``Approximator`` class and represents an abstraction above the
+  model classes. Because they can accept states and actions, this makes them dependent on these submodules in PRL.
+  Approximators are notably used to model policies (which maps states to actions), value function approximators (which
+  maps states to a scalar, or states and actions to a scalar, or states to a scalar for each discrete action), and
+  dynamic transition functions (which maps states and actions to the next states). These are described next.
+- ``policies``: this contains the various policies that can be used in PRL. They all inherit from the ``Policy`` class,
+  and use internally approximators, or models (if movement primitives). They can operate at different rates, and are
+  provided with a state and action instance at the initialization.
+- ``values``: this contains the various value function approximators that can be used in PRL. They accepts as inputs
+  the states and possibly the actions (if Q-value function approximator). They are mostly used by reinforcement
+  learning algorithms).
+- ``dynamics``: this contains the various dynamic function approximators. This is mostly used by model-based 
+  reinforcement learning algorithms. This is currently not fully-implemented/operational.
+
+- ``tasks``: this contains the various learning tasks/paradigms. They all inherit from the ``Task`` class and accepts 
+  at least as inputs the policy(ies) and environment. They act as a container for these two's, and calling the 
+  ``step`` method will perform one full cycle in the agent-environment interaction loop. Subsequently, you can also 
+  call ``run`` to run several loop for the specified number of steps. Tasks can notably be provided to algorithms 
+  (especially RL algorithms).
+
+- ``distribution``: this contains few distributions that are used by exloration strategies (see next bullet point).
+- ``exploration``: this contains the various exploration strategies that can be used by the policy; parameter and
+  action exploration. They all inherit from the ``Exploration`` class and accepts as inputs the policy that they wrap
+  around.
+- ``storages``: this contains the various data storages/containers (such as experience replay storage and batches)
+  that are used during the learning process.
+- ``losses``: this contains the various losses that are used by the various algorithms. As for the rewards, you can
+  perform arithmetic operations on them and combine them in different ways.
+- ``optimizers``: this contains the various optimizers that can be used. We provide a common interface and wrap popular
+  optimizers. Currently, some optimizers are not fully-operational.
+- ``returns``: this provides the various returns and estimators that are used in RL.
+- ``algos``: this contains the various learning algorithms on how to acquire the data and train the various models
+  (policies, values, dynamics, etc).
+- ``metrics``: this contains the various metrics that are used in different learning paradigms. They are not currently 
+  all implemented. You can put different metrics together and plot them by just calling the ``plot`` method.
+
+Other folders include:
+
+- ``filters``: this contains various filters (KF, EKF, UKF, HF, etc).

pyrobolearn/__init__.py

@@ -3,8 +3,10 @@
 
 name = "pyrobolearn"
 
+import os
 import sys
 import signal
+from itertools import count
 
 # logging
 import logging
@@ -26,6 +28,9 @@ from . import robots
 # import worlds
 from . import worlds
 
+# import utils
+from . import utils
+
 # import physics randomizer
 from . import physics
 
@@ -35,6 +40,9 @@ from . import states
 # import actions
 from . import actions
 
+# import terminal conditions
+from . import terminal_conditions
+
 # import rewards
 from . import rewards
 
@@ -82,12 +90,15 @@ from . import algos
 
 # import experiments
 
+# import priority tasks
+from . import priorities
+
 
 # Meta-information about the package
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "MIT"
+__license__ = "GNU GPLv3"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"
@@ -102,7 +113,6 @@ def signal_handler(sig, frame):
 
 signal.signal(signal.SIGINT, signal_handler)
 
-
 # https://stackoverflow.com/questions/30483246/how-to-check-if-a-python-module-has-been-imported
 # https://stackoverflow.com/questions/14050281/how-to-check-if-a-python-module-exists-without-importing-it/25045228
 def module_imported(module_name):  # TODO: improve this method
@@ -114,6 +124,11 @@ def module_imported(module_name):  # TODO: improve this method
     return False
 
 
+world_mesh_path = os.path.dirname(os.path.abspath(__file__)) + '/worlds/meshes/'
+
+__all__ = [simulators, robots, worlds, physics, states, actions, terminal_conditions, rewards, envs, models,
+           approximators, policies, values, actorcritics, dynamics, tools]
+
 # Define what submodules/classes/functions should be loaded when writing 'from pyrobolearn import *'
 # __all__ = [
 #     # Submodules

pyrobolearn/actions/__init__.py

@@ -3,10 +3,13 @@
 from .action import Action
 
 # import basic actions
-from .basic_actions import *
+from .basic_actions import FixedAction, FunctionalAction
 
 # import robot actions
 from .robot_actions import *
 
 # import gym actions
-from .gym_actions import *
+from .gym_actions import GymAction
+
+# impot world actions
+from .world_actions import AttachAction

pyrobolearn/actions/action.py

@@ -161,6 +161,17 @@ class Action(object):
         # one action: change the data
         # if self.has_data():
         else:
+            if self.is_discrete():  # discrete action
+                if isinstance(data, np.ndarray):  # data action is a numpy array
+                    # check if given logits or not
+                    if data.shape[-1] != 1:  # logits
+                        data = np.array([np.argmax(data)])
+                elif isinstance(data, (float, np.integer)):
+                    data = int(data)
+                else:
+                    raise TypeError("Expecting the `data` action to be an int, numpy array, instead got: "
+                                    "{}".format(type(data)))
+
             if not isinstance(data, np.ndarray):
                 if isinstance(data, (list, tuple)):
                     data = np.array(data)
@@ -288,14 +299,25 @@ class Action(object):
         """
         return torch.cat([data.reshape(-1) for data in self.merged_torch_data])
 
+    @property
+    def spaces(self):
+        """
+        Get the corresponding spaces as a list of spaces.
+        """
+        if self.has_space():
+            return [self._space]
+        return [action._space for action in self._actions]
+
     @property
     def space(self):
         """
         Get the corresponding space.
         """
         if self.has_space():
-            return [self._space]
-        return [action._space for action in self._actions]
+            # return gym.spaces.Tuple([self._space])
+            return self._space
+        # return [action._space for action in self._actions]
+        return gym.spaces.Tuple([action._space for action in self._actions])
 
     @space.setter
     def space(self, space):
@@ -303,9 +325,43 @@ class Action(object):
         Set the corresponding space. This can only be used one time!
         """
         if self.has_data() and not self.has_space() and \
-                isinstance(space, (gym.spaces.Box, gym.spaces.Discrete)):
+                isinstance(space, (gym.spaces.Box, gym.spaces.Discrete, gym.spaces.MultiDiscrete)):
             self._space = space
 
+    @property
+    def merged_space(self):
+        """
+        Get the corresponding merged space. Note that all the spaces have to be of the same type.
+        """
+        if self.has_space():
+            return self._space
+        spaces = self.spaces
+        result = []
+        dtype, prev_dtype = None, None
+        for space in spaces:
+            if isinstance(space, gym.spaces.Box):
+                dtype = 'box'
+                result.append([space.low, space.high])
+            elif isinstance(space, gym.spaces.Discrete):
+                dtype = 'discrete'
+                result.append(space.n)
+            else:
+                raise NotImplementedError
+
+            if prev_dtype is not None and dtype != prev_dtype:
+                return self.space
+
+            prev_dtype = dtype
+
+        if dtype == 'box':
+            low = np.concatenate([res[0] for res in result])
+            high = np.concatenate([res[1] for res in result])
+            return gym.spaces.Box(low=low, high=high, dtype=np.float32)
+        elif dtype == 'discrete':
+            return gym.spaces.Discrete(n=np.sum(result))
+
+        return self.space
+
     @property
     def name(self):
         """
@@ -456,7 +512,7 @@ class Action(object):
                 if data is None:
                     data = self._data
                 self._write(data)
-            else:  # read each action
+            else:  # write each action
                 if self.actions:
                     if data is None:
                         data = [None] * len(self.actions)
@@ -525,8 +581,9 @@ class Action(object):
         Does the action have discrete values?
         """
         if self._data is None:
-            return [isinstance(action._space, gym.spaces.Discrete) for action in self._actions]
-        if isinstance(self._space, gym.spaces.Discrete):
+            return [isinstance(action._space, (gym.spaces.Discrete, gym.spaces.MultiDiscrete))
+                    for action in self._actions]
+        if isinstance(self._space, (gym.spaces.Discrete, gym.spaces.MultiDiscrete)):
             return [True]
         return [False]
 
@@ -534,7 +591,10 @@ class Action(object):
         """
         If all the actions are discrete, then it is discrete.
         """
-        return all(self.has_discrete_values())
+        values = self.has_discrete_values()
+        if len(values) == 0:
+            return False
+        return all(values)
 
     def has_continuous_values(self):
         """
@@ -556,6 +616,7 @@ class Action(object):
         """
         If the action is continuous, it returns the lower and higher bounds of the action.
         If the action is discrete, it returns the maximum number of discrete values that the action can take.
+        If the action is multi-discrete, it returns the maximum number of discrete values that each subaction can take.
 
         Returns:
             list/tuple: list of bounds if multiple actions, or bounds of this action
@@ -566,6 +627,8 @@ class Action(object):
             return (self._space.low, self._space.high)
         elif isinstance(self._space, gym.spaces.Discrete):
             return (self._space.n,)
+        elif isinstance(self._space, gym.spaces.MultiDiscrete):
+            return (self._space.nvec,)
         raise NotImplementedError
 
     def apply(self, fct):

pyrobolearn/actions/robot_actions/__init__.py

@@ -1,9 +1,20 @@
 
 # import the basic robot actions
-from .robot_actions import *
+from .robot_actions import RobotAction
 
 # import the joint actions
-from .joint_actions import *
+from .joint_actions import JointAction, JointPositionAction, JointPositionChangeAction, JointVelocityAction, \
+    JointVelocityChangeAction, JointPositionAndVelocityAction, JointPositionAndVelocityChangeAction, \
+    JointTorqueAction, JointForceAction, JointTorqueGravityCompensationAction, JointTorqueChangeAction, \
+    JointAccelerationAction, JointAccelerationChangeAction
 
 # import the link / end-effector actions
-from .link_actions import *
+from .link_actions import LinkAction, LinkPositionAction, LinkPositionChangeAction, LinkOrientationAction, \
+    LinkOrientationChangeAction, LinkPoseAction, LinkPoseChangeAction, LinkVelocityAction, LinkVelocityChangeAction, \
+    LinkForceAction, LinkTorqueAction, LinkWrenchAction, ApplyForceAction, ApplyTorqueAction  # , ApplyWrenchAction
+
+# import the actuator actions
+from .actuator_actions import ActuatorAction
+
+# import grasping action
+from .grasp_actions import GraspAction

pyrobolearn/actions/robot_actions/actuator_actions.py

@@ -5,6 +5,7 @@
 from abc import ABCMeta
 import collections
 import numpy as np
+import copy
 
 from pyrobolearn.actions.action import Action
 from pyrobolearn.robots.actuators.actuator import Actuator
@@ -25,26 +26,55 @@ class ActuatorAction(Action):
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, actuators, ticks=1):
+    def __init__(self, actuator, ticks=1):
         """
         Initialize the sensor state.
 
         Args:
-            actuators (A, list of Actuator): actuator(s).
+            actuator (Actuator): actuator instance.
             ticks (int): number of ticks to sleep before setting the next action data.
         """
-        if not isinstance(actuators, collections.Iterable):
-            actuators = [actuators]
-        for actuator in actuators:
-            if not isinstance(actuator, Actuator):
-                raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, instead got: "
-                                "{}".format(type(actuator)))
-        self.actuators = actuators
         super(ActuatorAction, self).__init__(ticks=ticks)
 
+        # set the actuator instance
+        self.actuator = actuator
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def actuator(self):
+        """Return the actuator instance."""
+        return self._actuator
+
+    @actuator.setter
+    def actuator(self, actuator):
+        """Set the actuator instance."""
+        if not isinstance(actuator, Actuator):
+            raise TypeError("Expecting the given 'actuator' to be an instance of `Actuator`, instead got: "
+                            "{}".format(type(actuator)))
+        self._actuator = actuator
+
+    ###########
+    # Methods #
+    ###########
+
+    def _write(self, data):
+        """Write the data in the actuator and execute the actuator."""
+        # set the data in the actuator
+        self.actuator.data = data
+
+        # activate the actuator
+        self.actuator.act()
+
+    #############
+    # Operators #
+    #############
+
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(actuators=self.actuators, ticks=self.ticks)
+        return self.__class__(actuator=self.actuator, ticks=self.ticks)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the action. This can be overridden in the child class.
@@ -55,7 +85,7 @@ class ActuatorAction(Action):
         if self in memo:
             return memo[self]
 
-        actuators = copy.deepcopy(self.actuators, memo)
+        actuators = copy.deepcopy(self.actuator, memo)
         action = self.__class__(actuators=actuators, ticks=self.ticks)
 
         memo[self] = action

pyrobolearn/actions/robot_actions/grasp_actions.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+"""Define grasping actions
+"""
+
+import copy
+import numpy as np
+
+from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
+from pyrobolearn.robots.gripper import Gripper
+
+
+__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 GraspAction(RobotAction):
+    r"""Attach Action.
+
+    This is allows to
+    """
+
+    def __init__(self, gripper):
+        """
+        Initialize the grasping action.
+
+        Args:
+            gripper (Gripper): a gripper instance.
+        """
+        super(GraspAction, self).__init__(robot=gripper)
+        self.gripper = gripper
+
+    @property
+    def gripper(self):
+        """Return the gripper instance."""
+        return self._gripper
+
+    @gripper.setter
+    def gripper(self, gripper):
+        if not isinstance(gripper, Gripper):
+            raise TypeError("Expecting the given 'gripper' to be an instance of `Gripper`, but got instead: "
+                            "{}".format(type(gripper)))
+        self._gripper = gripper
+
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the continuous data representing the grasping strength.
+        """
+        if isinstance(data, np.ndarray):
+            data = data[0]
+        self.gripper.grasp(strength=data)
+
+    def __copy__(self):
+        """Return a shallow copy of the action. This can be overridden in the child class."""
+        return self.__class__(gripper=self.gripper)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the action. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass
+        """
+        if self in memo:
+            return memo[self]
+        gripper = copy.deepcopy(self.gripper)
+        action = self.__class__(gripper=gripper)
+        memo[self] = action
+        return action

pyrobolearn/actions/robot_actions/joint_actions.py

@@ -7,8 +7,9 @@ This includes notably the joint positions, velocities, and force/torque actions.
 import copy
 import numpy as np
 from abc import ABCMeta
+import gym
 
-from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
+from pyrobolearn.actions.robot_actions.robot_actions import RobotAction, Robot
 
 
 __author__ = "Brian Delhaisse"
@@ -26,13 +27,17 @@ class JointAction(RobotAction):
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, robot, joint_ids=None):
+    def __init__(self, robot, joint_ids=None, discrete_values=None):
         """
         Initialize the joint action.
 
         Args:
             robot (Robot): robot instance
             joint_ids (int, int[N]): joint id or list of joint ids
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
         """
         super(JointAction, self).__init__(robot)
 
@@ -43,13 +48,146 @@ class JointAction(RobotAction):
             joint_ids = [joint_ids]
         self.joints = joint_ids
 
+        # if discrete values, check the type and set the space
+        if discrete_values is not None:
+
+            # check the type
+            if not isinstance(discrete_values, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'discrete_values' to be a list/tuple/np.array of float/int, but "
+                                "instead got: {}".format(type(discrete_values)))
+
+            if len(discrete_values) == 0:
+                raise ValueError("Expecting at least one list of discrete values")
+            if not isinstance(discrete_values[0], (list, tuple, np.ndarray)):
+                discrete_values = [discrete_values]
+
+            # check that the number of list of discrete values match the number of joints
+            if len(discrete_values) != len(self.joints):
+                raise ValueError("The number of discrete value sets (={}) does not match the number of joints "
+                                 "(={})".format(len(discrete_values), len(self.joints)))
+
+            # check the type and shape of each discrete value set, and convert it to numpy arrays
+            for i, value in enumerate(discrete_values):
+                if not isinstance(value, (list, tuple, np.ndarray)):
+                    raise TypeError("Expecting each discrete value set to be a list/tuple/np.ndarray, instead got: "
+                                    "{} at index {}".format(type(value), i))
+                value = np.asarray(value)
+                discrete_values[i] = value
+                if value.ndim != 1:
+                    raise ValueError("Expecting each discrete value set to be a 1D array, instead got a shape of: "
+                                     "{}".format(value.shape))
+
+        # set the discrete values
+        self.discrete_values = discrete_values
+
+        # set the data and the space in the case of discrete values
+        if self.discrete_values is not None:
+            # set the space
+            if len(self.discrete_values) == 1:
+                self._space = gym.spaces.Discrete(len(self.discrete_values))
+                self.discrete_values = self.discrete_values[0]
+            else:
+                self._space = gym.spaces.MultiDiscrete([len(value) for value in self.discrete_values])
+
+            # set the data
+            if isinstance(self._space, gym.spaces.Discrete):
+                self.data = np.zeros(1, dtype=np.int)  # the first index is the default values
+            else:
+                self.data = np.zeros(len(self._space.nvec), dtype=np.int)  # the first indices are the default values
+
     # @property
     # def size(self):
     #     return len(self.joints)
 
-    def bounds(self):
-        """Return the joint limits."""
-        return self.robot.get_joint_limits(self.joints)
+    def _check_continuous_bounds(self, bounds):
+        """Check the given continuous bounds."""
+        # check the type of the bounds
+        if not isinstance(bounds, (tuple, list, np.ndarray)):
+            raise TypeError("Expecting the given bounds to be a tuple/list/np.ndarray of float, instead got: "
+                            "{}".format(type(bounds)))
+
+        # check that the bounds have a length of 2 (i.e. lower and upper bounds)
+        if len(bounds) != 2:
+            raise ValueError("Expecting the bounds to be of length 2 (i.e. lower and upper bounds), instead got a "
+                             "length of {}".format(len(bounds)))
+
+        # if both bounds are not None, reshape if necessary
+        if bounds[0] is not None and bounds[1] is not None:
+            bounds = np.asarray(bounds).reshape(2, -1)
+            if len(self.joints) != bounds.shape[1]:
+                if bounds.shape[1] == 1:
+                    bounds = np.array([bounds[0, 0] * np.ones(len(self.joints)),
+                                       bounds[1, 0] * np.ones(len(self.joints))])
+                else:
+                    raise ValueError("Expecting the number of bounds to match up with the number of joints")
+        else:
+            bounds = tuple(bounds)
+        return bounds
+
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the data can be discrete or continuous.
+        """
+        # - if the action is discrete, then the data should be an index, or an array of values from which takes the max
+        # - if the action is multi-discrete, then the data should be an array of index, or a list of array values from
+        #   which to take the max for each array
+        if self.discrete_values is not None:
+            # if the action is discrete
+            if isinstance(self._space, gym.spaces.Discrete):
+                # if the data is an array of values, take the argmax
+                if isinstance(data, np.ndarray):
+                    if data.size > 1:
+                        data = np.argmax(data.reshape(-1))
+                    else:
+                        data = int(data.reshape(-1)[0])
+
+                # elif not an index, raise an error
+                elif not isinstance(data, int):
+                    raise TypeError("Expecting the given data to be an int (representing the index) for the discrete "
+                                    "values, instead got: {}".format(type(data)))
+
+                # take the corresponding "continuous" data
+                data = self.discrete_values[data]
+
+            # if action is multi-discrete
+            elif isinstance(self._space, gym.spaces.MultiDiscrete):
+
+                # check the type of the data
+                if isinstance(data, (list, np.ndarray)):
+                    raise TypeError("Expecting the data to be an list of int/np.array, or a np.array, instead got: "
+                                    "{}".format(type(data)))
+
+                # check that the number of rows match the number of discrete actions
+                if len(data) != self._space.shape[0]:
+                    raise ValueError("The given data does not have the same length (={}) as the number of discrete "
+                                     "action (={})".format(len(data), self._space.shape[0]))
+
+                # check each data row and convert it to index if necessary
+                data_tmp = []
+                for d in data:
+                    if isinstance(d, np.ndarray):
+                        d = np.argmax(d.reshape(-1))
+                    elif not isinstance(d, int):
+                        raise TypeError("Expecting the given data to be an int (representing the index) for the "
+                                        "discrete values, instead got: {}".format(type(d)))
+                    data_tmp.append(d)
+
+                # take the corresponding "continuous" data
+                data = np.array([self.discrete_values[d] for d in data])
+
+        self._write_continuous(data)
+
+    def _write_continuous(self, data):
+        """
+        Write the given continuous data. Child method that has to be implement in the child classes.
+
+        Args:
+            data (np.ndarray): continuous data to be written.
+        """
+        raise NotImplementedError
 
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""
@@ -76,13 +214,50 @@ class JointPositionAction(JointAction):
     Set the joint positions using position control.
     """
 
-    def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None):
-        self.kp, self.kd, self.max_force = kp, kd, max_force
-        super(JointPositionAction, self).__init__(robot, joint_ids)
-        self.data = robot.get_joint_positions(self.joints)
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None,
+                 discrete_values=None):
+        """
+        Initialize the joint position action.
 
-    def _write(self, data):
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action.
+                If None it will use the default joint position limits.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If None, it will apply the
+                default maximum force values (read from the URDF).
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointPositionAction, self).__init__(robot, joint_ids, discrete_values=discrete_values)
+        self.kp, self.kd, self.max_force = kp, kd, max_force
+
+        # check max force and take the one by default
+        if self.max_force is None:
+            self.max_force = self.robot.get_joint_max_forces(self.joints)
+            if np.allclose(self.max_force, 0):
+                self.max_force = None
+
+        # set data and space if continuous
+        if self.discrete_values is None:
+            self.data = self.robot.get_joint_positions(self.joints)
+            bounds = self._check_continuous_bounds(bounds)
+            if bounds == (None, None):
+                bounds = self.robot.get_joint_limits(self.joints)
+            self._space = gym.spaces.Box(low=bounds[:, 0], high=bounds[:, 1])
+
+    def bounds(self):
+        """Return the joint limits."""
+        return self.robot.get_joint_limits(self.joints)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
+        # self.robot.set_joint_positions(data, self.joints, bounds=self.bounds, kp=self.kp, kd=self.kd,
+        #                                forces=self.max_force, discrete_values=self.discrete_values)
         self.robot.set_joint_positions(data, self.joints, kp=self.kp, kd=self.kd, forces=self.max_force)
 
     def __copy__(self):
@@ -99,44 +274,165 @@ class JointPositionAction(JointAction):
             return memo[self]
         robot = memo.get(self.robot, self.robot)  # copy.deepcopy(self.robot, memo)
         joints = copy.deepcopy(self.joints)
+        bounds = copy.deepcopy(self.bounds)
         kp = copy.deepcopy(self.kp)
         kd = copy.deepcopy(self.kd)
         max_force = copy.deepcopy(self.max_force)
-        action = self.__class__(robot=robot, joint_ids=joints, kp=kp, kd=kd, max_force=max_force)
+        discrete_values = copy.deepcopy(self.discrete_values)
+        action = self.__class__(robot=robot, joint_ids=joints, bounds=bounds, kp=kp, kd=kd, max_force=max_force,
+                                discrete_values=discrete_values)
         memo[self] = action
         return action
 
 
+class JointPositionChangeAction(JointPositionAction):
+    r"""Joint Position Change Action
+
+    Set the joint positions using position control; this class expect to receive a change in the joint positions
+    (i.e. instantaneous joint velocities). That is, the current joint positions are added to the given joint position
+    changes. If none are provided, it will stay at the current configuration.
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None,
+                 discrete_values=None):
+        """
+        Initialize the joint position change action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action.
+                If None it will use the default joint position limits.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointPositionChangeAction, self).__init__(robot, joint_ids, bounds=bounds, kp=kp, kd=kd,
+                                                        max_force=max_force, discrete_values=discrete_values)
+
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = np.zeros(len(self.joints))
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        # add the original joint positions
+        data += self.robot.get_joint_positions(self.joints)
+        super(JointPositionChangeAction, self)._write_continuous(data)
+
+
 class JointVelocityAction(JointAction):
     r"""Joint Velocity Action
 
     Set the joint velocities using velocity control.
     """
 
-    def __init__(self, robot, joint_ids=None):
-        super(JointVelocityAction, self).__init__(robot, joint_ids)
-        self.data = robot.get_joint_velocities(self.joints)
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint velocity action.
 
-    def _write(self, data):
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action.
+                If None it will use the default joint position limits.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointVelocityAction, self).__init__(robot, joint_ids, discrete_values=discrete_values)
+
+        # set data and space if continuous
+        if self.discrete_values is None:
+            self.data = robot.get_joint_velocities(self.joints)
+            bounds = self._check_continuous_bounds(bounds)
+            if bounds == (None, None):
+                bounds = self.robot.get_joint_max_velocities(self.joints)
+                if np.allclose(bounds, 0):
+                    bounds = np.array([-np.infty * np.ones(len(self.joints)),
+                                       np.infty * np.ones(len(self.joints))])
+            self._space = gym.spaces.Box(low=bounds[:, 0], high=bounds[:, 1])
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
         self.robot.set_joint_velocities(data, self.joints)
 
 
-class JointPositionAndVelocityAction(JointAction):
+class JointVelocityChangeAction(JointVelocityAction):
+    r"""Joint Velocity Change Action
+
+    Set the joint velocities using velocity control;  this class expect to receive a change in the joint velocities
+    (i.e. instantaneous joint accelerations). That is, the current joint velocities are added to the given joint
+    velocity changes. If none are provided, it will keep the current joint velocities.
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint velocity change action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): lower and upper bound in the case of continuous action.
+                If None it will use the default joint position limits.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointVelocityChangeAction, self).__init__(robot, joint_ids, bounds=bounds,
+                                                        discrete_values=discrete_values)
+
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = np.zeros(len(self.joints))
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_joint_velocities(self.joints)
+        super(JointVelocityChangeAction, self)._write_continuous(data)
+
+
+class JointPositionAndVelocityAction(JointAction):  # TODO: discrete values
     r"""Joint position and velocity action
 
-    Set the joint position using position control using PD control, where the contraint error to be minimized is
+    Set the joint position using position control using PD control, where the constraint error to be minimized is
     given by: :math:`error = kp * (q^* - q) - kd * (\dot{q}^* - \dot{q})`.
     """
 
-    def __init__(self, robot, joint_ids=None, kp=None, kd=None, max_force=None):
-        super(JointPositionAndVelocityAction, self).__init__(robot, joint_ids)
-        self.kp, self.kd, self.max_force = kp, kd, max_force
-        pos, vel = robot.get_joint_positions(self.joints), robot.get_joint_velocities(self.joints)
-        self.data = np.concatenate((pos, vel))
-        self.idx = len(pos)
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None,
+                 discrete_values=None):
+        """
+        Initialize the joint position and velocity action.
 
-    def _write(self, data):
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointPositionAndVelocityAction, self).__init__(robot, joint_ids, discrete_values=discrete_values)
+        self.kp, self.kd, self.max_force = kp, kd, max_force
+
+        # set data if continuous
+        if self.discrete_values is None:
+            pos, vel = robot.get_joint_positions(self.joints), robot.get_joint_velocities(self.joints)
+            self.data = np.concatenate((pos, vel))
+        self.idx = len(self.joints)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
         self.robot.set_joint_positions(data[:self.idx], self.joints, kp=self.kp, kd=self.kd,
                                        velocities=data[self.idx:], forces=self.max_force)
@@ -163,6 +459,43 @@ class JointPositionAndVelocityAction(JointAction):
         return action
 
 
+class JointPositionAndVelocityChangeAction(JointPositionAndVelocityAction):
+    r"""Joint position and velocity action
+
+    Set the joint position using position control using PD control, where the constraint error to be minimized is
+    given by: :math:`error = kp * (q^* - q) - kd * (\dot{q}^* - \dot{q})`.
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), kp=None, kd=None, max_force=None,
+                 discrete_values=None):
+        """
+        Initialize the joint position and velocity change action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id(s). If None, it will take all the actuated joints.
+            kp (float, np.array[N], None): position gain(s)
+            kd (float, np.array[N], None): velocity gain(s)
+            max_force (float, np.array[N], None, bool): maximum motor torques / forces. If True, it will apply the
+                default maximum force values.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointPositionAndVelocityChangeAction, self).__init__(robot, joint_ids, kp=kp, kd=kd, max_force=max_force,
+                                                                   discrete_values=discrete_values)
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = np.zeros(2*len(self.joints))
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        pos, vel = self.robot.get_joint_positions(self.joints), self.robot.get_joint_velocities(self.joints)
+        data += np.concatenate((pos, vel))
+        super(JointPositionAndVelocityChangeAction, self)._write_continuous(data)
+
+
 # class JointPositionVelocityAccelerationAction(JointAction):
 #     r"""Set the joint positions, velocities, and accelerations.
 #
@@ -172,26 +505,55 @@ class JointPositionAndVelocityAction(JointAction):
 #     pass
 
 
-class JointForceAction(JointAction):
-    r"""Joint Force Action
+class JointTorqueAction(JointAction):
+    r"""Joint Torque/Force Action
 
     Set the joint force/torque using force/torque control.
     """
 
-    def __init__(self, robot, joint_ids=None, f_min=-np.infty, f_max=np.infty):
-        super(JointForceAction, self).__init__(robot, joint_ids)
-        self.data = robot.get_joint_torques(self.joints)
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint torque/force action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): minimum and maximum torques/forces respectively. If None,
+                it will check the minimum/maximum torques/forces allowed. If it doesn't find them, it will set them
+                to -np.infty and np.infty.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointTorqueAction, self).__init__(robot, joint_ids, discrete_values=discrete_values)
+
+        # check torque bounds
+        f_min, f_max = self._check_continuous_bounds(bounds)
+        if f_min is None or f_max is None:
+            f = robot.get_joint_max_forces(joint_ids=self.joints)
+            f_min = -f if f_min is None else f_min
+            f_max = f if f_max is None else f_max
+            if np.allclose(f_min, 0):
+                f_min = -np.infty * np.ones(len(self.joints))
+            if np.allclose(f_max, 0):
+                f_max = np.infty * np.ones(len(self.joints))
         self.f_min = f_min
         self.f_max = f_max
 
-    def _write(self, data):
+        # set data and space if continuous
+        if self.discrete_values is None:
+            self.data = robot.get_joint_torques(self.joints)
+            self._space = gym.spaces.Box(low=self.f_min, high=self.f_max)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
         data = np.clip(data, self.f_min, self.f_max)
         self.robot.set_joint_torques(data, self.joints)
 
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(robot=self.robot, joint_ids=self.joints, f_min=self.f_min, f_max=self.f_max)
+        return self.__class__(robot=self.robot, joint_ids=self.joints, bounds=(self.f_min, self.f_max))
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the action. This can be overridden in the child class.
@@ -210,7 +572,52 @@ class JointForceAction(JointAction):
         return action
 
 
-class JointAccelerationAction(JointAction):
+# alias
+JointForceAction = JointTorqueAction
+
+
+class JointTorqueGravityCompensationAction(JointTorqueAction):
+    r"""Joint torque action with gravity compensation enabled.
+
+    This adds the given torques to the gravity compensation torques. That is, if a torque of 0 is provided, the robot
+    will be in a gravity compensation mode.
+    """
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint torque/force action with gravity compensation.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): minimum and maximum torques/forces respectively. If None,
+                it will check the minimum/maximum torques/forces allowed. If it doesn't find them, it will set them
+                to -np.infty and np.infty.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointTorqueGravityCompensationAction, self).__init__(robot, joint_ids, bounds=bounds,
+                                                                   discrete_values=discrete_values)
+        self.q_indices = self.robot.get_q_indices(joint_ids=self.joints)
+
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = np.zeros(len(self.joints))
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        # add gravity compensation torques
+        data += self.robot.get_gravity_compensation_torques(q_idx=self.q_indices)
+        super(JointTorqueGravityCompensationAction, self)._write_continuous(data)
+
+
+# alias
+# JointForceGravityCompensationAction = JointTorqueGravityCompensationAction
+JointTorqueChangeAction = JointTorqueGravityCompensationAction
+
+
+class JointAccelerationAction(JointAction):  # TODO: discrete values
     r"""Joint Acceleration Action
 
     Set the joint accelerations using force/torque control. In order to produce the given joint accelerations,
@@ -218,20 +625,40 @@ class JointAccelerationAction(JointAction):
     to be applied.
     """
 
-    def __init__(self, robot, joint_ids=None, a_min=-np.infty, a_max=np.infty):
-        super(JointAccelerationAction, self).__init__(robot, joint_ids)
-        self.data = robot.get_joint_accelerations(self.joints)
-        self.a_min = a_min
-        self.a_max = a_max
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint acceleration action.
 
-    def _write(self, data):
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): minimum and maximum accelerations. If None, it will use
+                the default joint acceleration limits. If it still doesn't find them, it will set -np.infty and
+                np.infty.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointAccelerationAction, self).__init__(robot, joint_ids, discrete_values=discrete_values)
+
+        # TODO
+        self.a_min = bounds[0]
+        self.a_max = bounds[1]
+
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = robot.get_joint_accelerations(self.joints)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
         data = np.clip(data, self.a_min, self.a_max)
         self.robot.set_joint_accelerations(data, self.joints)
 
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(robot=self.robot, joint_ids=self.joints, a_min=self.a_min, a_max=self.a_max)
+        return self.__class__(robot=self.robot, joint_ids=self.joints, bounds=(self.a_min, self.a_max),
+                              discrete_values=self.discrete_values)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the action. This can be overridden in the child class.
@@ -245,6 +672,43 @@ class JointAccelerationAction(JointAction):
         joints = copy.deepcopy(self.joints)
         a_min = copy.deepcopy(self.a_min)
         a_max = copy.deepcopy(self.a_max)
-        action = self.__class__(robot=robot, joint_ids=joints, f_min=a_min, f_max=a_max)
+        discrete_values = copy.deepcopy(self.discrete_values)
+        action = self.__class__(robot=robot, joint_ids=joints, bounds=(a_min, a_max), discrete_values=discrete_values)
         memo[self] = action
         return action
+
+
+class JointAccelerationChangeAction(JointAccelerationAction):
+    r"""Joint Acceleration Change Action
+
+    Set the joint accelerations using force/torque control. In order to produce the given joint accelerations,
+    we use inverse dynamics which given the joint accelerations produce the corresponding joint forces/torques
+    to be applied.
+    """
+
+    def __init__(self, robot, joint_ids=None, bounds=(None, None), discrete_values=None):
+        """
+        Initialize the joint acceleration action.
+
+        Args:
+            robot (Robot): robot instance.
+            joint_ids (int, list of int, None): joint id, or list of joint ids. If None, get all the actuated joints.
+            bounds (tuple of 2 float / np.array[N] / None): minimum and maximum accelerations. If None, it will use
+                the default joint acceleration limits. If it still doesn't find them, it will set -np.infty and
+                np.infty.
+            discrete_values (np.array[M], np.array[N,M], list of np.array[M], None): discrete values for each joint.
+                Note that by specifying this, the joint action is no more continuous but becomes discrete. By default,
+                the first value along the first axis / dimension are the values by default that are set if no data
+                is provided.
+        """
+        super(JointAccelerationChangeAction, self).__init__(robot, joint_ids, bounds=bounds,
+                                                            discrete_values=discrete_values)
+
+        # set data if continuous
+        if self.discrete_values is None:
+            self.data = np.zeros(len(self.joints))
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_joint_accelerations(self.joints)
+        super(JointAccelerationChangeAction, self)._write_continuous(data)

pyrobolearn/actions/robot_actions/link_actions.py

@@ -6,8 +6,11 @@ This includes notably the link positions, velocities, and force/torque actions.
 
 import copy
 from abc import ABCMeta
+import numpy as np
+import gym
 
 from pyrobolearn.actions.robot_actions.robot_actions import RobotAction
+from pyrobolearn.utils.transformation import get_rpy_from_quaternion, get_quaternion_from_rpy
 
 
 __author__ = "Brian Delhaisse"
@@ -20,29 +23,78 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class LinkAction(RobotAction):
+class LinkAction(RobotAction):  # TODO: multiple links
     r"""Link Action
     """
     __metaclass__ = ABCMeta
 
-    def __init__(self, robot, link_ids=None):
+    def __init__(self, robot, link_id=-1, discrete_values=None):
         """
         Initialize the link action.
 
         Args:
-            robot (Robot): robot instance
-            link_ids (int, int[N]): link id or list of link ids
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it is the base.
+            discrete_values (np.array[N, M], np.array[N], None): if provided, it represents the discrete values that
+                the action can take. Note that the action is no more continuous and becomes discrete at that point.
+                The first value will be the default value to be set if no data is provided.
         """
         super(LinkAction, self).__init__(robot)
 
-        # get the joints of the robot
-        if link_ids is None:
-            link_ids = robot.get_link_ids()
-        self.links = link_ids
+        # get the link of the robot
+        if link_id is None:
+            link_id = -1
+        self.link = int(link_id)
+
+        # if discrete values, check the type and create the space
+        if discrete_values is not None:
+            if not isinstance(discrete_values, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'discrete_values' to be a list/tuple/np.array of float/int, but "
+                                "instead got: {}".format(type(discrete_values)))
+            discrete_values = np.asarray(discrete_values)
+            self._space = gym.spaces.Discrete(len(discrete_values))
+        self.discrete_values = discrete_values
+
+        # set the data in the case it is discrete
+        if self.discrete_values is not None:
+            self.data = np.zeros(1, dtype=np.int)  # set the data to be the first index
+
+    def _check_discrete_values(self, dim, last_dim):
+        """Check that the discrete values have the correct dimensions / shape."""
+        # check discrete values
+        if self.discrete_values is not None:
+            if not (len(self.discrete_values.shape) == dim and self.discrete_values.shape[-1] == last_dim):
+                raise ValueError("Expecting the discrete values to have a dimension of {} and the last value of the "
+                                 "shape to be equal to {}, but instead got respectively: "
+                                 "{}, {}".format(dim, last_dim, len(self.discrete_values.shape),
+                                                 self.discrete_values.shape[-1]))
+
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the data can be discrete or continuous.
+        """
+        # if the action is discrete, then the data should be an index, or an array of values from which takes the max
+        if self.is_discrete():
+            if isinstance(data, np.ndarray):
+                data = np.argmax(data.reshape(-1))
+            data = self.discrete_values[data]
+        self._write_continuous(data)
+
+    def _write_continuous(self, data):
+        """
+        Write the given continuous data. Child method that has to be implement in the child classes.
+
+        Args:
+            data (np.ndarray): continuous data to be written.
+        """
+        raise NotImplementedError
 
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""
-        return self.__class__(self.robot, self.links)
+        return self.__class__(self.robot, self.link)
 
     def __deepcopy__(self, memo={}):
         """Return a deep copy of the action. This can be overridden in the child class.
@@ -53,101 +105,604 @@ class LinkAction(RobotAction):
         if self in memo:
             return memo[self]
         robot = memo.get(self.robot, self.robot)  # copy.deepcopy(self.robot, memo)
-        links = copy.deepcopy(self.links)
+        links = copy.deepcopy(self.link)
         action = self.__class__(robot, links)
         memo[self] = action
         return action
 
 
-class LinkPositionAction(LinkAction):
-    r"""Link position action
+class LinkPositionAction(LinkAction):  # TODO: multiple links
+    r"""Link world position action
 
-    Set the position using IK for the specified robot link(s).
+    Set the world position using IK for the specified robot link.
     """
 
-    def __init__(self, robot, link_ids=None):
+    def __init__(self, robot, link_id=-1, discrete_values=None):
         """
-        Initialize the link position action.
+        Initialize the link world position action.
 
         Args:
-            robot (Robot): robot instance
-            link_ids (int, int[N]): link id or list of link ids
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N,3], None): if provided, it represents the discrete values that the action
+                can take. Note that the action is no more continuous and becomes discrete at that point. Also, note
+                that this parameter makes probably  more sense with `LinkPositionChangeAction` instead. The first
+                value will be the default value to be set if no data is provided.
         """
-        super(LinkPositionAction, self).__init__(robot, link_ids)
+        super(LinkPositionAction, self).__init__(robot, link_id, discrete_values=discrete_values)
 
-    def _write(self, data):
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=3)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = self.robot.get_link_world_positions(link_ids=self.link, flatten=True)  # (3,)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
-        self.robot.set_link_positions(self.links, data)
+        self.robot.set_link_positions(link_ids=self.link, positions=data)
 
 
-class LinkVelocityAction(LinkAction):
-    r"""Link velocity action
+class LinkPositionChangeAction(LinkAction):  # TODO: multiple links
+    r"""Link world position change action
 
-    Set the cartesian velocity(ies) for the specified robot link(s).
+    Set the world position using IK for the specified robot link. Instead of specifying directly the desired
+    cartesian position(s), the amount of change in the current positions is provided.
     """
 
-    def __init__(self, robot, link_ids=None):
+    def __init__(self, robot, link_id=-1, discrete_values=None):
         """
-        Initialize the link position action.
+        Initialize the link world position change action.
 
         Args:
-            robot (Robot): robot instance
-            link_ids (int, int[N]): link id or list of link ids
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N,3], None): if provided, it represents the discrete values that the action
+                can take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
         """
-        super(LinkVelocityAction, self).__init__(robot, link_ids)
+        super(LinkPositionChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values)
 
-    def _write(self, data):
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=3)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = np.zeros(3)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
-        # self.robot
-        pass
+        data += self.robot.get_link_world_positions(link_ids=self.link, flatten=True)  # (3,)
+        self.robot.set_link_positions(link_ids=self.link, positions=data)
 
 
-class LinkForceAction(LinkAction):
+class LinkOrientationAction(LinkAction):  # TODO: multiple links
+    r"""Link world orientation action
+
+    Set the world orientation using IK for the specified robot link.
+    """
+
+    def __init__(self, robot, link_id=-1, discrete_values=None):
+        """
+        Initialize the link world orientation action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N,4], None): if provided, it represents the discrete values that the action can
+                take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
+        """
+        super(LinkOrientationAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=4)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = self.robot.get_link_world_orientations(link_ids=self.link, flatten=True)  # (4,)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        self.robot.set_link_positions(link_ids=self.link, orientations=data)
+
+
+class LinkOrientationChangeAction(LinkAction):  # TODO: multiple links
+    r"""Link world orientation change action
+
+    Set the world orientation using IK for the specified robot link. Instead of specifying directly the desired
+    cartesian orientation(s), the amount of change in the current orientations is provided.
+
+    Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians), and
+    not as quaternions.
+    """
+
+    def __init__(self, robot, link_id=-1, discrete_values=None):
+        """
+        Initialize the link world orientation change action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N,3], None): if provided, it represents the discrete values that the orientation
+                change action can take. The orientations are represented as Roll-Pitch-Yaw angles. Note that the
+                action is no more continuous and becomes discrete at that point. The first value will be the default
+                value to be set if no data is provided.
+        """
+        super(LinkOrientationChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=3)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = np.zeros(3)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        # get current orientations and convert them to RPY angles
+        orientation = self.robot.get_link_world_orientations(link_ids=self.link)  # (4,)
+        orientation = get_rpy_from_quaternion(orientation)  # (3,)
+
+        # add change in orientations
+        data += orientation  # (3,)
+
+        # convert them back to quaternions
+        data = get_quaternion_from_rpy(data)  # (4,)
+        self.robot.set_link_positions(link_ids=self.link, orientations=data)
+
+
+class LinkPoseAction(LinkAction):  # TODO: multiple link
+    r"""Link world pose action
+
+    Set the world pose using IK for the specified robot link.
+    """
+
+    def __init__(self, robot, link_id=-1, discrete_values=None):
+        """
+        Initialize the link world pose action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N,7], None): if provided, it represents the discrete values that the action can
+                take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
+        """
+        super(LinkPoseAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=7)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = self.robot.get_link_world_poses(link_ids=self.link, flatten=True)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        self.robot.set_link_positions(link_ids=self.link, positions=data[:3], orientations=data[3:])
+
+
+class LinkPoseChangeAction(LinkAction):  # TODO: multiple link
+    r"""Link world change pose action
+
+    Set the world pose using IK for the specified robot link. Instead of specifying directly the desired
+    cartesian pose(s), the amount of change in the current poses is provided.
+
+    Warnings: the difference in orientations should be provided as a change in roll-pitch-yaw angles (in radians),
+    and not as quaternions.
+    """
+
+    def __init__(self, robot, link_id=-1, discrete_values=None):
+        """
+        Initialize the link world pose change action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N, 6], None): if provided, it represents the discrete values that the link pose
+                change action can take. Note that the orientation part is represented as Roll-Pitch-Yaw angles.
+                Note that the action is no more continuous and becomes discrete at that point. The first value will
+                be the default value to be set if no data is provided.
+        """
+        super(LinkPoseChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=6)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = np.zeros(6)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        # get current pose
+        position = self.robot.get_link_world_poses(link_ids=self.link, flatten=False)  # (3,)
+        orientation = self.robot.get_link_world_orientations(link_ids=self.link, flatten=False)  # (4,)
+        orientation = get_rpy_from_quaternion(orientation)  # (3,)
+
+        # add changes
+        data[:3] += position        # (3,)
+        data[3:] += orientation     # (3,)
+
+        # convert back orientations to quaternions
+        data = np.concatenate((data[:3], get_quaternion_from_rpy(data[3:])))  # (7,)
+
+        # write poses
+        self.robot.set_link_positions(link_ids=self.link, positions=data[:3], orientations=data[3:])
+
+
+class LinkVelocityAction(LinkAction):  # TODO: multiple links
+    r"""Link world velocity action
+
+    Set the cartesian world velocity for the specified robot link.
+    """
+
+    def __init__(self, robot, link_id=-1, discrete_values=None):
+        """
+        Initialize the link world velocity action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N, 6], None): if provided, it represents the discrete values that the action
+                can take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
+        """
+        super(LinkVelocityAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=6)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = self.robot.get_link_world_velocities(link_ids=self.link)  # (6,)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        self.robot.set_link_velocities(link_ids=self.link, positions=data)
+
+
+class LinkVelocityChangeAction(LinkAction):  # TODO: multiple link
+    r"""Link world velocity change action
+
+    Set the cartesian world velocity for the specified robot link. Instead of specifying directly the desired
+    cartesian velocity, the amount of change in the current velocities is provided.
+    """
+
+    def __init__(self, robot, link_id=None, discrete_values=None):
+        """
+        Initialize the link world velocity change action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): link id. If -1, it represents the base.
+            discrete_values (np.array[N, 6], None): if provided, it represents the discrete values that the action
+                can take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
+        """
+        super(LinkVelocityChangeAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=6)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = np.zeros(6)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        data += self.robot.get_link_world_velocities(link_ids=self.link)
+        self.robot.set_link_velocities(link_ids=self.link, positions=data)
+
+
+class LinkForceAction(LinkAction):  # TODO: multiple links
     r"""Link force action
 
-    Set the cartesian force(s) for the specified robot link(s).
+    Set the robot joint torques in order to perform a desired cartesian force(s) with the specified robot link on
+    the environment. The final joint torques that are applied are:
+
+    .. math:: \tau = N(q,\dot{q}) + J(q)^\top f
+
+    where :math:`N(q,\dot{q})` contains the coriolis, centrifugal, and gravity effects, and :math:`f` is the cartesian
+    force that we wish to apply on the environment.
     """
-    def __init__(self, robot, link_ids=None):
+
+    def __init__(self, robot, link_id, discrete_values=None):  # link_ids=None
         """
-        Initialize the link position action.
+        Initialize the link force action.
 
         Args:
             robot (Robot): robot instance
-            link_ids (int, int[N]): link id or list of link ids
+            link_id (int): id of the link that has to perform the desired force.
+            discrete_values (np.array[N, 3], None): if provided, it represents the discrete values that the action
+                can take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
         """
-        super(LinkForceAction, self).__init__(robot, link_ids)
+        super(LinkForceAction, self).__init__(robot, link_id, discrete_values=discrete_values)
 
-    def _write(self, data):
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=3)
+
+        # set the original data
+        if self.is_continuous():  # continuous action  # TODO: check if we can sense the forces
+            self.data = np.zeros(3)
+
+    def _write_continuous(self, data):
         """apply the action data on the robot."""
-        # self.robot
-        pass
+        jacobian = self.robot.get_jacobian(link_id=self.link)[:3]  # (3,N)
+        tau = self.robot.get_coriolis_and_gravity_compensation_torques()  # (N,)
+        tau += jacobian.T.dot(data)  # (N,)
+        self.robot.set_joint_torques(tau)
+
+
+class LinkTorqueAction(LinkAction):  # TODO: multiple links
+    r"""Link torque action
+
+    Set the robot joint torques in order to perform a desired cartesian torque with the specified link on the
+    environment. The final joint torques that are applied are:
+
+    .. math:: \tau = N(q,\dot{q}) + J(q)^\top f
+
+    where :math:`N(q,\dot{q})` contains the coriolis, centrifugal, and gravity effects, and :math:`f` is the cartesian
+    torque that we wish to apply on the environment.
+    """
+
+    def __init__(self, robot, link_id, discrete_values=None):  # link_ids=None
+        """
+        Initialize the link torque action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): id of the link that has to perform the desired torque.
+            discrete_values (np.array[N, 3], None): if provided, it represents the discrete values that the action
+                can take. Note that the action is no more continuous and becomes discrete at that point. The first
+                value will be the default value to be set if no data is provided.
+        """
+        super(LinkTorqueAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=3)
+
+        # set the original data
+        if self.is_continuous():  # continuous action  # TODO: check if we can sense the torques
+            self.data = np.zeros(3)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        jacobian = self.robot.get_jacobian(link_id=self.link)[3:]  # (3,N)
+        tau = self.robot.get_coriolis_and_gravity_compensation_torques()  # (N,)
+        tau += jacobian.T.dot(data)  # (N,)
+        self.robot.set_joint_torques(tau)
+
+
+class LinkWrenchAction(LinkAction):  # TODO: multiple links
+    r"""Link wrench action
+
+    Set the robot joint torques in order to perform a desired cartesian wrench (concatenation of the cartesian force
+    and torque) with the specified link on the environment.  The final joint torques that are applied are:
+
+    .. math:: \tau = N(q,\dot{q}) + J(q)^\top f
+
+    where :math:`N(q,\dot{q})` contains the coriolis, centrifugal, and gravity effects, and :math:`f` is the cartesian
+    wrench that we wish to apply on the environment.
+    """
+
+    def __init__(self, robot, link_id, discrete_values=None):  # link_ids=None
+        """
+        Initialize the link wrench action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): id of the link that has to perform the desired wrench.
+            discrete_values (np.array[N,6], None): if provided, it represents the discrete values that the
+                action can take. Note that the action is no more continuous and becomes discrete at that point.
+                The first value will be the default value to be set if no data is provided.
+        """
+        super(LinkWrenchAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check discrete values
+        self._check_discrete_values(dim=2, last_dim=6)
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            self.data = np.zeros(6)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        jacobian = self.robot.get_jacobian(link_id=self.link)  # (6,N)
+        tau = self.robot.get_coriolis_and_gravity_compensation_torques()  # (N,)
+        tau += jacobian.T.dot(data)  # (N,)
+        self.robot.set_joint_torques(tau)
+
+
+class ApplyForceAction(LinkAction):  # TODO: multiple links
+    r"""Apply Force Action
+
+    This action allows you to apply a Cartesian force on a specific link. In the simulator, it just applies the force
+    on the specified link at the specified position. On the real platform, it projects the Cartesian force to joint
+    torques and apply them on the robot.
+    """
+
+    def __init__(self, robot, link_id=-1, local_position=None, axis=None, discrete_values=None):  # link_ids=None
+        """
+        Initialize the apply force action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): id of the link on which to apply the force. If -1, it is the base.
+            local_position (np.array[3], list of 3 float, None): local position on the link to apply the force on.
+                If None, the force will be applied on the CoM of the link.
+            axis (np.array[3], None): axis on which to apply the force. If provided, it will create an action that
+                only represents the magnitude of the force.
+            discrete_values (np.array[N,3], np.array[N], None): if provided, it represents the forces in a discrete
+                manner by using the provided force values. Note that the action is no more continuous and becomes
+                discrete at that point. The first value will be the default value to be set if no data is provided.
+        """
+        super(ApplyForceAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check local position
+        if local_position is not None:
+            if not isinstance(local_position, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, or None, "
+                                "but instead got: {}".format(type(local_position)))
+            if len(local_position) != 3:
+                raise ValueError("Expecting the given 'local_position' to be a list/tuple/np.array of 3 float, but "
+                                 "instead got a length of: {}".format(len(local_position)))
+        self.local_position = local_position
+
+        # check axis
+        if axis is not None:
+            if not isinstance(axis, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'axis' to be a list/tuple/np.array, but instead got: "
+                                "{}".format(type(axis)))
+            axis = np.asarray(axis)
+            if axis.size != 3:
+                raise ValueError("Expecting the given 'axis' to be list/tuple/np.array of 3 float, but instead got: "
+                                 "{}".format(axis.size))
+        self.axis = axis
+
+        # check discrete values
+        if self.discrete_values is not None:
+            # if an axis is not defined the discrete values must have a shape of (N,2)
+            if self.axis is None:
+                if not (len(self.discrete_values.shape) == 2 and self.discrete_values.shape[1] == 3):
+                    raise ValueError("Expecting the discrete values to have a shape of (N,3), but instead got: "
+                                     "{}".format(self.discrete_values.shape))
+            else:  # if an axis is defined, the discrete values must have a shape of (N,)
+                if len(self.discrete_values.shape) > 1:
+                    raise ValueError("Expecting the discrete values to have a shape of (N,), but instead got: "
+                                     "{}".format(self.discrete_values.shape))
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            if self.axis is not None:  # if an axis is defined, then set the initial data to be zero
+                self.data = np.zeros(1)
+            else:  # if no axis is defined, set the initial data to be a 3D zero vector
+                self.data = np.zeros(3)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        if self.axis is not None:
+            data = data * self.axis
+        self.robot.apply_external_force(force=data, link_id=self.link, position=self.local_position)
+
+
+class ApplyTorqueAction(LinkAction):  # TODO: multiple links
+    r"""Apply Torque Action
+
+    This action allows you to apply a Cartesian torque on a specific link. In the simulator, it just applies the torque
+    on the specified link at the specified position. On the real platform, it projects the Cartesian torque to joint
+    torques and apply them on the robot.
+    """
+
+    def __init__(self, robot, link_id=-1, axis=None, discrete_values=None):  # link_ids=None
+        """
+        Initialize the apply torque action.
+
+        Args:
+            robot (Robot): robot instance.
+            link_id (int): id of the link on which to apply the torque. If -1, it is the base.
+            axis (np.array[3], None): axis around which to apply the torque. If provided, it will create an action that
+                only represents the magnitude of the torque.
+            discrete_values (np.array[N,3], np.array[N], None): if provided, it represents the torques in a discrete
+                manner by using the provided torque values. Note that the action is no more continuous and becomes
+                discrete at that point. The first value will be the default value to be set if no data is provided.
+        """
+        super(ApplyTorqueAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+
+        # check axis
+        if axis is not None:
+            if not isinstance(axis, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'axis' to be a list/tuple/np.array, but instead got: "
+                                "{}".format(type(axis)))
+            axis = np.asarray(axis)
+            if axis.size != 3:
+                raise ValueError("Expecting the given 'axis' to be list/tuple/np.array of 3 float, but instead got: "
+                                 "{}".format(axis.size))
+        self.axis = axis
+
+        # check discrete values
+        if self.discrete_values is not None:
+            # if an axis is not defined the discrete values must have a shape of (N,2)
+            if self.axis is None:
+                if not (len(self.discrete_values.shape) == 2 and self.discrete_values.shape[1] == 3):
+                    raise ValueError("Expecting the discrete values to have a shape of (N,3), but instead got: "
+                                     "{}".format(self.discrete_values.shape))
+            else:  # if an axis is defined, the discrete values must have a shape of (N,)
+                if len(self.discrete_values.shape) > 1:
+                    raise ValueError("Expecting the discrete values to have a shape of (N,), but instead got: "
+                                     "{}".format(self.discrete_values.shape))
+
+        # set the original data
+        if self.is_continuous():  # continuous action
+            if self.axis is not None:
+                # if an axis is defined, then set the initial data to be zero
+                self.data = np.zeros(1)
+            else:
+                self.data = np.zeros(3)
+
+    def _write_continuous(self, data):
+        """apply the action data on the robot."""
+        if self.axis is not None:
+            data = data * self.axis
+        self.robot.apply_external_torque(torque=data, link_id=self.link)
+
+
+# class ApplyWrenchAction(LinkAction):  # TODO: multiple links
+#     r"""Apply wrench action
+#
+#     This action allows you to apply a Cartesian wrench (concatenation of the force and torque) on a specific link.
+#     In the simulator, it just applies the wrench on the specified link at the specified position. On the real
+#     platform, it projects the Cartesian wrench to joint torques and apply them on the robot.
+#     """
+#
+#     def __init__(self, robot, link_id, local_position=None, axis=None, discrete_values=None):  # link_ids=None
+#         """
+#         Initialize the apply wrench action.
+#
+#         Args:
+#             robot (Robot): robot instance.
+#             link_id (int): id of the link on which to apply the force. If -1, it is the base.
+#             local_position (np.array[3], list of 3 float, None): local position on the link to apply the force on.
+#                 If None, the force will be applied on the CoM of the link.
+#             axis (np.array[3], None): axis on which to apply the force. If provided, it will create an action that
+#                 only represents the magnitude of the force.
+#             discrete_values (np.array[M]): if provided, it represents the forces in a discrete manner by using the
+#                 provided force value. Note that the action is no more continuous and becomes discrete at that point.
+#         """
+#         super(ApplyWrenchAction, self).__init__(robot, link_id, discrete_values=discrete_values)
+#
+#     def _write(self, data):
+#         """apply the action data on the robot."""
+#         pass
 
 
 ########################
 # End Effector Actions #
 ########################
 
-class EndEffectorAction(LinkAction):
-
-    def __init__(self, robot, end_effector_ids=None):
-        if end_effector_ids is None:
-            end_effector_ids = robot.get_end_effector_ids()
-        super(EndEffectorAction, self).__init__(robot, end_effector_ids)
-
-
-class EndEffectorPositionAction(EndEffectorAction):
-
-    def __init__(self, robot, end_effector_ids=None):
-        super(EndEffectorPositionAction, self).__init__(robot, end_effector_ids)
-
-
-class EndEffectorVelocityAction(EndEffectorAction):
-
-    def __init__(self, robot, end_effector_ids=None):
-        super(EndEffectorVelocityAction, self).__init__(robot, end_effector_ids)
-
-
-class EndEffectorForceAction(EndEffectorAction):
-
-    def __init__(self, robot, end_effector_ids=None):
-        super(EndEffectorForceAction, self).__init__(robot, end_effector_ids)
+# class EndEffectorAction(LinkAction):
+#
+#     def __init__(self, robot, end_effector_ids=None):
+#         if end_effector_ids is None:
+#             end_effector_ids = robot.get_end_effector_ids()
+#         super(EndEffectorAction, self).__init__(robot, end_effector_ids)
+#
+#
+# class EndEffectorPositionAction(EndEffectorAction):
+#
+#     def __init__(self, robot, end_effector_ids=None):
+#         super(EndEffectorPositionAction, self).__init__(robot, end_effector_ids)
+#
+#
+# class EndEffectorVelocityAction(EndEffectorAction):
+#
+#     def __init__(self, robot, end_effector_ids=None):
+#         super(EndEffectorVelocityAction, self).__init__(robot, end_effector_ids)
+#
+#
+# class EndEffectorForceAction(EndEffectorAction):
+#
+#     def __init__(self, robot, end_effector_ids=None):
+#         super(EndEffectorForceAction, self).__init__(robot, end_effector_ids)

pyrobolearn/actions/robot_actions/robot_actions.py

@@ -34,20 +34,31 @@ class RobotAction(Action):
     __metaclass__ = ABCMeta
 
     def __init__(self, robot):
+        """Initialize the abstract robot action.
+
+        Args:
+            robot (Robot): a robot instance.
+        """
         super(RobotAction, self).__init__()
+
+        # check robot instance
         if not isinstance(robot, Robot):
-            raise TypeError("The 'robot' parameter has to be an instance of Robot")
+            raise TypeError("The 'robot' parameter has to be an instance of Robot, but instead got: "
+                            "{}".format(type(robot)))
         self._robot = robot
 
     @property
     def robot(self):
+        """Return the robot instance."""
         return self._robot
 
-    def is_discrete(self):
-        return False
-
-    def is_continuous(self):
-        return True
+    # def is_discrete(self):
+    #     """By default, robot actions are continuous."""
+    #     return False
+    #
+    # def is_continuous(self):
+    #     """By default, robot actions are continuous."""
+    #     return True
 
     def __copy__(self):
         """Return a shallow copy of the action. This can be overridden in the child class."""

pyrobolearn/actions/world_actions.py

@@ -0,0 +1,209 @@
+#!/usr/bin/env python
+"""Define world actions
+
+This includes:
+
+- AttachAction: this allows you to attach / detach a link with another link.
+"""
+
+from abc import ABCMeta
+import copy
+import numpy as np
+
+from pyrobolearn.actions.action import Action
+from pyrobolearn.robots.base import Body
+from pyrobolearn.worlds.world import World
+
+
+__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 WorldAction(Action):
+    r"""World action (abstract)
+
+    This provides the abstract class that allows to perform an action in the world. This includes to attach or detach
+    two bodies, and others.
+    """
+    __metaclass__ = ABCMeta
+
+    def __init__(self, world):
+        """
+        Initialize the world action.
+
+        Args:
+            world (World): world instance.
+        """
+        super(WorldAction, self).__init__()
+        self.world = world
+
+    @property
+    def world(self):
+        """Return the world instance."""
+        return self._world
+
+    @world.setter
+    def world(self, world):
+        """Set the world instance."""
+        self._world = world
+
+    def __copy__(self):
+        """Return a shallow copy of the action. This can be overridden in the child class."""
+        return self.__class__(world=self.world)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the action. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass
+        """
+        if self in memo:
+            return memo[self]
+        world = copy.deepcopy(self.world)
+        action = self.__class__(world=world)
+        memo[self] = action
+        return action
+
+
+class AttachAction(WorldAction):
+    r"""Attach Action.
+
+    The attach action is a discrete action which can take two values: 0 (=detach) or 1 (=attach). This allows to
+    attach a robot's link with another body's link in the world. Note that is only valid in the simulator. In order to
+    attach the robot's link with the other link, they both have to be close to each other.
+
+    Warnings:
+        - This is only valid in the simulator.
+        - Currently, the other link id to which we would like to attach has to be provided.
+    """
+
+    def __init__(self, world, body1, body2, link_id1=-1, link_id2=-1, distance_threshold=0.1,
+                 body1_frame_position=(0., 0., 0.), body2_frame_position=(0., 0., 0.),
+                 body1_frame_orientation=None, body2_frame_orientation=None):
+        """
+        Initialize the attach action.
+
+        Args:
+            world (World): world instance.
+            body1 (Body): first body instance.
+            body2 (Body): second body instance.
+            link_id1 (int): unique link id of the first body instance.
+            link_id2 (int): unique link id of the second body instance.
+            distance_threshold (float): distance threshold between the two links such that they can be attached.
+            body1_frame_position (np.array[3]): position of the joint frame relative to parent CoM frame.
+            body2_frame_position (np.array[3]): position of the joint frame relative to a given child CoM frame (or
+                world origin if no child specified)
+            body1_frame_orientation (np.array[4]): the orientation of the joint frame relative to parent CoM
+                coordinate frame (expressed as a quaternion [x,y,z,w])
+            body2_frame_orientation (np.array[4]): the orientation of the joint frame relative to the child CoM
+                coordinate frame, or world origin frame if no child specified (expressed as a quaternion [x,y,z,w])
+        """
+        super(AttachAction, self).__init__(world=world)
+
+        # check body instances
+        def check_body(body, name):
+            if not isinstance(body, Body):
+                raise TypeError("Expecting the given '" + name + "' to be an instance of `Body`, but got instead: "
+                                "{}".format(type(body)))
+            return body
+
+        self._body1 = check_body(body1, 'body1')
+        self._body2 = check_body(body2, 'body2')
+
+        # check links
+        def check_link(link, name):
+            if link is None:
+                link = -1
+            if not isinstance(link, int):
+                raise TypeError("Expecting the given '" + name + "' to be an int, but got instead: "
+                                                                 "{}".format(type(link)))
+            return link
+
+        self._link1 = check_link(link_id1, 'link_id1')
+        self._link2 = check_link(link_id2, 'link_id2')
+
+        # check distance threshold
+        if not isinstance(distance_threshold, (float, int)):
+            raise TypeError("Expecting the given 'distance_threshold' to be a float or int, but got instead: "
+                            "{}".format(type(distance_threshold)))
+        if distance_threshold < 0:
+            raise ValueError("The given 'distance_threshold' should be a positive number.")
+        self._distance_threshold = distance_threshold
+
+        # set other variables
+        self._body1_frame_position = body1_frame_position
+        self._body2_frame_position = body2_frame_position
+        self._body1_frame_orientation = body1_frame_orientation
+        self._body2_frame_orientation = body2_frame_orientation
+
+        # variable to remember if they are already attached or not
+        self._attached = False
+
+    def _write(self, data):
+        """
+        Write the data.
+
+        Args:
+            data (int, np.ndarray): the binary data; 0 = detach and 1 = attach.
+        """
+        # get data
+        if isinstance(data, np.ndarray):
+            data = data[0]
+
+        if data == 1:  # attach
+            if not self._attached:  # if not already attached
+
+                # check distance
+                results = self.world.get_closest_bodies(body=self._body1, radius=self._distance_threshold,
+                                                        link_id=self._link1, body2=self._body2, link2_id=self._link2)
+
+                # if found body2 in close vicinity of body1
+                if len(results) > 0:
+                    self.world.attach(body1=self._body1, body2=self._body2, link1=self._link1, link2=self._link2,
+                                      parent_frame_position=self._body1_frame_position,
+                                      child_frame_position=self._body2_frame_position,
+                                      parent_frame_orientation=self._body1_frame_orientation,
+                                      child_frame_orientation=self._body2_frame_orientation)
+                    self._attached = not self._attached
+
+        else:  # detach
+            if self._attached:  # if attached
+                self.world.detach(body1=self._body1, body2=self._body2, link1=self._link1, link2=self._link2)
+                self._attached = not self._attached
+
+    def __copy__(self):
+        """Return a shallow copy of the action. This can be overridden in the child class."""
+        return self.__class__(world=self.world, body1=self._body1, body2=self._body2, link_id1=self._link1,
+                              link_id2=self._link2, distance_threshold=self._distance_threshold,
+                              body1_frame_position=self._body1_frame_position,
+                              body2_frame_position=self._body2_frame_position,
+                              body1_frame_orientation=self._body1_frame_orientation,
+                              body2_frame_orientation=self._body2_frame_orientation)
+
+    def __deepcopy__(self, memo={}):
+        """Return a deep copy of the action. This can be overridden in the child class.
+
+        Args:
+            memo (dict): memo dictionary of objects already copied during the current copying pass
+        """
+        if self in memo:
+            return memo[self]
+        world = copy.deepcopy(self.world)
+        body1 = copy.deepcopy(self._body1)
+        body2 = copy.deepcopy(self._body2)
+        body1_frame_position = copy.deepcopy(self._body1_frame_position)
+        body2_frame_position = copy.deepcopy(self._body2_frame_position)
+        body1_frame_orientation = copy.deepcopy(self._body1_frame_orientation)
+        body2_frame_orientation = copy.deepcopy(self._body2_frame_orientation)
+        action = self.__class__(world=world, body1=body1, body2=body2, link_id1=self._link1, link_id2=self._link2,
+                                body1_frame_position=body1_frame_position, body2_frame_position=body2_frame_position,
+                                body1_frame_orientation=body1_frame_orientation,
+                                body2_frame_orientation=body2_frame_orientation)
+        memo[self] = action
+        return action

pyrobolearn/algos/evaluator.py

@@ -5,6 +5,7 @@ The evaluator assesses the quality of the actions/trajectories performed by the
 It is the step performed after the exploration phase, and before the update step.
 """
 
+import torch
 from pyrobolearn.returns import Estimator
 
 __author__ = "Brian Delhaisse"
@@ -76,17 +77,45 @@ class Evaluator(object):
         Evaluate the trajectories performed by the policy.
 
         Args:
-            verbose (bool): If true, print information on the standard output.
+            verbose (int, bool): verbose level, select between {0=False, 1=True, 2}. If 1 or 2, it will print
+                information about the evaluation process. The level 2 will print more detailed information. Do not use
+                it when the states / actions are big or high dimensional, as it could be very hard to make sense of
+                the data.
         """
         if self.estimator is not None:
             if verbose:
-                print("\n#### Starting the Evaluation phase ####")
+                print("\n#### 2. Starting the Evaluation phase ####")
+                print("Using estimator: {}".format(self.estimator))
 
             # compute the returns
             returns = self.estimator.evaluate(self.storage)
 
-            if verbose:
+            if verbose > 1:
                 # print("Returns: {}".format(returns))
+
+                print("\nFinal storage status: ")
+                states = self.storage['states'][0]
+                num_step, num_traj = states.shape[:2]
+                states = states.view(-1, *states.size()[2:])
+                print("states: {}".format(torch.cat((torch.Tensor(list(range(num_step)) * num_traj).view(-1, 1),
+                                                     states), dim=1)))
+                actions = self.storage['actions'][0]
+                actions = actions.view(-1, *actions.size()[2:])
+                print("actions: {}".format(torch.cat((torch.Tensor(list(range(num_step - 1)) * num_traj).view(-1, 1),
+                                                      actions), dim=1)))
+                rewards = self.storage['rewards'][:, :, 0]
+                print("rewards: {}".format(torch.cat((torch.arange(len(rewards), dtype=torch.float).view(-1, 1),
+                                                      rewards), dim=1)))
+                masks = self.storage['masks'][:, :, 0]
+                print("masks: {}".format(torch.cat((torch.arange(len(masks), dtype=torch.float).view(-1, 1),
+                                                    masks), dim=1)))
+                returns = self.storage[self.estimator][:, :, 0]
+                print("returns: {}".format(torch.cat((torch.arange(len(returns), dtype=torch.float).view(-1, 1),
+                                                      returns), dim=1)))
+
+                print("\n#### End of the Evaluation phase ####")
+
+            elif verbose:
                 print("#### End of the Evaluation phase ####")
 
     #############

pyrobolearn/algos/explorer.py

@@ -20,6 +20,7 @@ from pyrobolearn.exploration import Exploration
 from pyrobolearn.storages import RolloutStorage, ExperienceReplay
 
 from pyrobolearn import logger
+from pyrobolearn.metrics import Metric
 
 
 __author__ = "Brian Delhaisse"
@@ -46,7 +47,7 @@ class Explorer(object):
     storage unit.
     """
 
-    def __init__(self, task, explorer, storage, num_workers=1, backend='multiprocessing'):
+    def __init__(self, task, explorer, storage, num_workers=1, backend='multiprocessing', metrics=None):
         """
         Initialize the exploration phase.
 
@@ -64,6 +65,7 @@ class Explorer(object):
                 PyTorch has been built from source with MPI support). For more information, we refer the reader to
                 references [2,4]. If the backend is 'mpi', you have to run the code using the following command:
                 `mpirun -n 4 python <code>.py`.
+            metrics ((list of) Metric, None): metrics that are used to evaluate the algorithm.
 
         References:
             [1] Multiprocessing best practices: https://pytorch.org/docs/stable/notes/multiprocessing.html
@@ -74,6 +76,7 @@ class Explorer(object):
         self.task = task
         self.explorer = explorer
         self.storage = storage
+        self.metrics = metrics
 
         # check the number of workers
         if not isinstance(num_workers, (int, long)):
@@ -214,6 +217,31 @@ class Explorer(object):
                             "instead got: {}".format(type(storage)))
         self._storage = storage
 
+    @property
+    def metrics(self):
+        """Return the metric instances."""
+        return self._metrics
+
+    @metrics.setter
+    def metrics(self, metrics):
+        """Set the metrics."""
+        # check metrics type
+        if metrics is None:
+            metrics = []
+        elif isinstance(metrics, Metric):
+            metrics = [metrics]
+        elif not isinstance(metrics, list):
+            raise TypeError("Expecting the given 'metrics' to be an instance of `Metric` or a list of `Metric`, but "
+                            "got instead: {}".format(type(metrics)))
+
+        # check each metric type
+        for i, metric in enumerate(metrics):
+            if not isinstance(metric, Metric):
+                raise TypeError("The {}th metric is not an instance of `Metric`, but: {}".format(i, type(metric)))
+
+        # set metrics
+        self._metrics = metrics
+
     ###########
     # Methods #
     ###########
@@ -523,17 +551,23 @@ class Explorer(object):
             num_rollouts (int): number of trajectories/rollouts (only valid in the on-policy case).
             deterministic (bool): if deterministic is True, then it does not explore in the environment.
             render (bool): if we should render the environment.
-            verbose (bool): If true, print information on the standard output.
+            verbose (int, bool): verbose level, select between {0=False, 1=True, 2}. If 1 or 2, it will print
+                information about the exploration process. The level 2 will print more detailed information. Do not use
+                it when the states / actions are big or high dimensional, as it could be very hard to make sense of
+                the data.
 
         Returns:
             DictStorage: updated memory storage
         """
+        if verbose:
+            print("\n#### 1. Starting the Exploration phase ####")
+
         for rollout in range(num_rollouts):
             # reset environment
             observation = self.env.reset()
 
             if verbose:
-                print("Start rollout: {}/{}".format(rollout + 1, num_rollouts))
+                print("\nStart rollout: {}/{}".format(rollout + 1, num_rollouts))
                 # print("Explorer - initial state: {}".format(observation))
 
             # reset storage
@@ -543,6 +577,7 @@ class Explorer(object):
             self.explorer.reset()
 
             # run RL task for T steps
+            step = 0
             for step in range(num_steps):
                 # if we need to render
                 if render:
@@ -578,7 +613,8 @@ class Explorer(object):
             self.storage.end(rollout)
 
             if verbose:
-                print("End rollout: {}/{}".format(rollout + 1, num_rollouts))
+                print("End rollout: {}/{} with performed step: {}/{}".format(rollout + 1, num_rollouts,
+                                                                             step + 1, num_steps))
                 # print("states: {}".format(self.storage['states']))
                 # print("actions: {}".format(self.storage['actions']))
                 # print("rewards: {}".format(self.storage['rewards']))
@@ -588,6 +624,9 @@ class Explorer(object):
             # # clear explorer
             # self.explorer.clear()
 
+        if verbose:
+            print("\n#### End of the Exploration phase ####")
+
         # return storage unit
         return self.storage
 

pyrobolearn/algos/reinforce.py

@@ -15,7 +15,7 @@ from pyrobolearn.actorcritics import ActorCritic
 from pyrobolearn.exploration import ActionExploration
 
 from pyrobolearn.storages import RolloutStorage
-from pyrobolearn.samplers import StorageSampler
+from pyrobolearn.samplers import BatchRandomSampler
 from pyrobolearn.returns import ActionRewardEstimator, PolicyEvaluator
 from pyrobolearn.losses import PGLoss, ValueL2Loss
 from pyrobolearn.optimizers import Adam
@@ -132,15 +132,17 @@ class REINFORCE(GradientRLAlgo):
 
 
     References:
-        [1] "Simple Statistical Gradient-Following Algorithms for Connectionist Reinforcement Learning", Williams, 1992
-        [2] "Policy Gradient Methods", Peters, 2010 (Scholarpedia)
-        [3] "A Survey on Policy Search for Robotics", Deisenroth et al., 2013
-        [4] PyTorch Reinforce: https://github.com/pytorch/examples/blob/master/reinforcement_learning/reinforce.py
-        [5] OpenAI - Spinning Up: https://spinningup.openai.com/en/latest/algorithms/vpg.html
-        [6] "Policy Gradient Algorithms":
-            https://lilianweng.github.io/lil-log/2018/04/08/policy-gradient-algorithms.html
+        - [1] "Simple Statistical Gradient-Following Algorithms for Connectionist Reinforcement Learning", Williams,
+          1992
+        - [2] "Policy Gradient Methods", Peters, 2010 (Scholarpedia)
+        - [3] "A Survey on Policy Search for Robotics", Deisenroth et al., 2013
+        - [4] PyTorch Reinforce: https://github.com/pytorch/examples/blob/master/reinforcement_learning/reinforce.py
+        - [5] OpenAI - Spinning Up: https://spinningup.openai.com/en/latest/algorithms/vpg.html
+        - [6] "Policy Gradient Algorithms":
+          https://lilianweng.github.io/lil-log/2018/04/08/policy-gradient-algorithms.html
 
     Other implementations:
+
     - https://github.com/rll/rllab/blob/master/rllab/algos/vpg.py
     - https://github.com/pytorch/examples/blob/master/reinforcement_learning/reinforce.py
     - https://github.com/JamesChuanggg/pytorch-REINFORCE
@@ -149,7 +151,7 @@ class REINFORCE(GradientRLAlgo):
     - https://github.com/rlcode/reinforcement-learning/blob/master/2-cartpole/3-reinforce/cartpole_reinforce.py
     """
 
-    def __init__(self, task, approximators, gamma=0.99, lr=0.001, num_workers=1):
+    def __init__(self, task, approximators, gamma=0.99, lr=0.001, num_batches=10, batch_size=10, num_workers=1):
         """
         Initialize the REINFORCE on-policy RL algorithm.
 
@@ -190,7 +192,7 @@ class REINFORCE(GradientRLAlgo):
         states, actions = policy.states, policy.actions
         storage = RolloutStorage(num_steps=1000, state_shapes=states.merged_shape, action_shapes=actions.merged_shape,
                                  num_trajectories=1)
-        sampler = StorageSampler(storage)
+        sampler = BatchRandomSampler(storage, num_batches=10, batch_size_bounds=(8, 64))
 
         # create return: R_t = \sum_{t'=t}^{T} \gamma^{t'-t} r_{t'}
         returns = ActionRewardEstimator(storage, gamma=gamma)
@@ -218,7 +220,7 @@ class REINFORCE(GradientRLAlgo):
         updater = Updater(approximators, sampler, loss, optimizer, evaluators=[policy_evaluator])
 
         # initialize RL algorithm
-        super(REINFORCE, self).__init__(explorer, evaluator, updater)
+        super(REINFORCE, self).__init__(explorer, evaluator, updater, )
 
 
 # alias

pyrobolearn/algos/rl_algo.py

@@ -15,6 +15,8 @@ from pyrobolearn.algos.explorer import Explorer
 from pyrobolearn.algos.evaluator import Evaluator
 from pyrobolearn.algos.updater import Updater
 
+from pyrobolearn.metrics import Metric
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -148,7 +150,7 @@ class RLAlgo(object):  # Algo):
         [5] OpenAI - Spinning Up: https://spinningup.openai.com/
     """
 
-    def __init__(self, explorer, evaluator, updater, dynamic_model=None):
+    def __init__(self, explorer, evaluator, updater, dynamic_model=None, metrics=None):
         """
         Initialize the reinforcement learning algorithm.
 
@@ -157,6 +159,7 @@ class RLAlgo(object):  # Algo):
             evaluator (Evaluator): evaluate the actions
             updater (Updater): update the approximators (rl, value-functions,...)
             dynamic_model (None): dynamical model
+            metrics ((list of) Metric, None): metrics that are used to evaluate the algorithm.
         """
 
         super(RLAlgo, self).__init__()
@@ -172,6 +175,8 @@ class RLAlgo(object):  # Algo):
         self.best_reward = -np.infty
         self.best_parameters = None
 
+        self.metrics = metrics
+
     ##############
     # Properties #
     ##############
@@ -265,6 +270,31 @@ class RLAlgo(object):  # Algo):
         """Return the losses."""
         return self.updater.losses
 
+    @property
+    def metrics(self):
+        """Return the metric instances."""
+        return self._metrics
+
+    @metrics.setter
+    def metrics(self, metrics):
+        """Set the metrics."""
+        # check metrics type
+        if metrics is None:
+            metrics = []
+        elif isinstance(metrics, Metric):
+            metrics = [metrics]
+        elif not isinstance(metrics, list):
+            raise TypeError("Expecting the given 'metrics' to be an instance of `Metric` or a list of `Metric`, but "
+                            "got instead: {}".format(type(metrics)))
+
+        # check each metric type
+        for i, metric in enumerate(metrics):
+            if not isinstance(metric, Metric):
+                raise TypeError("The {}th metric is not an instance of `Metric`, but: {}".format(i, type(metric)))
+
+        # set metrics
+        self._metrics = metrics
+
     ###########
     # Methods #
     ###########
@@ -298,7 +328,7 @@ class RLAlgo(object):  # Algo):
     #     self.evaluator = evaluator
     #     self.updater = updater
 
-    def train(self, num_steps, num_rollouts=1, num_episodes=1, verbose=False, seed=None):
+    def train(self, num_steps, num_rollouts=1, num_episodes=1, verbose=0, seed=None):
         """
         Train the policy in the provided environment.
 
@@ -306,11 +336,14 @@ class RLAlgo(object):  # Algo):
             num_steps (int): number of step per rollout/trajectory
             num_rollouts (int): number of rollouts/trajectories per episode (default: 1)
             num_episodes (int): number of episodes (default: 1)
-            verbose (bool): if True, print details about the optimization process
+            verbose (int, bool): verbose level, select between {0=False, 1=True, 2}. If 1 or 2, it will print
+                information about the training process. The level 2 will print more detailed information. Do not use
+                it when the states / actions are big or high dimensional, as it could be very hard to make sense of
+                the data.
             seed (int): random seed
 
         Returns:
-            dict: history
+           Metric, list of Metric: metric instance(s).
         """
         history = {}
 
@@ -323,9 +356,16 @@ class RLAlgo(object):  # Algo):
         # set the policy in training mode
         self.policy.train()
 
+        # compute metrics  # TODO
+
         # for each episode
         for episode in range(num_episodes):
 
+            if verbose:
+                print("\n#####################")
+                print("#### Episode {}/{} ####".format(episode+1, num_episodes))
+                print("#####################")
+
             # # for each rollout
             # for rollout in range(num_rollouts):
             #     # TODO: consider to learn the dynamic model if provided
@@ -344,16 +384,25 @@ class RLAlgo(object):  # Algo):
             # 3. update
             losses = self.updater.update(verbose=verbose)
 
+            # compute metrics
+            for metric in self.metrics:
+                metric.end_episode_update(episode_idx=episode, num_episodes=num_episodes)
+
             # add the loss in the history
             history.setdefault('losses', []).append(losses)
 
         # set the policy in test mode
         self.policy.eval()
 
+        # compute metrics  # TODO
+
         if verbose:
             print("\n#### End of the RL algo ####")
 
-        return history
+        # return history
+        if len(self.metrics) == 1:
+            return self.metrics[0]
+        return self.metrics
 
     def test(self, num_steps, dt=0., use_terminating_condition=False, render=True):  # , storage):
         """
@@ -400,13 +449,24 @@ class GradientRLAlgo(RLAlgo):
         TD residual,...)
     """
 
-    def __init__(self, explorer, evaluator, updater, dynamic_model=None):  # hyperparameters=None)
-        super(GradientRLAlgo, self).__init__(explorer, evaluator, updater, dynamic_model)
+    def __init__(self, explorer, evaluator, updater, dynamic_model=None, metrics=None):  # hyperparameters=None)
+        """
+        Initialize the gradient reinforcement learning algorithm.
+
+        Args:
+            explorer (Explorer): explorer that specifies how to explore in the environment
+            evaluator (Evaluator): evaluate the actions
+            updater (Updater): update the approximators (rl, value-functions,...)
+            dynamic_model (None): dynamical model
+            metrics ((list of) Metric, None): metrics that are used to evaluate the algorithm.
+        """
+        super(GradientRLAlgo, self).__init__(explorer, evaluator, updater, dynamic_model=dynamic_model, metrics=metrics)
 
 
 class EMRLAlgo(RLAlgo):
     r"""Expectation-Maximization reinforcement learning algorithm.
     """
 
-    def __init__(self, task, exploration_strategy, storage, dynamic_model=None):  # hyperparameters=None)
-        super(EMRLAlgo, self).__init__(task, exploration_strategy, storage, dynamic_model)
+    def __init__(self, task, exploration_strategy, storage, dynamic_model=None, metrics=None):  # hyperparameters=None)
+        super(EMRLAlgo, self).__init__(task, exploration_strategy, storage, dynamic_model=dynamic_model,
+                                       metrics=metrics)

pyrobolearn/algos/updater.py

@@ -2,13 +2,13 @@
 """Provide the Updater class used in the third and final step of RL algorithms
 
 The updater update the approximator (such as the policy and/or value function) parameters based on the loss, and
-using the specified optmizer.
+using the specified optimizer.
 
 Dependencies:
 - `pyrobolearn/approximators`: models (which contain parameters to update)
 - `pyrobolearn/losses`: to compute the loss
 - `pyrobolearn/optimizers`: the optimizers used to update the model parameters
-- `pyrobolearn/samplers`:
+- `pyrobolearn/samplers`: to sample from batches
 """
 
 import collections
@@ -27,6 +27,8 @@ from pyrobolearn.samplers import StorageSampler
 from pyrobolearn.returns import Return, Target, Evaluator
 from pyrobolearn.parameters.updater import ParameterUpdater
 
+from pyrobolearn.metrics import Metric
+
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
@@ -50,7 +52,8 @@ class Updater(object):
     This class focuses on the third step of RL algorithms.
     """
 
-    def __init__(self, approximators, sampler, losses, optimizers, evaluators=None, updaters=None, ticks=None):
+    def __init__(self, approximators, sampler, losses, optimizers, evaluators=None, updaters=None, ticks=None,
+                 metrics=None):
         """
         Initialize the update phase.
 
@@ -67,6 +70,7 @@ class Updater(object):
             ticks (None, dictionary): dictionary containing as the key (updater or loss) and the value are the number
                 of time steps to wait before updating the corresponding key. By default, it will evaluate the given
                 losses and updaters at each time step.
+            metrics ((list of) Metric, None): metrics that are used to evaluate the algorithm.
         """
         self.approximators = approximators
         self.sampler = sampler
@@ -75,6 +79,7 @@ class Updater(object):
         self.evaluators = evaluators
         self.updaters = updaters
         self.ticks = ticks
+        self.metrics = metrics
 
         # counter
         self._cnt = 0
@@ -227,7 +232,7 @@ class Updater(object):
             raise TypeError("Expecting the given ticks to be a dictionary, instead got: {}".format(type(ticks)))
 
         # check first the items already present in the ticks
-        for key, value in ticks.iteritems():
+        for key, value in ticks.items():
             # check that the key is a Loss or ParamaterUpdater
             if not isinstance(key, (Loss, ParameterUpdater)):
                 raise TypeError("Expecting the given key for the tick to be an instance of `Loss` or "
@@ -259,6 +264,31 @@ class Updater(object):
         # set the ticks
         self._ticks = ticks
 
+    @property
+    def metrics(self):
+        """Return the metric instances."""
+        return self._metrics
+
+    @metrics.setter
+    def metrics(self, metrics):
+        """Set the metrics."""
+        # check metrics type
+        if metrics is None:
+            metrics = []
+        elif isinstance(metrics, Metric):
+            metrics = [metrics]
+        elif not isinstance(metrics, list):
+            raise TypeError("Expecting the given 'metrics' to be an instance of `Metric` or a list of `Metric`, but "
+                            "got instead: {}".format(type(metrics)))
+
+        # check each metric type
+        for i, metric in enumerate(metrics):
+            if not isinstance(metric, Metric):
+                raise TypeError("The {}th metric is not an instance of `Metric`, but: {}".format(i, type(metric)))
+
+        # set metrics
+        self._metrics = metrics
+
     ###########
     # Methods #
     ###########
@@ -270,7 +300,10 @@ class Updater(object):
         Args:
             num_epochs (int): number of epochs.
             num_batches (int): number of batches.
-            verbose (bool): If true, print information on the standard output.
+            verbose (int, bool): verbose level, select between {0=False, 1=True, 2}. If 1 or 2, it will print
+                information about the update process. The level 2 will print more detailed information. Do not use
+                it when the states / actions are big or high dimensional, as it could be very hard to make sense of
+                the data.
 
         Returns:
             dict: dictionary of losses. There is a key for each loss, and the value is a nested list which contains
@@ -283,7 +316,7 @@ class Updater(object):
         losses = {}
 
         if verbose:
-            print("\n#### Starting the Update phase ####")
+            print("\n#### 3. Starting the Update phase ####")
 
         # for each epoch
         for epoch in range(num_epochs):
@@ -292,7 +325,8 @@ class Updater(object):
             for batch_idx, batch in enumerate(self.sampler):
 
                 if verbose:
-                    print("Epoch: {}/{} - Batch: {}/{}".format(epoch + 1, num_epochs, batch_idx + 1, num_batches))
+                    print("\nEpoch: {}/{} - Batch: {}/{} with size {}".format(epoch + 1, num_epochs, batch_idx + 1,
+                                                                              num_batches, batch.size))
 
                 # evaluate the evaluators with the current parameters on the given batch and save the results in the
                 # batch's `current` attribute
@@ -315,9 +349,8 @@ class Updater(object):
 
                         # append the loss value in the history of losses
                         if loss not in losses:
-                            losses[loss] = [[]] * num_epochs
-                        else:
-                            losses[loss][epoch].append(loss_value)
+                            losses[loss] = [[] for epoch in range(num_epochs)]
+                        losses[loss][epoch].append(loss_value.detach())
 
                         # update parameters
                         if verbose:
@@ -332,9 +365,20 @@ class Updater(object):
                                 print("\tRun updater {}".format(updater))
                             updater()
 
+                # compute metrics
+                for metric in self.metrics:
+                    metric.end_batch_update(batch_idx=batch_idx, num_batches=num_batches)
+
                 # increase counter
                 self._cnt += 1
 
+            # compute metrics
+            for metric in self.metrics:
+                metric.end_epoch_update(epoch_idx=epoch, num_epochs=num_epochs)
+
+        if verbose > 1:
+            print("Losses: {}".format(losses))
+
         if verbose:
             print("#### End of the Update phase ####")
 

pyrobolearn/control/README.md

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

pyrobolearn/controllers/README.md

@@ -1,5 +1,7 @@
 ## Controllers
 
+THIS IS UNDER CONSTRUCTION
+
 Controllers are basically policies that do not possess any (hyper-)parameters to optimize. They are manually coded by the user.
 
 Planning (TODO):

pyrobolearn/envs/control/README.rst

@@ -0,0 +1,2 @@
+Control Environments
+--------------------

pyrobolearn/envs/control/acrobot.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python
+"""Provide the acrobot environment.
+
+This is based on the control problem proposed in OpenAI Gym:
+"The acrobot system includes two joints and two links, where the joint between the two links is actuated. Initially,
+the links are hanging downwards, and the goal is to swing the end of the lower link up to a given height." [1]
+
+References:
+    - [1] Acrobot environment in OpenAI Gym: https://gym.openai.com/envs/Acrobot-v1/
+    - [2] "Generalization in Reinforcement Learning: Successful Examples Using Sparse Coarse Coding", Sutton, 1996.
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.control import ControlEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class AcrobotEnv(ControlEnv):
+    r"""Acrobot Environment
+
+    This is based on the control problem proposed in OpenAI Gym [1]:
+    "The acrobot system includes two joints and two links, where the joint between the two links is actuated.
+    Initially, the links are hanging downwards, and the goal is to swing the end of the lower link up to a given
+    height." [1]
+
+    Here are the various environment features:
+
+    - world: basic world with gravity enabled, a basic floor and the acrobot.
+    - state: the state is given by :math:`[cos(q_1), sin(q_1), cos(q_2), sin(q_2), \dot{q}_1, \dot{q}_2]`
+    - action: discrete joint torques :math:`\tau_2 \in \{-1., 0., +1.\}`
+    - reward: -1 if not terminal
+    - initial state generator: initialize uniformly the joint position and velocity states between [-0.1, 0.1]
+    - physics randomizer: uniform distribution of the mass of the  [mass - mass/10, mass + mass/10]
+    - terminal condition: if the end-effector link is above a certain height.
+
+    References:
+        - [1] Acrobot environment in OpenAI Gym: https://gym.openai.com/envs/Acrobot-v1/
+        - [2] "Generalization in Reinforcement Learning: Successful Examples Using Sparse Coarse Coding", Sutton, 1996.
+    """
+
+    def __init__(self, simulator=None, use_reward_shaping=False, verbose=False):
+        """
+        Initialize the acrobot environment.
+
+        Args:
+            simulator (Simulator): simulator instance. If None, by default, it will instantiate the Bullet
+                simulator.
+            use_reward_shaping (bool): if True, it will use a reward that guides how to achieve the goal.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet(render=verbose)
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+        robot = world.load_robot('acrobot')
+        robot.disable_motor()
+        if verbose:
+            robot.print_info()
+
+        # create state: [cos(q_1), sin(q_1), cos(q_2), sin(q_2), \dot{q}_1, \dot{q}_2]
+        trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+        velocity_state = prl.states.JointVelocityState(robot=robot)
+        state = trig_position_state + velocity_state
+        if verbose:
+            print("\nObservation: {}".format(state))
+
+        # create action: \tau_2 in {0., -1., +1.}
+        action = prl.actions.JointTorqueAction(robot, joint_ids=robot.joints[-1],
+                                               discrete_values=np.array([0., -1., +1.]))
+        if verbose:
+            print("\nAction: {}".format(action))
+
+        # create terminal condition:
+        terminal_condition = prl.terminal_conditions.LinkPositionCondition(robot, link_id=robot.joints[-1],
+                                                                           bounds=(2.5, np.infty), dim=2,
+                                                                           out=True, stay=False)
+
+        # create reward: -1 if not terminal
+        if use_reward_shaping:  # use continuous reward
+            # distance_cost = prl.rewards.DistanceCost()
+            # orientation_cost = prl.rewards.OrientationCost()
+            position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
+                                                          target_state=np.zeros(len(robot.joints)),
+                                                          update_state=True)
+            velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
+            torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot), update_state=True)
+            # reward = distance_cost + orientation_cost + 0.1 * velocity_cost + 0.01 * torque_cost
+            reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+        else:  # use discrete reward
+            reward = prl.rewards.TerminalReward(terminal_condition, subreward=-1., final_reward=0.)
+
+        # create initial state generator: generate the state each time we reset the environment
+        def reset_robot(robot):  # function to disable the motors every time we reset the joint state
+            def reset():
+                robot.disable_motor()
+            return reset
+
+        init_state = prl.states.JointPositionState(robot) + velocity_state
+        num_joints = len(robot.joints)
+        low = [[np.pi + 0.1] + [0]*(num_joints-1), [-0.1]*num_joints]
+        high = [[np.pi - 0.1] + [0]*(num_joints-1), [0.1]*num_joints]
+        initial_state_generator = prl.states.generators.UniformStateGenerator(state=init_state, low=low, high=high,
+                                                                              fct=reset_robot(robot))
+
+        # create physics randomizer: randomize the mass each time we reset the environment
+        masses = robot.get_link_masses(link_ids=robot.joints)
+        masses = (masses - masses / 10., masses + masses / 10.)
+        physics_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
+
+        # create environment using composition
+        super(AcrobotEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                         initial_state_generators=initial_state_generator,
+                                         physics_randomizers=physics_randomizer, terminal_conditions=terminal_condition)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = AcrobotEnv(sim, verbose=True)
+
+    # run simulation
+    env.reset()
+    for _ in count():
+        env.step(sleep_dt=1./240)

pyrobolearn/envs/control/cartpole.py

@@ -0,0 +1,197 @@
+#!/usr/bin/env python
+"""Provide the inverted pole on a cart (Cartpole) environment.
+
+This is based on the control problem proposed in OpenAI Gym:
+"A pole is attached by an un-actuated joint to a cart, which moves along a frictionless track. The system is
+controlled by applying a force of +1 or -1 to the cart. The pendulum starts upright, and the goal is to prevent it
+from falling over. A reward of +1 is provided for every timestep that the pole remains upright. The episode ends when
+the pole is more than 15 degrees from vertical, or the cart moves more than 2.4 units from the center." [1]
+
+Note that compared to [1], you can specify the number of links that forms the inverted pole.
+
+References:
+    - [1] Cartpole environment in OpenAI Gym: https://gym.openai.com/envs/CartPole-v1/
+    - [2] "Neuronlike Adaptive Elements That Can Solve Difficult Learning Control Problem", Barto et al., 1993.
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.control import ControlEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class CartpoleEnv(ControlEnv):
+    r"""Cartpole Environment
+
+    This is based on the control problem proposed in OpenAI Gym:
+    "A pole is attached by an un-actuated joint to a cart, which moves along a frictionless track. The system is
+    controlled by applying a force of +1 or -1 to the cart. The pendulum starts upright, and the goal is to prevent it
+    from falling over. A reward of +1 is provided for every timestep that the pole remains upright. The episode ends
+    when the pole is more than 15 degrees from vertical, or the cart moves more than 2.4 units from the center." [1]
+
+    Note that compared to [1], you can specify the number of links that forms the inverted pole.
+
+    Here are the various environment features (from [1]):
+
+    - world: basic world with gravity enabled, a basic floor and the cartpole.
+    - state: the state is given by :math:`[x, \dot{x}, q_1, \dot{q}_1]` for one inverted pole with one link.
+    - action: discrete forces applied on the cart (+10., -10.)
+    - reward: +1 until termination step
+    - initial state generator: initialize uniformly the state with [-0.05, 0.05]
+    - physics randomizer: uniform distribution of the mass of the  [mass - mass/10, mass + mass/10]
+    - terminal conditions:
+        - pole angle is more than 12 degrees
+        - cart position is more than 2.5m from the center
+        - episode length is greater than 200 steps
+
+    References:
+        - [1] Cartpole environment in OpenAI Gym: https://gym.openai.com/envs/CartPole-v1/
+        - [2] "Neuronlike Adaptive Elements That Can Solve Difficult Learning Control Problem", Barto et al., 1993.
+    """
+
+    def __init__(self, simulator=None, num_links=1, num_steps=200, verbose=True):
+        """
+        Initialize the Cartpole environment.
+
+        Args:
+            simulator (Simulator): simulator instance. If None, by default, it will instantiate the Bullet
+                simulator.
+            num_links (int): the number of links that forms the inverted pendulum.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet(render=verbose)
+
+        # create basic world
+        world = prl.worlds.World(simulator)
+        robot = prl.robots.CartPole(simulator, position=(0., 0., 0.), num_links=num_links, inverted_pole=False)
+        world.load_robot(robot)
+        robot.disable_motor(robot.joints)
+        if verbose:
+            robot.print_info()
+
+        # create state: [x, \dot{x}, q_i, \dot{q}_i]
+        state = prl.states.JointPositionState(robot) + prl.states.JointVelocityState(robot)
+        if verbose:
+            print("\nState: {}".format(state))
+
+        # create action: f_cart = (-10., +10.)
+        action = prl.actions.JointForceAction(robot=robot, joint_ids=0, discrete_values=[-10., 10.])
+        if verbose:
+            print("\nAction: {}".format(action))
+
+        # create terminal condition
+        pole_angle_condition = prl.terminal_conditions.JointPositionCondition(robot, joint_ids=1,
+                                                                              bounds=(-12 * np.pi/180, 12 * np.pi/180),
+                                                                              out=False, stay=True)
+        cart_position_condition = prl.terminal_conditions.LinkPositionCondition(robot, link_id=1, bounds=(-1., 1.),
+                                                                                dim=0, out=False, stay=True)
+        time_length_condition = prl.terminal_conditions.TimeLimitCondition(num_steps=num_steps)
+        terminal_conditions = [pole_angle_condition, cart_position_condition, time_length_condition]
+
+        # create reward: +1 until termination step
+        reward = prl.rewards.TerminalReward(terminal_conditions=terminal_conditions, subreward=1., final_reward=1.)
+        if verbose:
+            print("\nReward: {}".format(state))
+
+        # create initial state generator: generate the state each time we reset the environment
+        def reset_robot(robot):  # function to disable the motors every time we reset the joint state
+            def reset():
+                robot.disable_motor(robot.joints)
+            return reset
+
+        initial_state_generator = prl.states.generators.UniformStateGenerator(state=state, low=-0.05, high=0.05,
+                                                                              fct=reset_robot(robot))
+
+        # create environment using composition
+        super(CartpoleEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                          terminal_conditions=terminal_conditions,
+                                          initial_state_generators=initial_state_generator)
+
+
+# class CartDoublePoleEnv(Env):
+#     r"""CartDoublepole Environment
+#
+#     This provide the double inverted poles on a cart environment. Compare to the standard inverted pendulum on a cart,
+#     the goal this time is to balance two poles of possibly different lengths / masses, which are initialized at
+#     different angles but connected at the same joint attached to the cart.
+#     """
+#
+#     def __init__(self, simulator=None, pole_lengths=(1., 1.), pole_masses=(1., 1.), pole_angles=(0., 0.)):
+#         """
+#         Initialize the double inverted poles on a cart environment.
+#
+#         Args:
+#             simulator (Simulator): simulator instance.
+#         """
+#         # create basic world
+#         world = prl.worlds.BasicWorld(simulator)
+#         robot = prl.robots.CartDoublePole(simulator, pole_lengths=pole_lengths, pole_masses=pole_masses,
+#                                           pole_angles=pole_angles)
+#         world.load_robot(robot)
+#
+#         # create state
+#         state =
+#
+#         # create action
+#         action =
+#
+#         # create reward
+#         reward =
+#
+#         # create terminal condition
+#         terminal_condition =
+#
+#         # create initial state generator
+#         initial_state_generator =
+#
+#         # create environment using composition
+#         super(CartDoublePoleEnv, self).__init__(world=world, states=state, rewards=reward, actions=action)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = CartpoleEnv(sim)
+
+    state = env.reset()
+    # run simulation
+    for _ in count():
+        state, reward, done, info = env.step(sleep_dt=1./240)
+        print("done: {}, reward: {}, state: {}".format(done, reward, state))
+
+    # # create basic world
+    # sim = prl.simulators.Bullet()
+    # world = prl.worlds.World(sim)
+    # robot = prl.robots.CartPole(sim, num_links=1, inverted_pole=True)
+    # robot.disable_motor(robot.joints)
+    # world.load_robot(robot)
+    #
+    # # create state: [x, \dot{x}, q_i, \dot{q}_i]
+    # state = prl.states.JointPositionState(robot) + prl.states.JointVelocityState(robot)
+    #
+    # # create action
+    # action = prl.actions.JointForceAction(robot=robot, joint_ids=0, discrete_values=[-10., 10.])
+    #
+    # flip = 1
+    # for i in prl.count():
+    #     # if i % 10 == 0:
+    #     #     flip = (flip+1) % 2
+    #     action(flip)
+    #     world.step(sleep_dt=sim.dt)

pyrobolearn/envs/control/control.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+"""Provide the abstract control environment from which all the other control 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 ControlEnv(Env):
+    r"""Control Environment (abstract)
+
+    This is the abstract control environment from which all control 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 control 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(ControlEnv, 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/control/pendulum.py

@@ -0,0 +1,141 @@
+#!/usr/bin/env python
+"""Provide the inverted pendulum swing-up environment.
+
+This is based on the control problem proposed in OpenAI Gym:
+"The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the problem,
+the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
+
+References:
+    - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
+"""
+
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.control.control import ControlEnv
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["OpenAI", "Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class InvertedPendulumSwingUpEnv(ControlEnv):
+    r"""Inverted Pendulum Swing-up Environment
+
+    This is based on the control problem proposed in OpenAI Gym [1]:
+    "The inverted pendulum swingup problem is a classic problem in the control literature. In this version of the
+    problem, the pendulum starts in a random position, and the goal is to swing it up so it stays upright." [1]
+
+    Here are the various environment features:
+
+    - world: basic world with gravity, a basic floor, and the pendulum loaded at the center.
+    - state: the state is given by :math:`[cos(q_1), sin(q_1), \dot{q}_1]`
+    - action: the action is the joint torque :math:`\tau_1`
+    - cost: :math:`||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2`, where :math:`d(\cdot, \cdot)`
+      is the minimum angle difference between two angles.
+    - initial state generator: initialize the joint angle between [-pi, pi] (q=0 when the pendulum is pointing up)
+    - physics randomizer: uniform distribution of the mass of the pendulum [mass - mass/10, mass + mass/10]
+
+    References:
+        - [1] Pendulum environment in OpenAI Gym: https://gym.openai.com/envs/Pendulum-v0/
+    """
+
+    def __init__(self, simulator, verbose=False):
+        """
+        Initialize the inverted pendulum swing-up environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If None, by default, it will instantiate the Bullet
+                simulator.
+            verbose (bool): if True, it will print information when creating the environment
+        """
+        # simulator
+        if simulator is None:
+            simulator = prl.simulators.Bullet(render=verbose)
+
+        # create basic world with the robot
+        world = prl.worlds.BasicWorld(simulator)
+        robot = world.load_robot('pendulum')
+        robot.disable_motor()
+        if verbose:
+            robot.print_info()
+
+        # create state: [cos(q_1), sin(q_1), \dot{q}_1]
+        trig_position_state = prl.states.JointTrigonometricPositionState(robot=robot)
+        velocity_state = prl.states.JointVelocityState(robot=robot)
+        state = trig_position_state + velocity_state
+        if verbose:
+            print("\nObservation: {}".format(state))
+
+        # create action: \tau_1
+        action = prl.actions.JointTorqueAction(robot, bounds=(-2., 2.))
+        if verbose:
+            print("\nAction: {}".format(action))
+
+        # create reward/cost: ||d(q,q_{target})||^2 + 0.1 * ||\dot{q}||^2 + 0.001 * ||\tau||^2
+        position_cost = prl.rewards.JointPositionCost(prl.states.JointPositionState(robot),
+                                                      target_state=np.zeros(len(robot.joints)),
+                                                      update_state=True)
+        velocity_cost = prl.rewards.JointVelocityCost(velocity_state)
+        torque_cost = prl.rewards.JointTorqueCost(prl.states.JointForceTorqueState(robot=robot), update_state=True)
+        reward = position_cost + 0.1 * velocity_cost + 0.001 * torque_cost
+        if verbose:
+            print("Reward: {}".format(reward))
+
+        # create initial state generator: generate the state each time we reset the environment
+        def reset_robot(robot):  # function to disable the motors every time we reset the joint state
+            def reset():
+                robot.disable_motor()
+            return reset
+
+        init_state = prl.states.JointPositionState(robot)
+        low, high = np.array([-np.pi] * len(robot.joints)), np.array([np.pi] * len(robot.joints))
+        # init_state.data = np.array([np.pi / 2])  # initial data
+        # initial_state_generator = prl.states.generators.FixedStateGenerator(state=init_state, fct=reset_robot(robot))
+        initial_state_generator = prl.states.generators.UniformStateGenerator(state=init_state, low=low, high=high,
+                                                                              fct=reset_robot(robot))
+
+        # create physics randomizer: randomize the mass each time we reset the environment
+        masses = robot.get_link_masses(link_ids=robot.joints)
+        masses = (masses - masses/10., masses + masses/10.)
+        physics_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
+
+        # could create terminal conditions (if necessary) such as:
+        # - success if we stay at the upper position for more than 20 steps
+        # - failure if we can achieve the goal after 10,000 steps
+        # In the OpenAI gym, there are no terminal conditions for this problem
+        terminal_condition = None
+
+        # create environment using composition
+        super(InvertedPendulumSwingUpEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                         initial_state_generators=initial_state_generator,
+                                                         physics_randomizers=physics_randomizer,
+                                                         terminal_conditions=terminal_condition)
+
+
+# Test
+if __name__ == "__main__":
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = InvertedPendulumSwingUpEnv(sim, verbose=True)
+    action = env.action
+    action.data = 2.
+
+    # run simulation
+    env.reset()
+    for t in prl.count():
+        if (t % 800) == 0:
+            env.reset()  # test reset function
+        action()
+        states, rewards, done, info = env.step(sleep_dt=1./240)
+        # print("State: {}".format(states))
+        # print("Reward: {}".format(rewards))

pyrobolearn/envs/env.py

@@ -12,7 +12,8 @@ Dependencies:
 
 import copy
 import pickle
-# import gym
+import numpy as np
+import gym
 
 from pyrobolearn.worlds import World, BasicWorld
 from pyrobolearn.states import State
@@ -34,7 +35,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
@@ -50,9 +51,9 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
     the `gym.Env` class (see `core.py` in `https://github.com/openai/gym/blob/master/gym/core.py`).
 
     References:
-        [1] "Reinforcement Learning: An Introduction", Sutton and Barto, 1998
-        [2] "Wikipedia: Composition over Inheritance", https://en.wikipedia.org/wiki/Composition_over_inheritance
-        [3] "OpenAI gym": https://gym.openai.com/   and    https://github.com/openai/gym
+        - [1] "Reinforcement Learning: An Introduction", Sutton and Barto, 1998
+        - [2] "Wikipedia: Composition over Inheritance", https://en.wikipedia.org/wiki/Composition_over_inheritance
+        - [3] "OpenAI gym": https://gym.openai.com/   and    https://github.com/openai/gym
     """
 
     def __init__(self, world, states, rewards=None, terminal_conditions=None, initial_state_generators=None,
@@ -66,11 +67,11 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
             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
+            terminal_conditions (None, callable, TerminalCondition, list[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
+            initial_state_generators (None, StateGenerator, list[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
+            physics_randomizers (None, PhysicsRandomizer, list[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
@@ -85,9 +86,13 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         self.terminal_conditions = terminal_conditions
         self.physics_randomizers = physics_randomizers
         self.state_generators = initial_state_generators
-        self.extra_info = extra_info if extra_info is not None else lambda: False
+        self.extra_info = extra_info if extra_info is not None else lambda: dict()
         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 +148,51 @@ 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
+
+    # alias
+    observations = states
+    observation = state
+    observation_dict = state_dict
+    observation_spaces = state_spaces
+    observation_space = state_space
+
     @property
     def actions(self):
         """Return the actions."""
@@ -172,6 +222,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."""
@@ -205,7 +269,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
             conditions = [conditions]
         elif isinstance(conditions, (list, tuple)):
             for idx, condition in enumerate(conditions):
-                if not callable(conditions):
+                if not isinstance(condition, TerminalCondition):
                     raise TypeError("Expecting the {} item in the given terminal conditions to be an instance of "
                                     "`TerminalCondition`, instead got: {}".format(idx, type(condition)))
         else:
@@ -291,15 +355,19 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         for randomizer in self.physics_randomizers:
             randomizer.randomize()
 
-        # generate initial states
+        # generate initial states (states are reset by the states generators)
         for generator in self.state_generators:
-            generator()
+            generator()  # reset_state=False)
+
+        # self.world.step()
 
         # reset states and return first states/observations
-        states = [state.reset() for state in self.states]
+        # states = [state.reset(merged_data=True) for state in self.states]
+        # print("Reset: ", states)
+        states = [state.merged_data for state in self.states]
         return self._convert_state_to_data(states)
 
-    def step(self, actions=None):
+    def step(self, actions=None, sleep_dt=None):
         """
         Run one timestep of the environment's dynamics. When end of episode is reached, you are responsible for
         calling `reset()` to reset this environment's state. Accepts an action and returns a tuple (observation,
@@ -307,17 +375,20 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
 
         Args:
             actions (None, (list of) Action, (list of) np.array): an action provided by the policy(ies) to the
-                environment. Note that this is not used in this method; calling the actions should be done inside the
-                policy(ies), and not in the environment. The policy decides when to execute an action. Several problems
-                can appear by providing the actions in the environment instead of letting the policy executes them.
-                For instance, think about when there are multiple policies, when using multiprocessing, or when the
-                environment runs in real-time.
+                environment. Note that this is not normally used in this method; calling the actions should be done
+                inside the policy(ies), and not in the environment. The policy decides when to execute an action.
+                Several problems can appear by providing the actions in the environment instead of letting the policy
+                executes them. For instance, think about when there are multiple policies, when using multiprocessing,
+                or when the environment runs in real-time. However, if an action is given as a (list of) np.array,
+                it will be set as the action data, and the action will be executed. If the action is a (list of) Action,
+                it will call each action.
+            sleep_dt (float): time to sleep.
 
         Returns:
             observation (object): agent's observation of the current environment
             reward (float) : amount of reward returned after previous action
-            done (boolean): whether the episode has ended, in which case further step() calls will return undefined
-                            results
+            done (bool): whether the episode has ended, in which case further step() calls will return undefined
+              results
             info (dict): contains auxiliary diagnostic information (helpful for debugging, and sometimes learning)
         """
         # if not isinstance(actions, (list, tuple)):
@@ -334,8 +405,35 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         # if actions is not None and isinstance(actions, Action):
         #     actions()
 
+        # if the actions are provided, set and apply them in the environment
+        if actions is not None:
+            if isinstance(actions, Action):
+                actions()
+            elif isinstance(actions, (np.ndarray, int, float, np.integer)) and \
+                    isinstance(self.actions, list):  # set the data
+                if len(self.actions) == 1:
+                    self.actions[0].data = actions
+                else:
+                    raise ValueError("There are multiple actions defined in the environment, so it is unclear to "
+                                     "which action the data should be set to.")
+            elif isinstance(actions, (list, tuple)):
+                for idx, action in enumerate(actions):
+                    if isinstance(action, Action):
+                        action()
+                    elif isinstance(action, np.ndarray) and self.actions is not None:
+                        if len(actions) != len(self.actions):
+                            raise ValueError("The number of given actions (={}) is different from the number of "
+                                             "actions defined in the environments (={})".format(len(actions),
+                                                                                                len(self.actions)))
+                        self.actions[idx].data = action
+                    else:
+                        raise TypeError("Expecting a list of np.array or `Action` instead got: {}".format(type(action)))
+            else:
+                raise TypeError("Expecting an instance of `Action`, np.array, or a list of the previous ones, but got "
+                                "instead: {}".format(type(actions)))
+
         # perform a step forward in the simulation which computes all the dynamics
-        self.world.step()
+        self.world.step(sleep_dt=sleep_dt)
 
         # compute reward
         # rewards = [reward.compute() for reward in self.rewards]
@@ -346,7 +444,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
 
         # get next state/obs for each policy
         # TODO: this should be before computing the rewards as some rewards need the next state
-        states = [state() for state in self.states]
+        states = [state(merged_data=True) for state in self.states]
         states = self._convert_state_to_data(states, convert=True)
 
         # get extra information
@@ -361,7 +459,7 @@ class Env(object):  # gym.Env):  # TODO: make it inheriting the gym.Env
         self.sim.render()
 
     def hide(self):
-        """hide the GUI."""
+        """Hide the GUI."""
         self.is_rendering = False
         self.sim.hide()
 
@@ -441,13 +539,47 @@ 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
+    from pyrobolearn.simulators import Bullet
     import time
 
     # create simulator
-    sim = BulletSim()
+    sim = Bullet()
 
     # create world
     world = BasicWorld(sim)

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/README.rst

@@ -0,0 +1,4 @@
+Locomotion Environments
+-----------------------
+
+This folder contains locomotion environments.

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,271 @@
+#!/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 numpy as np
+
+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 LocomotionQuadrupedBulletEnv(LocomotionEnv):
+    r"""Locomotion Quadruped Bullet 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:
+        - joint positions (N)
+        - joint velocities (N)
+        - joint torques (N)
+        - base orientation as quaternion (4)
+    - action: PD joint position targets (or joint torques)
+    - reward: reward = 1.0 * r_f + 0. * c_d + 0. * c_s + 0.005 c_e
+        - forward reward: :math:`r_f = x_t - x_{t-1}` where :math:`x` is the base x-position.
+        - drift cost: :math:`c_d = - |y_t - y_{t-1}|` where :math:`y` is the base y-position.
+        - shake cost: :math:`c_s = - |z_t - z_{t-1}|` where :math:`z` is the base z-position.
+        - energy cost: :math:`c_e = -|\tau * dq| * dt` where :math:`\tau` are the torques, :math:`dq` are the joint
+          velocities, and :math:`dt` is the simulation time step.
+    - initial state generator:
+        - reset base position and orientation to initial position / orientation
+        - reset base velocity: [0,0,0,0,0,0]
+        - reset joint positions to initial joint positions
+        - reset joint velocities to 0
+    - physics randomizer:
+        - additive base mass noise: U([-0.2, 0.2]) kg
+        - additive leg mass noise: U([-0.2, 0.2]) kg
+        - the coefficient of friction for the feet is sampled from :math:`U([0.8, 1.5])`.
+    - terminal condition:
+        - fallen:
+            - orientation: :math:`a_z \cdot [0,0,1] < a` where :math:`a_z` is the z-axis of the base, and :math:`a` is
+              the angle threshold (0.85).
+            - height: :math:`z < h` where :math:`h` is the height threshold.
+        - distance limit: :math:`\sqrt{x^2 + y^2} > \text{threshold}`  where :math:`threshold` is set to inf.
+
+    More information:
+    - inner control_loop = 5.
+
+    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', verbose=False):
+        """
+        Initialize the locomotion with quadruped environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If None, by default, it will instantiate the Bullet
+                simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # create simulator if necessary
+        if simulator is None:
+            simulator = prl.simulators.Bullet(render=verbose)
+
+        # create basic world
+        world = prl.worlds.BasicWorld(simulator)
+
+        # load robot in world
+        self.robot = world.load_robot(robot)
+        if not isinstance(self.robot, prl.robots.LeggedRobot):  # prl.robots.QuadrupedRobot
+            raise TypeError("Expecting a legged robot, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        tau_state = prl.states.JointForceTorqueState(robot=self.robot)
+        quat_state = prl.states.BaseOrientationState(robot=self.robot)
+        state = q_state + dq_state + tau_state + quat_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create terminal condition
+        orientation_condition = prl.terminal_conditions.BaseOrientationAxisCondition(self.robot, angle=0.85,
+                                                                                     axis=(0., 0., 1.), dim=2,
+                                                                                     stay=True, out=False)
+        height_condition = prl.terminal_conditions.BaseHeightCondition(self.robot, height=self.robot.base_height/8.,
+                                                                       stay=True, out=True)
+        distance_condition = prl.terminal_conditions.DistanceCondition(self.robot, distance=float("inf"),
+                                                                       dim=[1, 1, 0], stay=True, out=False)
+        terminal_condition = [orientation_condition, height_condition, distance_condition]
+        if verbose:
+            print("Terminal condition: {}".format(terminal_condition))
+
+        # create reward
+        forward_reward = prl.rewards.ForwardProgressReward(self.robot, direction=(1., 0., 0.))
+        base_position_state = prl.states.BasePositionState(self.robot)
+        drift_cost = prl.rewards.DriftCost(base_position_state, update_state=True)  # y component
+        shake_cost = prl.rewards.ShakeCost(base_position_state)  # z component
+        energy_cost = prl.rewards.JointEnergyCost(self.robot, dt=simulator.dt)
+        reward = 1. * forward_reward + 0.005 * energy_cost + 0. * shake_cost + 0. * drift_cost
+        if verbose:
+            print(reward)
+
+        # create initial state generator
+        base_pose_gen = prl.states.generators.FixedStateGenerator(state=prl.states.BasePoseState(self.robot))
+        base_vel_gen = prl.states.generators.FixedStateGenerator(state=prl.states.BaseLinearVelocityState(self.robot))
+        q_init = self.robot.get_joint_configurations('home') if self.robot.has_joint_configuration('home') else \
+            np.zeros(len(self.robot.joints))
+        q_gen = prl.states.generators.FixedStateGenerator(state=q_state, data=q_init)
+        dq_gen = prl.states.generators.FixedStateGenerator(state=dq_state, data=np.zeros(len(self.robot.joints)))
+
+        initial_state_generator = [base_pose_gen, base_vel_gen, q_gen, dq_gen]
+        if verbose:
+            print("Initial state generator: {}".format(initial_state_generator))
+
+        # create environment using composition
+        super(LocomotionQuadrupedBulletEnv, self).__init__(world=world, states=state, rewards=reward, actions=action,
+                                                           terminal_conditions=terminal_condition,
+                                                           initial_state_generators=initial_state_generator)
+
+
+class LocomotionQuadrupedRaisimEnv(LocomotionEnv):
+    r"""Locomotion Quadruped Raisim 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.
+
+    More information:
+    - inner control_loop = int(control_dt / simulation_dt) where control_dt=0.01 and simulation_dt=0.001.
+
+    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', verbose=False):
+        """
+        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.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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)
+        if not isinstance(self.robot, prl.robots.LeggedRobot):  # prl.robots.QuadrupedRobot
+            raise TypeError("Expecting a legged robot, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # create state
+        height_state = prl.states.BaseHeightState(robot=self.robot)
+        z_axis_state = prl.states.BaseAxisState(robot=self.robot, base_axis=2)  # z-axis
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        lin_vel_state = prl.states.BaseLinearVelocityState(robot=self.robot)
+        ang_vel_state = prl.states.BaseAngularVelocityState(robot=self.robot)
+        state = height_state + z_axis_state + q_state + dq_state + lin_vel_state + ang_vel_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create terminal condition (all links that are not feet must stay out of contact)
+        terminal_condition = prl.terminal_conditions.ContactCondition(robot=self.robot, link_ids=self.robot.feet,
+                                                                      all=True, stay=True, out=True, complement=True)
+        if verbose:
+            print("Terminal condition: {}".format(terminal_condition))
+
+        # create reward
+        vel_reward = prl.rewards.BaseLinearVelocityReward(state=lin_vel_state, axis=0)
+        torque_cost = prl.rewards.JointTorqueCost(state=self.robot)
+        terminal_reward = prl.rewards.TerminalReward(terminal_conditions=terminal_condition, final_reward=-10.)
+        reward = 0.3 * vel_reward + 2e-5 * torque_cost + terminal_reward
+        if verbose:
+            print(reward)
+
+        # create initial state generator
+        q_init = self.robot.get_joint_configurations('home') if self.robot.has_joint_configuration('home') else \
+            np.zeros(len(self.robot.joints))
+        initial_state_generator = prl.states.generators.FixedStateGenerator(state=q_state, data=q_init)
+        if verbose:
+            print("Initial state generator: {}".format(initial_state_generator))
+
+        super(LocomotionQuadrupedRaisimEnv, 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 = LocomotionQuadrupedRaisimEnv(sim, verbose=True)
+    # env = LocomotionQuadrupedBulletEnv(sim, verbose=True)
+
+    # run simulation
+    for _ in count():
+        obs, reward, done, info = env.step(sleep_dt=1./240)
+        # print("obs: {}".format(obs))
+        print("reward: {}".format(reward))
+        print("done: {}".format(done))
+        print("info: {}".format(info))
+        if done:
+            print("End")
+            break

pyrobolearn/envs/locomotion/robust_quadruped.py

@@ -0,0 +1,709 @@
+#!/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(x, \alpha) = \frac{-1}{e^{\alpha x} + 2 + e^{-\alpha x}}` 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', verbose=False):
+        """
+        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.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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
+        robot = world.load_robot(robot)
+        self.robot = robot
+
+        # check if the robot is a legged robot
+        if not isinstance(self.robot, prl.robots.LeggedRobot):
+            raise TypeError("Expecting the robot to be a legged robot, but instead got: {}".format(type(self.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 action
+        action = prl.actions.JointPositionAction(robot, kp=robot.kp, kd=robot.kd)
+
+        # create state
+        ang_vel_state = prl.states.BaseAngularVelocityState(robot)
+        q_state = prl.states.JointPositionState(robot)
+        dq_state = prl.states.JointVelocityState(robot)
+        action_state = prl.states.PreviousActionState(action)
+        state = ang_vel_state + q_state + dq_state
+
+        # create cost
+        c_tau = prl.rewards.JointTorqueCost(state=robot)
+        c_jslim = prl.rewards.JointSpeedLimitCost(state=robot)
+        c_ad = prl.rewards.ActionDifferentCost(action=action)
+        c_o = prl.rewards.OrientationGravityCost(state=robot)
+        c_jp = prl.rewards.JointAngleDifferenceCost(state=, )
+        c_bi = prl.rewards.BodyImpulseCost(robot)
+        c_bs = prl.rewards.BodySlippageCost(robot)
+        c_cin = prl.rewards.SelfCollisionCost(robot)
+        cost = 0.0005 * c_tau + 0.2 * c_jslim + 0.0025 * c_ad + 6 * c_o + 6 * c_jp + 6 * c_bi + 6 * c_bs + 6 * c_cin
+
+        # create terminal condition
+        terminal_condition = prl.terminal_conditions.TimeLimitCondition(num_steps=6 * 1./simulator.dt)
+
+        # create initial state generator
+        joint_position_generator = prl.states.generators.NormalStateGenerator(state=q_state, )
+        drop_generator = prl.states.generators.DropStateGenerator(robot, height=5, condition='fixed')
+        initial_state_generator = [joint_position_generator, drop_generator]
+
+        # create physics randomizer
+        masses = robot.get_link_masses(link_ids=robot.joints)
+        masses = (masses - masses / 10., masses + masses / 10.)
+        mass_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, masses=masses)
+        com = (-0.03, 0.03)
+        com_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.joints, local_inertia_positions=com)
+        friction_randomizer = prl.physics.LinkPhysicsRandomizer(robot, link_ids=robot.feet, lateral_frictions=(0.8, 2.))
+        physics_randomizer = [mass_randomizer, com_randomizer, friction_randomizer]
+
+        # create environment using composition
+        super(SelfRightingEnv, self).__init__(world=world, states=state, rewards=cost, actions=action,
+                                              terminal_conditions=terminal_condition,
+                                              physics_randomizers=physics_randomizer,
+                                              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(x, \alpha) = \frac{-1}{e^{\alpha x} + 2 + e^{-\alpha x}}` 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', verbose=False):
+        """
+        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.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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(x, \alpha) = \frac{-1}{e^{\alpha x} + 2 + e^{-\alpha x}}` 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', verbose=False):
+        """
+        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.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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(x, \alpha) = \frac{-1}{e^{\alpha x} + 2 + e^{-\alpha x}}` 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 ~30min 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, 2] with the ANYmal robotic platform, where they
+    introduce the actuator net.
+
+    - 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)
+        - estimated base height (h_e) (1)
+        - 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)
+        - desired velocity commands (forward velocity, lateral velocity, yaw rate) (3)
+        - additive noise for observation:
+            - joint velocities U(-0.5, 0.5) rad/s
+            - linear velocity of the base U(-0.08, 0.08) m/s
+            - angular velocity of the base U(-0.16, 0.16) m/s
+    - 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:  TODO: the costs are similar but a bit different from the ones reported here
+        - 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(x, \alpha) = \frac{-1}{e^{\alpha x} + 2 + e^{-\alpha x}}` 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 (for ANYmal):
+        - base position: mean = [0,0,0.55], std = 1.5cm
+        - base orientation: mean = [1,0,0,0], std = 0.06 rad about a random axis
+        - joint positions: mean = standing configuration = [0, 0.4, -0.8, 0, 0.4, -0.8, 0, -0.4, 0.8, 0, -0.4, 0.8],
+          std = 0.25 rad
+        - base linear velocity: mean = [0]*3, std = 0.012 m/s
+        - base angular velocity: mean = [0]*3, std = 0.4 rad/s
+        - joint velocities: mean = [0]*12, std = 2 rad/s
+        - 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 for the command-conditioned locomotion motion, or U(-1.6, 1.6) m/s,
+          U(-0.2, 0.2) m/s, and U(-0.3, 0.3) rad/s respectively for the high-speed locomotion motion. Note that this
+          depends on the joystick/game controller that is being used.
+    - physics randomizer:
+        - additive noise for center of mass positions: U(-2, 2) cm
+        - additive noise for the link masses: U(-15, 15)%
+        - additive noise for joint positions: U(-2, 2) cm
+    - 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, 2]:
+
+    - actuator network: 6N input units (=joint position error history and joint velocity history), 3 * [32 (softsign)
+      units], N output units (torques)
+    - policy network: input, 256 (tanh) units, 128 (tanh) units, N output units
+    - value network: input, 256 (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.9988, 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.
+        - scaling factor: :math:`k_{c,j+1} = (k_{c,j})^{k_d}`, with :math:`k_{c,0} = 0.3` and :math:`k_d = 0.997`.
+    - For ANYmal robot: kp = 50 N·m/rad and kd = 0.1 N·m / (rad·s)
+        - kp = nominal range of torque (30 N·m) / nominal range of motion (0.6 rad)
+
+
+    Notes:
+    - the authors report that they could train the locomotion policy in ~4h on a single desktop machine (32 GB memory,
+      Intel i7-8700K and Geforce GTX 1070) with a fully C++ code.
+    - the choice of the nonlinear activation function has a strong effect on performance on the physical system. The
+      authors advise for bounded soft activation functions such as tanh and softsign instead of ReLU for instance.
+    - with respect to the kernel function for some cost terms: "An Euclidean norm generates a high cost in the
+      beginning of training where the tracking error is high such that termination (i.e. falling) becomes more
+      rewarding strategy. On the other hand, the logistic kernel ensures that the cost is lower-bounded by zero and
+      termination becomes less favorable" [2].
+
+    References:
+        - [1] "Learning agile and dynamic motor skills for legged robots", Hwangbo et al., 2019
+        - [2] Supp: https://robotics.sciencemag.org/content/robotics/suppl/2019/01/14/4.26.eaau5872.DC1/aau5872_SM.pdf
+    """
+
+    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
+    import time
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    N = int(6 * 1./sim.dt)
+    start = time.time()
+    for t in range(N):
+        sim.step(sleep_time=sim.dt)
+    end = time.time()
+    print("Total time: {}".format(end - start))
+
+    # # create environment
+    # env = RobustLocomotionQuadrupedEnv(sim)
+    #
+    # # run simulation
+    # for _ in count():
+    #     env.step(sleep_dt=1. / 240)

pyrobolearn/envs/manipulation/README.rst

@@ -0,0 +1,4 @@
+Manipulation Environments
+-------------------------
+
+This folder provides manipulation environments.

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/manipulation/manipulation.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python
+"""Provide the abstract manipulation environment from which all the other manipulation 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 ManipulationEnv(Env):
+    r"""Manipulation Environment (abstract)
+
+    This is the abstract manipulation environment from which all manipulation 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 manipulation 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(ManipulationEnv, 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/manipulation/reaching.py

@@ -0,0 +1,130 @@
+#!/usr/bin/env python
+"""Provide the reaching manipulation environment.
+
+The goal is to reach a certain 3D (fixed or movable) target with the end-effector of a robot.
+"""
+
+import re
+import numpy as np
+
+import pyrobolearn as prl
+from pyrobolearn.envs.manipulation.manipulation import ManipulationEnv
+
+
+__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 ReachingManipulationEnv(ManipulationEnv):
+    r"""Reaching Manipulation Env
+
+    The goal is to reach a certain 3D (fixed or movable) target with the end-effector of a robot.
+    """
+
+    def __init__(self, world, target=(0.5, 0., 0.5), robot='kuka_iiwa', end_effector_id=None, control_mode='position',
+                 *args, **kwargs):
+        """
+        Initialize the reaching manipulation environment.
+
+        Args:
+            world (World, Simulator): world/simulator instance.
+            target (list/tuple of 3 floats, np.array[3], Body): target to reach. If a list, tuple, or array is
+                provided, it will load a visual sphere at the specified target location.
+            robot (str, Robot): robot instance, or robot name to load in the world.
+            end_effector_id (int, None): end effector link id that has to reach the target. If None, it will check in
+                the end_
+            control_mode (str): joint action control mode, select between {'position', 'position change', 'velocity',
+                'velocity change', 'torque', 'torque with gravity compensation'/'torque change'}. Note that 'torque
+                change' and 'torque with gravity compensation' are the same (they are synonyms).
+            args (list): list of arguments that are given to the `world.load_robot` method.
+            kwargs (dict): dict of arguments that are given to the `world.load_robot` method.
+        """
+        # create basic world if not already created
+        if isinstance(world, prl.simulators.Simulator):
+            world = prl.worlds.BasicWorld(world)
+        elif not isinstance(world, prl.worlds.World):
+            raise TypeError("Expecting the world to be an instance of `World` or `Simulator`, instead got: "
+                            "{}".format(type(world)))
+
+        # load robot
+        if isinstance(robot, str):
+            robot = world.load_robot(robot, *args, **kwargs)
+        elif isinstance(robot, prl.robots.Robot):
+            # check if robot already loaded
+            if robot.id not in world.bodies:
+                world.load_robot(robot)
+        else:
+            raise TypeError("Expecting the given robot to be a string or an instance of `Robot`, instead got: "
+                            "{}".format(type(robot)))
+        self.robot = robot
+
+        # load target
+        if not isinstance(target, prl.robots.Body):
+            if not isinstance(target, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the target to be list/tuple/array of 3 floats representing the target "
+                                "position, or an instance of `Body`, but got instead: {}".format(type(target)))
+            target = world.load_visual_sphere(position=target, radius=0.05, color=(1, 0, 0, 0.5), return_body=True)
+        # save target body such that the user can use it (to change its position for instance)
+        self.target = target
+
+        # check end effector id
+        if end_effector_id is None:
+            if not hasattr(robot, 'end_effectors'):
+                raise ValueError("We could not find any end effectors for the given robot... Please specify one by "
+                                 "setting the 'end_effector_id' parameter.")
+            end_effector_id = robot.end_effectors[0]
+        if not isinstance(end_effector_id, int):
+            raise TypeError("Expecting the 'end_effector_id' to be None or an integer, but got instead: "
+                            "{}".format(type(end_effector_id)))
+
+        # create state
+        state = prl.states.LinkWorldPositionState(robot, link_ids=end_effector_id)
+
+        # create action based on the specified joint action control mode
+        control_mode = control_mode.lower()
+        control_mode = ' '.join(re.findall(r"[a-z]*[^\-\_]", control_mode))
+        if control_mode == 'position':
+            action = prl.actions.JointPositionAction(robot)
+        elif control_mode == 'position change':
+            action = prl.actions.JointPositionChangeAction(robot)
+        elif control_mode == 'velocity':
+            action = prl.actions.JointVelocityAction(robot)
+        elif control_mode == 'velocity change':
+            action = prl.actions.JointVelocityChangeAction(robot)
+        elif control_mode == 'torque':
+            action = prl.actions.JointTorqueAction(robot)
+        elif control_mode == 'torque with gravity compensation' or control_mode == 'torque change':
+            action = prl.actions.JointTorqueGravityCompensationAction(robot)
+        else:
+            raise ValueError("Please select the `control_mode` to be between ['position', 'position change', "
+                             "'velocity', 'velocity change', 'torque', "
+                             "'torque with gravity compensation'/'torque change'], and not: "
+                             "{}".format(control_mode))
+
+        # create distance cost
+        reward = prl.rewards.DistanceCost(state, target)
+
+        # create environment using composition
+        super(ReachingManipulationEnv, self).__init__(world=world, states=state, rewards=reward, actions=action)
+
+
+# Test
+if __name__ == "__main__":
+    from itertools import count
+    import pyrobolearn as prl
+
+    # create simulator
+    sim = prl.simulators.Bullet()
+
+    # create environment
+    env = ReachingManipulationEnv(sim)
+
+    # run simulation
+    for _ in count():
+        env.step(sleep_dt=1./240)

pyrobolearn/envs/sports/README.rst

@@ -0,0 +1,6 @@
+Sport Environments
+==================
+
+WARNING: This is currently a work in progress. The rewards and other functions have not been defined yet.
+
+This folder provides sport environments.

pyrobolearn/envs/sports/baseball.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+r"""Provide the baseball environment.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.baseball import BaseballWorld
+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 BaseballEnv(Env):
+    r"""Baseball environment
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the baseball environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = BaseballWorld(simulator)
+
+        # load manipulator in world
+        self.robot = world.load_robot(robot)
+        if not isinstance(self.robot, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # attach bat to robot end effector
+        world.attach(body1=self.robot, body2=world.bat, link1=self.robot.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.], parent_frame_position=[0., 0., world.bat_grip_radius],
+                     child_frame_position=[0., 0.3, 0.], parent_frame_orientation=[0., 0., 0., 1.])
+
+        # apply force to ball to throw it; f=dp/dt thus dp = f dt (change of momentum)
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        state = q_state + dq_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(BaseballEnv, 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 = BaseballEnv(sim)
+
+    # run simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)

pyrobolearn/envs/sports/basketball.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+r"""Provide the basketball environment.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.basketball import BasketBallWorld
+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 BasketBallEnv(Env):
+    r"""Basketball environment
+
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the basketball environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = BasketBallWorld(simulator)
+
+        # load manipulator in world
+        self.robot = world.load_robot(robot)
+        if not isinstance(self.robot, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # attach ball to robot end effector
+        world.attach(body1=self.robot, body2=world.ball, link1=self.robot.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.], parent_frame_position=[0., 0., world.ball_radius],
+                     child_frame_position=[0., 0., -0.01], parent_frame_orientation=[0., 0., 0., 1.])
+
+        # apply force to ball to throw it; f=dp/dt thus dp = f dt (change of momentum)
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        state = q_state + dq_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(BasketBallEnv, 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 = BasketBallEnv(sim)
+
+    # run simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)

pyrobolearn/envs/sports/billiard.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+r"""Provide the billiard environment.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.billiard import BilliardWorld
+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 BilliardEnv(Env):
+    r"""Billiard environment
+
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the billiard environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = BilliardWorld(simulator)
+
+        # load manipulator in world
+        self.robot = world.load_robot(robot, position=[-2., 0.2, 0.])
+        if not isinstance(self.robot, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # attach cue to robot end effector
+        # Note that you can detach the cue from the robot end effector using `world.detach`
+        world.attach(body1=self.robot, body2=world.cue1, link1=self.robot.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.], parent_frame_position=[-0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                     parent_frame_orientation=[0., 0., 0., 1.])
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        state = q_state + dq_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(BilliardEnv, 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 world
+    env = BilliardEnv(sim)
+
+    # run simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)

pyrobolearn/envs/sports/darts.py

@@ -0,0 +1,97 @@
+#!/usr/bin/env python
+r"""Provide the darts environments.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.darts import DartsWorld
+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 DartsEnv(Env):
+    r"""Darts environment
+
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the darts environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = DartsWorld(simulator)
+
+        # load manipulator in world
+        self.robot = world.load_robot(robot)
+        if not isinstance(self.robot, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # attach first dart to robot end effector
+        world.attach(body1=self.robot, body2=world.darts[0], link1=self.robot.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.],
+                     parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                     parent_frame_orientation=[0, 0., 0., 1.])
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        state = q_state + dq_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(DartsEnv, 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 = DartsEnv(sim)
+
+    # run simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)

pyrobolearn/envs/sports/football.py

@@ -0,0 +1,91 @@
+#!/usr/bin/env python
+r"""Provide the football/soccer environment.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.football import FootballWorld
+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 FootballEnv(Env):
+    r"""Football/Soccer environment.
+
+    """
+
+    def __init__(self, simulator, robot='coman', verbose=False):
+        """
+        Initialize the darts environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = FootballWorld(simulator)
+
+        # load manipulator in world
+        self.robot = world.load_robot(robot)
+        if not isinstance(self.robot, prl.robots.LeggedRobot):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        state = q_state + dq_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(FootballEnv, 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 = FootballEnv(sim, robot='coman')
+
+    # run simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)

pyrobolearn/envs/sports/kendo.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+r"""Provide the kendo environment.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.kendo import KendoWorld
+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 KendoEnv(Env):
+    r"""Kendo environment.
+
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the kendo environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = KendoWorld(simulator, position=(0., 0., 1.5), num_shinai=2)
+
+        # create manipulators
+        robot1 = world.load_robot(robot)
+        robot2 = world.load_robot(robot, position=(1., 0.), orientation=(0., 0., 1., 0.))
+        self.robot1, self.robot2 = robot1, robot2
+
+        # attach shinai to robot end effectors
+        world.attach(body1=robot1, body2=world.shinai[0], link1=robot1.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.],
+                     parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.15],
+                     parent_frame_orientation=[0, -0.707, 0., .707])
+        world.attach(body1=robot2, body2=world.shinai[1], link1=robot2.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.],
+                     parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.15],
+                     parent_frame_orientation=[0, -0.707, 0., .707])
+
+        if not isinstance(robot1, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(robot1)))
+        if verbose:
+            self.robot1.print_info()
+
+        # create states
+        states = []
+        for i, robot in enumerate([robot1, robot2]):
+            q_state = prl.states.JointPositionState(robot=robot)
+            dq_state = prl.states.JointVelocityState(robot=robot)
+            state = q_state + dq_state
+            states.append(state)
+        if verbose:
+            print(states)
+
+        # create actions
+        actions = []
+        for i, robot in enumerate([robot1, robot2]):
+            action = prl.actions.JointPositionAction(robot=robot, kp=robot.kp, kd=robot.kd)
+            actions.append(action)
+        if verbose:
+            print(actions)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(KendoEnv, self).__init__(world=world, states=states, rewards=reward, actions=actions,
+                                       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 = KendoEnv(sim)
+
+    # run simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)

pyrobolearn/envs/sports/ping_pong.py

@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+r"""Provide the ping pong (table tennis) environment.
+"""
+
+# TODO: finish to implement this environment
+
+import pyrobolearn as prl
+from pyrobolearn.worlds.samples.sports.ping_pong import PingPongWorld, BallOnPaddleWorld
+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 PingPongEnv(Env):
+    r"""Ping Pong environment.
+
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the ping pong environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = PingPongWorld(simulator)
+
+        # load 2 manipulators in world
+        robot1 = world.load_robot(robot, position=[1.8, 0., 0.2], fixed_base=True)
+        robot2 = world.load_robot(robot, position=[-1.8, 0., 0.2], fixed_base=True)
+        self.robot1, self.robot2 = robot1, robot2
+        if not isinstance(robot1, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(robot1)))
+
+        # attach each paddle to the robot's end-effector
+        world.attach(body1=robot1, body2=world.paddle1, link1=robot1.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.],
+                     parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                     parent_frame_orientation=[0, -0.707, 0, 0.707])
+        world.attach(body1=robot2, body2=world.paddle2, link1=robot2.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.],
+                     parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                     parent_frame_orientation=[0, 0.707, 0, 0.707])
+
+        if verbose:
+            self.robot1.print_info()
+
+        # create states
+        states = []
+        for i, robot in enumerate([robot1, robot2]):
+            q_state = prl.states.JointPositionState(robot=robot)
+            dq_state = prl.states.JointVelocityState(robot=robot)
+            state = q_state + dq_state
+            states.append(state)
+        if verbose:
+            print(states)
+
+        # create actions
+        actions = []
+        for i, robot in enumerate([robot1, robot2]):
+            action = prl.actions.JointPositionAction(robot=robot, kp=robot.kp, kd=robot.kd)
+            actions.append(action)
+        if verbose:
+            print(actions)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(PingPongEnv, self).__init__(world=world, states=states, rewards=reward, actions=actions,
+                                          terminal_conditions=terminal_condition,
+                                          initial_state_generators=initial_state_generator)
+
+
+class BallOnPaddleEnv(Env):
+    r"""Ball on a paddle environment.
+
+    """
+
+    def __init__(self, simulator, robot='kuka_iiwa', verbose=False):
+        """
+        Initialize the ball on paddle environment.
+
+        Args:
+            simulator (Simulator, None): simulator instance. If simulator is None, it will use the PyBullet simulator.
+            robot (str): robot name.
+            verbose (bool): if True, it will print information when creating the environment.
+        """
+        # 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 world
+        world = BallOnPaddleWorld(simulator)
+
+        # load manipulator in world
+        self.robot = world.load_robot(robot)
+        if not isinstance(self.robot, prl.robots.Manipulator):
+            raise TypeError("Expecting a manipulator, but got instead {}".format(type(self.robot)))
+        if verbose:
+            self.robot.print_info()
+
+        # attach each paddle to the robot's end-effector
+        world.attach(body1=self.robot, body2=world.paddle, link1=self.robot.end_effectors[0], link2=-1,
+                     joint_axis=[0., 0., 0.], parent_frame_position=[0., 0., 0.02], child_frame_position=[0., 0., 0.],
+                     parent_frame_orientation=[0, 0.707, 0, 0.707])
+
+        world.ball.position = [0., 0., 2.]
+
+        # create state
+        q_state = prl.states.JointPositionState(robot=self.robot)
+        dq_state = prl.states.JointVelocityState(robot=self.robot)
+        state = q_state + dq_state
+        if verbose:
+            print(state)
+
+        # create action
+        action = prl.actions.JointPositionAction(robot=self.robot, kp=self.robot.kp, kd=self.robot.kd)
+        if verbose:
+            print(action)
+
+        # create reward  # TODO: use geometrical rewards
+        reward = None
+
+        # create terminal condition
+        terminal_condition = None
+
+        # create initial state generator
+        initial_state_generator = None
+
+        super(BallOnPaddleEnv, 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 = BallOnPaddleEnv(sim)
+    env = PingPongEnv(sim)
+
+    # run the simulation
+    for t in count():
+        env.step(sleep_dt=sim.dt)