add algos, interfaces and bridges

pyrobolearn/algos/__init__.py

@@ -0,0 +1,27 @@
+
+# import RL algo
+# from rl_algo import *
+
+# import CEM
+from cem import CEM
+
+# import CMAES
+from cmaes import CMAES
+
+# import from NEAT
+from neat_algo import NEAT
+
+# import BO
+from bo import BO
+
+# import FD
+from fd import FD
+
+# import PoWER
+from power import PoWER
+
+# import REINFORCE
+# from reinforce import REINFORCE
+
+# import PPO
+# from ppo import PPO

pyrobolearn/algos/bo.py

@@ -0,0 +1,269 @@
+#!/usr/bin/env python
+"""Provide the Bayesian Optimization algorithm.
+"""
+
+import numpy as np
+import torch
+import time
+import GPy
+import GPyOpt
+
+from pyrobolearn.envs import Env
+from pyrobolearn.tasks import RLTask
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class BO(object):
+    r"""Bayesian Optimization
+
+    Bayesian Optimization is a global (gradient-free), probabilistic, non-parametric, model-based, optimization of
+    black-box functions.
+
+    Bayesian optimization can be formulated as an optimization problem:
+
+    .. math:: \theta^* = arg\,max_{\theta} f(\theta)
+
+    where :math:`\theta` are the parameters of the model we are trying to optimize, and :math:`f` is the unknown
+    objective function which is modeled using a probabilistic model such as a Gaussian Process (GP). By samp
+
+    Popular acquisition functions which specify which parameters to test next by making a trade-off between
+    exploitation and exploration, include:
+    * Probability of Improvement (PI) [7]:
+    * Expected Improvement (EI) [8]:
+    * Upper Confidence Bound (UCB) [9]:
+
+    Pseudo-Algo (from [3]):
+        D <-- if available: {\theta, f(\theta)}
+        Prior <-- if available: prior of the response surface
+        while optimize:
+            train a response surface from D
+
+    References:
+        [1] "Bayesian Approach to Global Optimization: Theory and Applications", Mockus, 1989
+        [2] "A Tutorial on Bayesian Optimization of Expensive Cost Functions, with Application to Active User Modeling
+            and Hierarchical Reinforcement Learning", Brochu et al., 2010
+        [3] "Taking the Human Out of the Loop: a Review of Bayesian Optimization", Shahriari et al., 2016
+        [4] "Bayesian Optimization for Learning Gaits under Uncertainty: An Experimental Comparison on a Dynamic
+            Bipedal Walker", Calandra et al., 2015
+        [5] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006
+        [6] "GPyOpt: A Bayesian Optimization framework in python" (2016), https://github.com/SheffieldML/GPyOpt
+        [7] "A New Method of Locating the Maximum Point of an Arbitrary Multipeak Curve in the Presence of Noise",
+            Kushner, 1964
+        [8] "The Application of Bayesian Methods for Seeking the Extremum", Mockus et al., 1978
+        [9] "A Statistical Method for Global Optimization", Cox et al., 1997
+    """
+
+    def __init__(self, task=None, policy=None, domain=(-3., 3.), num_workers=1):
+        """
+        Initialize the Bayesian Optimization algorithm.
+
+        Args:
+            task (RLTask, Env): RL task/env to run
+            policy (Policy): specify the policy (model) to optimize
+            num_workers (int): number of workers/jobs to run in parallel
+        """
+        if isinstance(task, Env):
+            task = RLTask(task, policy)
+        self.task = task
+        self.policy = policy
+        self.num_workers = num_workers
+
+        self.best_reward = -np.infty
+        self.best_parameters = None
+
+        self.num_steps = 1000
+        self.num_rollouts = 1
+        self.verbose = False
+        self.episode = 0
+
+        self.domain = domain
+
+        self.rewards = []
+
+    # def get_vectorized_parameters(self, to_numpy=True):
+    #     parameters = self.policy.parameters
+    #
+    #     vector = []
+    #     from_numpy = False
+    #     for parameter in parameters:
+    #         if isinstance(parameter, np.ndarray):
+    #             from_numpy = True
+    #         vector.append(parameter.reshape(-1))
+    #
+    #     if from_numpy:
+    #         print(vector)
+    #         vector = np.concatenate(vector)
+    #         if not to_numpy:
+    #             return torch.from_numpy(vector)
+    #     else:
+    #         vector = torch.cat(vector)
+    #         if to_numpy:
+    #             return vector.detach().numpy()
+    #     return vector
+    #
+    # def set_vectorized_parameters(self, vector):
+    #     # convert the vector to torch array
+    #     if isinstance(vector, np.ndarray):
+    #         vector = torch.from_numpy(vector).float()
+    #
+    #     # set the parameters from the vectorized one
+    #     idx = 0
+    #     for parameter in self.policy.parameters:
+    #         size = parameter.nelement()
+    #         parameter.data = vector[idx:idx+size].reshape(parameter.shape)
+    #         idx += size
+
+    def explore_and_evaluate(self, params):
+        # set policy parameters
+        # self.set_vectorized_parameters(params[0])
+        self.policy.set_vectorized_parameters(params[0])
+
+        # run a number of rollouts
+        reward = []
+        for rollout in range(self.num_rollouts):
+            rew = self.task.run(num_steps=self.num_steps, dt=1./240, use_terminating_condition=True, render=False)
+            reward.append(rew)
+        reward = np.mean(reward)
+
+        self.rewards.append(reward)
+
+        # print info
+        self.episode += 1
+        if self.verbose:
+            print('Episode {} - reward: {}'.format(self.episode, reward))
+
+        return reward
+
+    def train(self, num_steps=1000, num_rollouts=1, num_episodes=1, verbose=False, seed=None, max_time=3600):
+        # set few variables
+        self.num_steps = num_steps
+        self.num_rollouts = num_rollouts
+        self.episode = 0
+        self.verbose = verbose
+        self.rewards = []
+
+        # set seed if specified
+        if seed is not None:
+            np.random.seed(seed)
+
+        # init
+        # parameters = self.get_vectorized_parameters(to_numpy=True)
+        parameters = self.policy.get_vectorized_parameters(to_numpy=True)
+
+        # define domain
+        domain = [{'name': 'params', 'type': 'continuous', 'domain': self.domain, 'dimensionality': len(parameters)}]
+
+        # Solve the optimization
+        opt = GPyOpt.methods.BayesianOptimization(f=self.explore_and_evaluate,
+                                                  domain=domain,
+                                                  model_type='GP',  # 'sparseGP'
+                                                  acquisition_type='EI',  # 'UCB'/'LCB', 'EI', 'MPI'
+                                                  acquisition_optimizer_type='lbfgs',  # 'DIRECT', 'CMA'
+                                                  num_cores=self.num_workers,
+                                                  verbosity=verbose,
+                                                  maximize=True,  # True
+                                                  verbosity_model=False,  # True
+                                                  kernel=GPy.kern.RBF(input_dim=1))
+
+        # print(opt.model.kernel.name)
+
+        # Run the optimization
+        max_iter = num_episodes  # evaluation budget (min=4), nb_eval = 4 + max_iter
+        max_time = max_time  # time budget
+        eps = 10e-6  # Minimum allows distance between the last two observations
+
+        if verbose:
+            print('Optimizing...')
+
+        start = time.time()
+        opt.run_optimization(max_iter, max_time, eps)
+        end = time.time()
+
+        if verbose:
+            print('Done with total time: {}'.format(end - start))
+
+        # save best parameters and reward
+        self.best_parameters = opt.x_opt
+        self.best_reward = -opt.fx_opt
+
+        # print best reward
+        if verbose:
+            print("\nBest reward found: {}".format(self.best_reward))
+
+        # set the best parameters
+        self.policy.set_vectorized_parameters(self.best_parameters)
+
+        return self.rewards
+
+    def test(self, num_steps=1000, dt=0, use_terminating_condition=False, render=True):
+        return self.task.run(num_steps=num_steps, dt=dt, use_terminating_condition=use_terminating_condition,
+                             render=render)
+
+    # def optimize(self, fct, num_iter=6, max_time=3600, seed=None, verbose=False):
+    #     """
+    #     Optimize the given model based
+    #
+    #     Args:
+    #         num_iter (int): number of iteration
+    #         max_time (float):
+    #         seed (int): random seed
+    #         verbose (bool): True if we should print information during the optimization process
+    #
+    #     Returns:
+    #
+    #     """
+    #     # set seed if specified
+    #     if seed is not None:
+    #         np.random.seed(seed)
+    #
+    #     # define domain
+    #     domain = [{'name': 'params', 'type': 'continuous', 'domain': (-1, 1), 'dimensionality': 1}]
+    #
+    #     # Solve the optimization
+    #     opt = GPyOpt.methods.BayesianOptimization(f=fct,
+    #                                               domain=domain,
+    #                                               model_type='GP',  # 'sparseGP'
+    #                                               acquisition_type='EI',  # 'UCB'/'LCB', 'EI', 'MPI'
+    #                                               acquisition_optimizer_type='lbfgs',  # 'DIRECT', 'CMA'
+    #                                               num_cores=1,
+    #                                               verbosity=verbose,
+    #                                               maximize=True,  # True
+    #                                               verbosity_model=False,  # True
+    #                                               kernel=GPy.kern.RBF(input_dim=1))
+    #
+    #     # print(opt.model.kernel.name)
+    #
+    #     # Run the optimization
+    #     max_iter = num_iter  # evaluation budget (min=4), nb_eval = 4 + max_iter
+    #     max_time = max_time  # time budget
+    #     eps = 10e-6  # Minimum allows distance between the last two observations
+    #
+    #     if verbose:
+    #         print('Optimizing...')
+    #
+    #     start = time.time()
+    #     opt.run_optimization(max_iter, max_time, eps)
+    #     end = time.time()
+    #
+    #     if verbose:
+    #         print('Done with total time: {}'.format(end - start))
+    #
+    #     # save best parameters and reward
+    #     self.best_parameters = opt.x_opt
+    #     self.best_reward = opt.fx_opt
+    #
+    #     return opt
+
+
+# Tests
+if __name__ == '__main__':
+    pass

pyrobolearn/algos/cem.py

@@ -0,0 +1,185 @@
+#!/usr/bin/env python
+"""Provide the Cross-Entropy Method algorithm.
+
+This CEM is an evolutionary algorithm that explores in the parameter space of the policy in an episodic way.
+"""
+
+import numpy as np
+import torch
+# from pathos.multiprocessing import Pool
+
+from pyrobolearn.envs import Env
+from pyrobolearn.tasks import RLTask
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class CEM(object):  # RLAlgo
+    r"""Cross-Entropy Method
+
+    Type: population-based and model-free, exploration in parameter space, episode-based.
+
+    The Cross-Entropy Method (CEM) is an evolutionary algorithm that explores in the parameter space of the policy
+    in an episodic way. That is, everytime the parameters are updated, the policy is run for a whole episode before
+    being evaluated.
+
+    This algorithm works by first assuming that the parameters are generated from a multivariate normal distribution
+    with an initial mean and a fixed covariance matrix. Few samples are then drawn from this distribution to form
+    the initial population of parameter vectors. Each parameter vector is then set on the policy and evaluated on
+    the whole episode. The best parameter vectors which constitutes a fraction of the population and called
+    the elites are then selected. From them, a new mean and standard covariance matrix are computed and used to form
+    the new multivariate normal distribution from which the next population is generated. This process is carried
+    out for several generation. At the end, the best parameter (the elite) is returned.
+
+    References:
+        [1] "The Cross-Entropy Method: A Unified Approach to Combinatorial Optimization, Monte-Carlo Simulation
+            and Machine Learning", Rubinstein et al., 2004
+        [2] "A Tutorial on the Cross-Entropy Method", de Boer, 2003
+        [3] "The Cross Entropy Method for Fast Policy Search", Mannor et al., 2003 (ICML)
+
+    Interesting codes (the code in this file was inspired from the first reference):
+        - Schulman's presentation (2016)
+        - modular_rl: https://github.com/joschu/modular_rl
+        - pytorch-rl: https://github.com/khushhallchandra/pytorch-rl
+        - rllab: https://github.com/rll/rllab
+    """
+
+    def __init__(self, task, policy, population_size=20, elite_fraction=0.2, num_workers=1):
+        """
+        Initialize the CEM algorithm.
+
+        Args:
+            task (RLTask, Env): RL task/env to run
+            policy (Policy): specify the policy (model) to optimize
+            population_size (int): size of the population
+            elite_fraction (float): fraction of elites to use to compute the new mean and covariance matrix of the
+                multivariate normal distribution
+            num_workers (int): number of workers/jobs to run in parallel
+        """
+        # create explorer
+        # create evaluator
+        # create updater
+        # super(CEM, self).__init__(self, explorer, evaluator, updater, num_workers=1)
+
+        if isinstance(task, Env):
+            task = RLTask(task, policy)
+        self.task = task
+        self.policy = policy
+        self.population_size = population_size
+        self.elite_fraction = elite_fraction
+        self.num_workers = num_workers
+
+        self.best_reward = -np.infty
+        self.best_parameters = None
+
+    def get_vectorized_parameters(self, to_numpy=True):
+        parameters = self.policy.parameters
+        vector = torch.cat([parameter.reshape(-1) for parameter in parameters])  # np.concatenate = torch.cat
+        if to_numpy:
+            return vector.detach().numpy()
+        return vector
+
+    def set_vectorized_parameters(self, vector):
+        # convert the vector to torch array
+        if isinstance(vector, np.ndarray):
+            vector = torch.from_numpy(vector).float()
+
+        # set the parameters from the vectorized one
+        idx = 0
+        for parameter in self.policy.parameters:
+            size = parameter.nelement()
+            parameter.data = vector[idx:idx+size].reshape(parameter.shape)
+            idx += size
+
+    def train(self, num_steps=1000, num_rollouts=1, num_episodes=1, verbose=False, seed=None):
+        # set seed
+        if seed is not None:
+            np.random.seed(seed)
+
+        # create recorders
+        max_rewards, avg_rewards = [], []
+
+        # init
+        theta_mean = self.get_vectorized_parameters(to_numpy=True)
+        theta_std = np.ones(len(theta_mean))
+        # pool = Pool(self.num_workers)
+
+        # for each episode/generation
+        for episode in range(num_episodes):
+            if verbose:
+                print('\nEpisode {}'.format(episode+1))
+
+            # 1. Explore
+            # sample parameter vectors
+            thetas = np.random.multivariate_normal(theta_mean, np.diag(theta_std), self.population_size)
+
+            # perform one episode for each parameter
+            # jobs = [pool.apipe(self.task.run, num_steps, use_terminating_condition=False) for theta in thetas]
+            rewards = []
+            for i, theta in enumerate(thetas):
+                # set policy parameters
+                self.set_vectorized_parameters(theta)
+
+                # run a number of rollouts
+                reward = []
+                for rollout in range(num_rollouts):
+                    rew = self.task.run(num_steps=num_steps, use_terminating_condition=True, render=False)
+                    reward.append(rew)
+                reward = np.mean(reward)
+                rewards.append(reward)
+
+                # print info
+                if verbose:
+                    print(' -- individual {} with avg reward of {}'.format(i+1, reward))
+
+            # 2. Evaluate (compute loss)
+
+            # 3. Update
+
+            # get elite parameters
+            num_elites = int(self.population_size * self.elite_fraction)
+            elite_ids = np.argsort(rewards)[-num_elites:]
+            elite_thetas = np.array([thetas[i] for i in elite_ids])
+
+            # update theta_mean and theta_std
+            theta_mean = elite_thetas.mean(axis=0)
+            theta_std = np.sqrt(np.mean((elite_thetas - theta_mean) ** 2, axis=0))
+
+            # 4. Save best reward and associated parameter
+            max_reward, avg_reward = np.max(rewards), np.mean(rewards)
+            if max_reward > self.best_reward:
+                self.best_reward = max_reward
+                self.best_parameters = thetas[elite_ids[-1]]
+
+            # print info
+            if verbose:
+                print("Episode {} mean reward: {} max reward: {}".format(episode+1, avg_reward, max_reward))
+
+            # Save the evolution of the algo
+            avg_rewards.append(avg_reward)
+            max_rewards.append(max_reward)
+
+        # print best reward
+        if verbose:
+            print("\nBest reward found: {}".format(self.best_reward))
+
+        # set the best parameters
+        self.set_vectorized_parameters(self.best_parameters)
+
+        return avg_rewards, max_rewards
+
+    def test(self, num_steps=1000, dt=0, use_terminating_condition=False, render=True):
+        return self.task.run(num_steps=num_steps, dt=dt, use_terminating_condition=use_terminating_condition,
+                             render=render)
+
+
+# Tests
+if __name__ == '__main__':
+    pass

pyrobolearn/algos/cmaes.py

@@ -0,0 +1,184 @@
+#!/usr/bin/env python
+"""Provide the Covariance Matrix Adaptation Evolution Strategy algorithm.
+
+'The Covariance Matrix Adaptation Evolution Strategy (CMA-ES) is a stochastic derivative-free numerical optimization
+algorithm for difficult (non-convex, ill-conditioned, multi-modal, rugged, noisy) optimization problems in
+continuous search spaces.' [1]
+
+References:
+    [1] "Python implementation of CMA-ES", Hansen et al., 2019 (https://github.com/CMA-ES/pycma)
+"""
+
+import numpy as np
+
+try:
+    import cma
+except ImportError as e:
+    raise ImportError(e.__str__() + "\n HINT: you can install CMA-ES or `pycma` directly via 'pip install cma'.")
+
+from pyrobolearn.envs import Env
+from pyrobolearn.tasks import RLTask
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class CMAES(object):  # Algo):
+    r"""Covariance Matrix Adaptation Evolution Strategy (CMA-ES)
+
+    Type: population-based (genetic), stochastic and derivative-free, exploration in parameter space, optimization
+    for non-linear and non-convex functions, episode-based.
+
+    'The Covariance Matrix Adaptation Evolution Strategy (CMA-ES) is a stochastic derivative-free numerical
+    optimization algorithm for difficult (non-convex, ill-conditioned, multi-modal, rugged, noisy) optimization
+    problems in continuous search spaces.' [3]
+
+    Complexity:
+
+    References:
+        [1] "Completely Derandomized Self-Adaptation in Evolution Strategies", Hansen et al., 2001
+        [2] "The CMA Evolution Strategy: A Tutorial", Hansen, 2016
+        [3] "Python implementation of CMA-ES", Hansen et al., 2019: https://github.com/CMA-ES/pycma
+        [4] pycma API documentation: cma.gforge.inria.fr/apidocs-pycma
+
+    Python Implementations:
+    - pycma: https://github.com/CMA-ES/pycma
+    - rllab: https://github.com/rll/rllab
+    - DEAP: https://github.com/DEAP/deap   and   http://deap.readthedocs.io/en/master/examples/cmaes.html
+    """
+
+    def __init__(self, task, policy, population_size=20, sigma=0.5, num_workers=1):
+        """
+        Initialize the CMA-ES algorithm.
+
+        Args:
+            task (RLTask, Env): RL task/env to run
+            policy (Policy): specify the policy (model) to optimize
+            population_size (int): size of the population
+            sigma (float): initial standard deviation for CMA-ES
+            num_workers (int): number of workers/jobs to run in parallel
+        """
+        # create explorer
+        # create evaluator
+        # create updater
+        # super(CEM, self).__init__(self, explorer, evaluator, updater, num_workers=1)
+
+        if isinstance(task, Env):
+            task = RLTask(task, policy)
+        self.task = task
+        self.policy = policy
+        self.population_size = population_size
+        self.sigma = sigma
+        self.num_workers = num_workers
+        self.es = None
+
+        self.best_reward = -np.infty
+        self.best_parameters = None
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def population_size(self):
+        return self._population_size
+
+    @population_size.setter
+    def population_size(self, size):
+        # check argument
+        if not isinstance(size, int):
+            raise TypeError("Expecting the population size to be an integer.")
+        if size < 1:
+            raise ValueError("Expecting the population size to be an integer bigger than 0.")
+
+        # set population size
+        self._population_size = size
+
+    ###########
+    # Methods #
+    ###########
+
+    def explore_and_evaluate(self, params, num_steps, num_rollouts):
+        # set policy parameters
+        self.policy.set_vectorized_parameters(params)
+
+        # run a number of rollouts
+        reward = []
+        for rollout in range(num_rollouts):
+            rew = self.task.run(num_steps=num_steps, use_terminating_condition=True, render=False)
+            reward.append(rew)
+        reward = np.mean(reward)
+
+        # return cost to minimize
+        return -reward
+
+    def train(self, num_steps=1000, num_rollouts=1, num_episodes=1, verbose=False, seed=None):
+        # create CMA-ES
+        self.es = cma.CMAEvolutionStrategy(self.policy.get_vectorized_parameters(), sigma0=self.sigma,
+                                           inopts={'popsize': self.population_size})  # {'bounds': [-np.inf, np.inf]}
+
+        # set seed
+        if seed is not None:
+            self.es.opts.set({'seed': seed})
+
+        # set number of iterations
+        self.es.opts.set({'maxiter': num_episodes})
+
+        # create recorders
+        max_rewards, avg_rewards = [], []
+
+        # optimize
+        # self.es.optimize(self.explore_and_evaluate, iterations=num_episodes, args=(num_steps, num_rollouts),
+        #                  verb_disp=int(verbose))
+        # evaluate
+        # (solutions, costs) = self.es.ask_and_eval(self.explore_and_evaluate)
+
+        # for each episode/generation
+        # while not self.es.stop():
+        for episode in range(num_episodes):
+            if self.es.stop():
+                break
+
+            # optimize
+            parameters = self.es.ask()
+            costs = [self.explore_and_evaluate(params, num_steps, num_rollouts) for params in parameters]
+            self.es.tell(parameters, costs)
+
+            # get rewards
+            max_rewards.append(-np.min(costs))
+            avg_rewards.append(-np.mean(costs))
+
+            # print info
+            if verbose:
+                # self.es.disp(1)
+                print("Episode {} mean reward: {} max reward: {}".format(episode + 1, avg_rewards[-1], max_rewards[-1]))
+
+        # if verbose:
+        #     self.es.result_pretty()
+
+        # print info
+        if verbose:
+            # self.es.disp(1)
+            print('Termination by {}'.format(self.es.stop()))
+            print('Best reward found = {}'.format(-self.es.result[1]))
+            # print('solution = {}'.format(self.es.result[0]))
+
+        # get results (check documentation of ` _CMAEvolutionStrategyResult`)
+        self.best_parameters = self.es.result[0]
+        self.best_reward = -self.es.result[1]
+
+        # set the best parameters
+        self.policy.set_vectorized_parameters(self.best_parameters)
+
+        return avg_rewards, max_rewards
+
+    def test(self, num_steps=1000, dt=0, use_terminating_condition=False, render=True):
+        return self.task.run(num_steps=num_steps, dt=dt, use_terminating_condition=use_terminating_condition,
+                             render=render)

pyrobolearn/algos/fd.py

@@ -0,0 +1,188 @@
+#!/usr/bin/env python
+"""Provide the Finite-Difference (FD) method algorithm.
+
+This FD method is a policy gradient algorithm that explores in the parameter space of the policy in an episodic way.
+"""
+
+import numpy as np
+import torch
+
+from pyrobolearn.envs import Env
+from pyrobolearn.tasks import RLTask
+# from rl_algo import GradientRLAlgo
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class FD(object):  # GradientRLAlgo):
+    r"""Finite-Difference Policy Gradient Method.
+
+    Type: policy gradient based (on-policy be definition) with exploration in the parameter space
+
+    The goal of RL is to maximize the expected return:
+
+    .. math:: J(\theta) = \int p(\tau) R(\tau) d\tau
+
+    The Finite-Difference (FD) algorithm perturbs the parameter space of the policy and evaluate for each perturbation
+    the expected return J(\theta_i + \Delta_{\theta_i})
+
+    The gradient :math:`g_{FD} \approx \nabla_\theta J`is then given by:
+
+    .. math:: `g_{FD} = (\Delta\Theta^\top \Delta\Theta)^{-1} \Delta\Theta^\top \Delta J`
+
+    which is used to to perform a gradient ascent step: :math:`\theta_{i+1} = \theta_{i} + \eta g_{FD}`,
+    where :math:`\eta` is the learning rate coefficient.
+
+    Properties:
+        * Exploration is performed in the parameter space of the policy
+    Pros:
+        * Easy to implement and test
+        * work with deterministic and stochastic rl
+        * highly efficient in simulation
+    Cons:
+        * the perturbation of the parameters is hard (especially with systems that can go unstable)
+        * O(M^3) for the time complexity (because of the matrix inversion), where M is the number of parameters
+
+    Pseudo-algo:
+
+    References:
+        [1] "Policy Gradient Methods" (http://www.scholarpedia.org/article/Policy_gradient_methods), Peters, 2010
+    """
+
+    def __init__(self, task, policy, num_variations=None, std_dev=0.01, difference_type='central', learning_rate=0.001,
+                 normalize_grad=False, num_workers=1):
+        # hyperparameters
+        """
+        Initialize the FD algorithm.
+
+        Args:
+            task (RLTask, Env): RL task/env to run
+            policy (Policy): specify the policy (model) to optimize
+            num_variations (None, int): number of times we vary the parameters by a small different increment.
+                If None, it will be twice the number of parameters as according to [1], it yields very accurate
+                gradient estimates.
+            std_dev (float): the small increments are generated from a Normal distribution center at 0 and
+            difference_type (str): there are two difference type of estimators: 'forward' or 'central'.
+                The forward-difference estimator computes the gradient using
+                :math:`J(\theta + \Delta\theta) - J(\theta)`, while the central-difference estimator computes the
+                gradient using :math:`J(\theta + \Delta\theta) - J(\theta - \Delta\theta)`
+            learning_rate (float): learning rate (=coefficient) for the gradient ascent step
+            normalize_grad (bool): specify if we should normalize the gradients
+            num_workers (int): number of workers/jobs to run in parallel
+        """
+        # create explorer
+        # create evaluator
+        # create updater
+        # super(FD, self).__init__(self, explorer, evaluator, updater, num_workers=1)
+
+        if isinstance(task, Env):
+            task = RLTask(task, policy)
+        self.task = task
+        self.policy = policy
+        self.num_workers = num_workers
+
+        # set the number of variations (small increments to vary the parameters)
+        # From [1]: "Empirically it can be observed that taking the number of variations as twice the number
+        # of parameters yields very accurate gradient estimates"
+        if num_variations is None:
+            self.num_variations = 2 * self.policy.num_parameters
+
+        # set standard deviation
+        self.stddev = np.abs(std_dev)
+
+        # set difference type
+        if difference_type != 'forward' and difference_type != 'central':
+            raise ValueError("Expecting the 'difference_type' argument to be 'forward' or 'central'. Instead got "
+                             "'{}'".format(difference_type))
+        self.difference_type = difference_type
+
+        # set other parameters
+        self.lr = learning_rate
+        self.normalize_grad = bool(normalize_grad)
+
+        # remember best parameters
+        self.best_reward = -np.infty
+        self.best_parameters = None
+
+    def explore_and_evaluate(self, params, num_steps, num_rollouts):
+        # set policy parameters
+        self.policy.set_vectorized_parameters(params)
+
+        # run a number of rollouts
+        reward = []
+        for rollout in range(num_rollouts):
+            rew = self.task.run(num_steps=num_steps, use_terminating_condition=True, render=False)
+            reward.append(rew)
+        reward = np.mean(reward)
+
+        return reward
+
+    def train(self, num_steps=1000, num_rollouts=1, num_episodes=1, verbose=False, seed=None):
+        # set seed
+        if seed is not None:
+            np.random.seed(seed)
+
+        # for each episode
+        rewards = []
+        for episode in range(num_episodes):
+
+            # get parameters
+            params = self.policy.get_vectorized_parameters()
+            J_plus, J_minus = np.zeros(self.num_variations), np.zeros(self.num_variations)
+            Delta_Params = np.zeros((self.num_variations, len(params)))
+
+            # evaluate with the current parameters
+            J = self.explore_and_evaluate(params, num_steps, num_rollouts)
+            rewards.append(J)
+
+            # Save best reward and associated parameter
+            if J > self.best_reward:
+                self.best_reward = J
+                self.best_parameters = params
+
+            # print info
+            if verbose:
+                print('\nEpisode {} - expected return: {}'.format(episode + 1, J))
+
+            # 1. Explore
+            for i in range(self.num_variations):
+                # sample parameter increment step vector
+                delta_params = np.random.normal(loc=0.0, scale=self.stddev, size=len(params))
+                Delta_Params[i] = delta_params
+
+                # estimate J(\theta + \delta)
+                new_params = params + delta_params
+                J_plus[i] = self.explore_and_evaluate(new_params, num_steps, num_rollouts)
+
+                # estimate J(\theta - \delta)
+                if self.difference_type == 'forward':
+                    J_minus[i] = J
+                elif self.difference_type == 'central':
+                    new_params = params - delta_params
+                    J_minus[i] = self.explore_and_evaluate(new_params, num_steps, num_rollouts)
+                else:
+                    raise ValueError("Expecting the 'difference_type' argument to be 'forward' or 'central'. "
+                                     "Instead got '{}'".format(self.difference_type))
+            # 2. Evaluate
+
+            # 3. Update
+            delta_J = J_plus - J_minus
+            grad = np.linalg.pinv(Delta_Params).dot(delta_J)
+            if self.normalize_grad:
+                grad /= np.linalg.norm(grad)
+            params = params + self.lr * grad  # TODO: allows the user to choose the optimizer
+            # self.optimizer.optimize(self.policy.get_params(), grad)
+            self.policy.set_vectorized_parameters(params)
+
+        return rewards
+
+    def test(self, num_steps=1000, dt=0, use_terminating_condition=False, render=True):
+        return self.task.run(num_steps=num_steps, dt=dt, use_terminating_condition=use_terminating_condition,
+                             render=render)

pyrobolearn/algos/neat_algo.py

@@ -0,0 +1,185 @@
+#!/usr/bin/env python
+"""Define the NEAT algorithm.
+
+This uses the Neuro-Evolution through Augmenting topologies (NEAT) framework. It allows the evolution of not only the
+parameters/weights but also the topological structure of neural networks. Note that the following algorithm only
+works with neural networks and is thus tightly coupled with its associated policy/model.
+"""
+
+import numpy as np
+
+from pyrobolearn.envs import Env
+from pyrobolearn.tasks import RLTask
+from pyrobolearn.policies import NEATPolicy
+# from rl_algo import RLAlgo
+
+try:
+    import neat
+    # from neat import nn, population, config, statistics
+except ImportError as e:
+    raise ImportError(e.__str__() + "\n HINT: you can install NEAT directly via 'pip install neat-python'.")
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class NEAT(object):  # RLAlgo):
+    r"""NEAT: Neuro-Evolution through Augmenting Topologies
+
+    Exploration is carried out in the parameter and hyperparameter spaces.
+
+    Warnings: This algorithm is a little bit special and currently only works with the associated policy/learning model.
+
+    References:
+        [1] "Evolving Neural Networks through Augmenting Topologies", Stanley et al., 2002
+        [2] NEAT-Python
+            - documentation: https://neat-python.readthedocs.io/en/latest/index.html
+            - github repo: https://github.com/CodeReclaimers/neat-python
+        [3] PyTorch NEAT (built upon NEAT-Python): https://github.com/uber-research/PyTorch-NEAT
+    """
+
+    def __init__(self, task, policy, population_size=20, species_elitism=2, elitism=2, min_species_size=2,
+                 survival_threshold=0.2, max_stagnation=15, compatibility_threshold=3, num_workers=1):
+        r"""
+        Initialize the NEAT algorithm.
+
+        Args:
+            task (RLTask, Env): RL task/env to run
+            policy (Policy): specify the policy (model) to optimize
+            population_size (int): size of the population
+            elitism (int): The number of most-fit individuals in each species that will be preserved as-is from one
+                generation to the next.
+            num_workers (int): number of workers/jobs to run in parallel
+        """
+        # create explorer
+        # create evaluator
+        # create updater
+        # super(NEAT, self).__init__(self, explorer, evaluator, updater, num_workers=1)
+
+        # set task
+        if isinstance(task, Env):
+            task = RLTask(task, policy)
+        self.task = task
+
+        # set policy
+        if policy is None:
+            policy = self.task.policies[0]  # TODO: currently assume only 1 policy
+        if not isinstance(policy, NEATPolicy):
+            raise TypeError("Expecting the policy to be an instance of 'NEATPolicy'.")
+        self.policy = policy
+
+        # set config file
+        # more info about genome's config file: https://neat-python.readthedocs.io/en/latest/config_file.html
+        # more info about activation fct: https://neat-python.readthedocs.io/en/latest/activation.html
+        config_dict = {'[NEAT]': {'fitness_criterion': 'max',
+                                  'fitness_threshold': 100,
+                                  'no_fitness_termination': True,
+                                  'pop_size': population_size,
+                                  'reset_on_extinction': True},
+                       '[DefaultSpeciesSet]': {'compatibility_threshold': compatibility_threshold},
+                       '[DefaultStagnation]': {'species_fitness_func': 'max',
+                                               'max_stagnation': max_stagnation,
+                                               'species_elitism': species_elitism},
+                       '[DefaultReproduction]': {'elitism': elitism,
+                                                 'survival_threshold': survival_threshold,
+                                                 'min_species_size': min_species_size}}
+
+        # update config file of policy
+        self.policy.update_config(config_dict)
+
+        # get population
+        self.population = self.policy.population
+
+        # create useful variables
+        self.num_steps = 1000
+        self.num_rollouts = 1
+        self.verbose = False
+        self.episode = 0
+        self.avg_rewards, self.max_rewards = [], []
+
+        self.best_reward = -np.infty
+        self.best_parameters = None
+
+    def explore_and_evaluate(self, genomes, config):
+        # print info
+        self.episode += 1
+        if self.verbose:
+            print('\nEpisode {}'.format(self.episode))
+
+        # for each individual in the population, evaluate it on the task
+        rewards = []
+        for genome_id, genome in genomes:
+            # set genome
+            self.policy.set_network(genome, config)
+
+            # run a number of rollouts
+            reward = []
+            for rollout in range(self.num_rollouts):
+                # run the task
+                rew = self.task.run(num_steps=self.num_steps, use_terminating_condition=True, render=False)
+                reward.append(rew)
+
+            # run in parallel
+            # jobs = [self.pool.apipe(evaluate, NEAT_Agent, self.env, self.cfg['num_steps'],
+            #                         genome, typeNN=self.type) for genome in genomes]
+            # for job, genome in zip(jobs, genomes):
+            #     _, traj = job.get()
+            #     genome.fitness = traj['tot_reward']
+
+            # set fitness value
+            reward = np.mean(reward)
+            genome.fitness = reward
+            rewards.append(reward)
+
+            # save best reward and associated parameter/genome
+            if reward > self.best_reward:
+                self.best_reward = reward
+                self.best_parameters = genome
+
+            # print info
+            if self.verbose:
+                print('  -- Genome id {} - reward: {}'.format(genome_id, reward))
+
+        # append rewards
+        self.avg_rewards.append(np.mean(rewards))
+        self.max_rewards.append(np.max(rewards))
+
+        # print info
+        if self.verbose:
+            print('Episode {}: average reward = {} and best reward = {}'.format(self.episode, self.avg_rewards[-1],
+                                                                                self.max_rewards[-1]))
+
+    def optimize(self):
+        pass
+
+    def train(self, num_steps=1000, num_rollouts=1, num_episodes=1, verbose=False):
+        # set few variables
+        self.num_steps = num_steps
+        self.num_rollouts = num_rollouts
+        self.episode = 0
+        self.verbose = verbose
+        self.avg_rewards, self.max_rewards = [], []
+
+        # run the algo for the specified number of generations / episodes
+        winner = self.population.run(self.explore_and_evaluate, num_episodes)
+
+        # print best reward
+        if verbose:
+            print("\nBest reward found: {}".format(self.best_reward))
+
+        # set the best genome
+        self.policy.genome = winner
+
+        # return the average rewards and max rewards per generation
+        return self.avg_rewards, self.max_rewards
+
+    def test(self, num_steps=1000, dt=0, use_terminating_condition=False, render=True):
+        return self.task.run(num_steps=num_steps, dt=dt, use_terminating_condition=use_terminating_condition,
+                             render=render)

pyrobolearn/algos/power.py

@@ -0,0 +1,231 @@
+#!/usr/bin/env python
+"""Provide the PoWER reinforcement learning algorithm.
+
+This Policy learning by Weighting Exploration with the Returns (PoWER) algorithm is an model-free, on-policy, and
+Expectation-Maximization (EM) algorithm. The exploration is carried out in the parameter space.
+"""
+
+import numpy as np
+import sys
+
+from pyrobolearn.envs import Env
+from pyrobolearn.tasks import RLTask
+# from rl_algo import EMRLAlgo
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class PoWER(object):  # EMRLAlgo):
+    r"""PoWER: Policy learning by Weighting Exploration with the Returns
+
+    Type:: this is a model-free, on-policy, episode-based, EM (Expectation-Maximization) algorithm.
+
+    .. math:: TODO
+
+    Properties:
+        * The learning model is linear with respect to the parameters (in order to compute the close form solution)
+        * Exploration is performed in the parameter space with a gaussian distribution over the parameters
+    Pros:
+        * Pros of using an EM algorithm (improvement on the lower bound)
+    Cons:
+        * Initialization of the EM algorithm is crucial; it works better when providing first few demonstrations
+            of the task to complete
+        * the rewards have to be strictly positives
+        * doesn't work with nonlinear models (in terms of the parameters)
+
+    .. note::
+
+        * the reward has to be strictly positive in order to be a proper probability distribution (see [1])
+        * this algorithm is popular and works well with Dynamic Movement Primitives
+
+    The code is based on [2].
+
+    Pseudocode:
+        1. Exploration:
+        2. Evaluation:
+        3. Update:
+
+    Examples:
+        TODO
+
+    References:
+        [1] "Policy Search for Motor Primitives in Robotics", Kober et al., 2010
+        [2] Original Matlab code (Kober): http://www.ausy.tu-darmstadt.de/uploads/Member/JensKober/matlab_PoWER.zip
+    """
+
+    def __init__(self, task, policy, std_params=1., num_best_rollouts=10, num_workers=1):
+        """
+        Initialize the PoWER algorithm.
+
+        Args:
+            task (RLTask, Env): RL task/env to run
+            policy (Policy): specify the policy (model) to optimize
+            std_params (float):
+        """
+        # create explorer
+        # create evaluator
+        # create updater
+        # super(PoWER, self).__init__(task, exploration_strategy, memory, hyperparameters)
+
+        # set task
+        if isinstance(task, Env):
+            task = RLTask(task, policy)
+        if not isinstance(task, RLTask):
+            raise TypeError("Expecting task to be an instance of RLTask.")
+        self.task = task
+
+        # set policy
+        self.policy = policy
+        if not self.policy.is_parametric():
+            raise ValueError("The policy should be parametric")
+        if not self.policy.is_linear():
+            raise ValueError("The policy should be linear with respect to the parameters")
+
+        # set standard deviation of the parameters
+        self.std_params = std_params
+
+        # set num best rollouts for memory
+        self.num_best_rollouts = num_best_rollouts
+
+        # remember best parameters
+        self.best_reward = -np.infty
+        self.best_parameters = None
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def std_params(self):
+        return self._std_params
+
+    @std_params.setter
+    def std_params(self, std_params):
+        if std_params < 0.:
+            std_params = 1.
+        self._std_params = std_params
+
+    ###########
+    # Methods #
+    ###########
+
+    def explore_and_evaluate(self, params, num_steps, num_rollouts):
+        # set policy parameters
+        self.policy.set_vectorized_parameters(params)
+
+        # run a number of rollouts
+        reward = []
+        for rollout in range(num_rollouts):
+            rew = self.task.run(num_steps=num_steps, use_terminating_condition=True, render=False)
+            if rew <= 0:
+                raise ValueError("With PoWER, the reward must be strictly positive.")
+            reward.append(rew)
+        reward = np.mean(reward)
+
+        return reward
+
+    # def train(self, num_episodes=1, num_steps=100, std_params=1., task=None, seed=None, debug=False):
+    def train(self, num_steps=1000, num_rollouts=1, num_episodes=1, verbose=False, seed=None):
+        # check parameters
+        if num_episodes < 1:
+            num_episodes = 1
+        if num_steps < 1:
+            num_steps = 1
+        if num_rollouts < 1:
+            num_rollouts = 1
+
+        # set random seed
+        if seed is not None:
+            np.random.seed(seed)
+
+        # Initial policy parameters
+        num_params = self.policy.num_parameters
+        w = self.policy.get_vectorized_parameters()
+
+        # define exploration parameters
+        mean = np.zeros(num_params)
+        C_init = np.ones(num_params) * np.sqrt(self.std_params)
+        C = C_init
+
+        # Evaluate reward with current parameter
+        reward = self.explore_and_evaluate(w, num_steps=num_steps, num_rollouts=num_rollouts)
+
+        # remember best set of weights and best reward
+        if self.best_reward < reward:
+            self.best_reward, self.best_parameters = reward, np.copy(w)
+
+        # print info
+        if verbose:
+            print('Episode {}/{} with current and best reward: {}, {}'.format(0, num_episodes, reward,
+                                                                              self.best_reward))
+
+        # for each episode
+        rewards, memory = [reward], []
+        memory = []
+        for episode in range(num_episodes):
+
+            # 1. Explore
+            # explore and sample
+            eps = np.random.multivariate_normal(mean, np.diag(C))
+            weps = w + eps
+            reward = self.explore_and_evaluate(weps, num_steps=num_steps, num_rollouts=num_rollouts)
+            rewards.append(reward)
+
+            # remember best set of weights and best reward
+            if self.best_reward < reward:
+                self.best_reward, self.best_parameters = reward, weps
+
+            # print info
+            if verbose:
+                print('Episode {}/{} with current and best reward: {}, {}'.format(episode+1, num_episodes, reward,
+                                                                                  self.best_reward))
+            # record in memory
+            memory.append((reward, weps, C))
+
+            # 2. Evaluate
+            # Reweight: importance sampling
+            list.sort(memory, key=lambda x: x[0])  # in-place operation
+            memory = memory[-self.num_best_rollouts:]  # just keep the best rollouts
+
+            # 3. Update
+            # Update weight parameters
+            num, den = 0, 0
+            for (rew, weps, Ceps) in memory[-self.num_best_rollouts:]:
+                # prec = np.linalg.inv(Ceps)
+                prec = np.linalg.inv(np.diag(Ceps))
+                eps = weps - w
+                num += prec.dot(eps) * rew
+                den += prec * rew
+            w = w + np.linalg.inv(den + 1e-8).dot(num)
+
+            # Update Covariance matrix
+            num, den = 0, 0
+            for (rew, weps, Ceps) in memory[-self.num_best_rollouts:]:
+                eps = weps - w
+                # num += eps.dot(eps.T)*rew
+                num += (eps ** 2) * rew
+                den += rew
+            C = num / (den + 1e-10)
+
+            # Apply an upper and lower limit to the exploration (so we still get a kind of exploration)
+            C = np.minimum(np.maximum(C, 0.1 * C_init), 10. * C_init)
+
+        # print best reward
+        if verbose:
+            print("\nBest reward found: {}".format(self.best_reward))
+
+        # set the best parameters
+        self.policy.set_vectorized_parameters(self.best_parameters)
+
+        return rewards
+
+    def test(self, num_steps=1000, dt=0, use_terminating_condition=False, render=True):
+        return self.task.run(num_steps=num_steps, dt=dt, use_terminating_condition=use_terminating_condition,
+                             render=render)

pyrobolearn/models/nn/ae.py

@@ -28,7 +28,7 @@ from dnn import NN
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/cnn.py

@@ -28,7 +28,7 @@ from dnn import NN
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/dnn.py

@@ -27,7 +27,7 @@ import torch
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/gan.py

@@ -30,7 +30,7 @@ from dnn import NN
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/mlp.py

@@ -27,7 +27,7 @@ from dnn import NN, NNTorch
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/neat_model.py

@@ -19,7 +19,7 @@ except ImportError as e:
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/rcnn.py

@@ -29,7 +29,7 @@ from dnn import NN
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/rnn.py

@@ -28,7 +28,7 @@ from dnn import NN
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/models/nn/vae.py

@@ -28,7 +28,7 @@ from dnn import NN
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/actuators/actuator.py

@@ -10,7 +10,7 @@ simulation to reality.
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/actuators/joints.py

@@ -9,7 +9,7 @@ from actuator import Actuator
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/base.py

@@ -8,7 +8,7 @@ import quaternion
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/camera.py

@@ -11,7 +11,7 @@ from links import LinkSensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/contact.py

@@ -7,7 +7,7 @@ from links import LinkSensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/force_torque.py

@@ -9,7 +9,7 @@ from joints import JointSensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/imu.py

@@ -11,7 +11,7 @@ from links import LinkSensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/joints.py

@@ -11,7 +11,7 @@ from sensor import Sensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/light.py

@@ -9,7 +9,7 @@ from links import LinkSensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/links.py

@@ -11,7 +11,7 @@ from sensor import Sensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/misc.py

@@ -7,7 +7,7 @@ from links import Sensor, LinkSensor
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/robots/sensors/sensor.py

@@ -17,7 +17,7 @@ from pyrobolearn.utils.converter import QuaternionListConverter, NumpyListConver
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2018, PyRoboLearn"
 __credits__ = ["Brian Delhaisse"]
-__license__ = "(c) Brian Delhaisse"
+__license__ = "MIT"
 __version__ = "1.0.0"
 __maintainer__ = "Brian Delhaisse"
 __email__ = "briandelhaisse@gmail.com"

pyrobolearn/tools/README.md

@@ -0,0 +1,8 @@
+## Tools
+
+This folder the *interfaces* and the *bridges*.
+
+* The I/O *interfaces* get (or set) the data from (to) the hardware, process it, and store it inside the class. 
+* The *bridges* makes the connection between the interface and a component (such as the world or an element in that world such as a robot) in the framework.
+
+The separation between interfaces and bridges allows for better flexibility. For instance, a game controller interface allows us to get data from the hardware, process it, and store it inside the class. The bridge can then map the specific controller events to a robot. Moving a joystick up could mean to move a UAV robot up in the air, or move a wheeled robot forward. 

pyrobolearn/tools/__init__.py

@@ -0,0 +1,6 @@
+
+# import interfaces
+# import interfaces
+
+# import bridges
+# import bridges

pyrobolearn/tools/bridges/__init__.py

@@ -0,0 +1,19 @@
+# a bridge links an interface with something else
+
+# general bridge import
+from bridge import Bridge
+
+# Bridge for mouse-keyboard interfaces
+from mouse_keyboard import *
+
+# Bridge for audio interfaces
+# from audio import *
+
+# Bridge for camera interfaces
+# from camera import *
+
+# Bridge for controller interfaces
+# from controllers import *
+
+# Bridge for VR interfaces
+# from vr import *

pyrobolearn/tools/bridges/audio/__init__.py

@@ -0,0 +1,8 @@
+
+## Bridge for audio interfaces ##
+
+# Bridge between audio and robot
+from robots import *
+
+# Bridge between audio and world
+from world import *

pyrobolearn/tools/bridges/audio/robots/__init__.py

@@ -0,0 +1,6 @@
+
+# bridge between audio and wheeled robot
+from bridge_speech_wheeled import *
+
+# bridge between audio and rototary wing UAV
+from bridge_speech_rotatory_uav import *

pyrobolearn/tools/bridges/audio/robots/bridge_speech_rotatory_uav.py

@@ -0,0 +1,51 @@
+# Bridges between audio interface and rotatory wing robots
+
+from pyrobolearn.robots import RotaryWingUAV
+from pyrobolearn.tools.interfaces.audio import SpeechRecognizerInterface
+from pyrobolearn.tools.bridges.bridge import Bridge
+
+
+class BridgeSpeechRecognizerRotatoryUAV(Bridge):
+    r"""Bridge Speech Wheeled Robot
+
+    Bridge between the speech recognizer interface and a wheeled robot. You can give oral orders to the robot.
+    """
+
+    def __init__(self, interface, uav_robot, init_speed=1.):
+        if not isinstance(interface, SpeechRecognizerInterface):
+            raise TypeError("Expecting a speech recognizer interface")
+        if not isinstance(uav_robot, RotaryWingUAV):
+            raise TypeError("Expecting a wheeled robot")
+        super(BridgeSpeechRecognizerRotatoryUAV, self).__init__(interface)
+        self.robot = uav_robot
+        self.speed = init_speed
+
+    def step(self):
+        data = self.interface.data
+        data = data.split()
+        # print('data: {}'.format(data))
+        if data[0] == 'stop' or data[0] == 'stay':
+            self.robot.stop()
+        elif data[-1] == 'higher':
+            pass
+        elif data[-1] == 'lower':
+            pass
+        elif data[-1] == 'forward':
+            pass
+        elif data[-1] == 'backward':
+            pass
+        elif data == 'turn right':
+            pass
+        elif data == 'turn left':
+            pass
+        elif data[-1] == 'right':
+            pass
+        elif data[-1] == 'left':
+            pass
+        elif data[-1] == 'faster':
+            self.speed *= 2.
+        elif data[-1] == 'slower':
+            self.speed /= 2.
+        elif data:
+            pass
+            # print('I do not know the meaning of {}'.format(data))

pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py

@@ -0,0 +1,83 @@
+# Bridges between audio interface and wheeled robots
+
+import numpy as np
+
+from pyrobolearn.robots import WheeledRobot, AckermannWheeledRobot
+from pyrobolearn.tools.interfaces.audio import SpeechRecognizerInterface
+from pyrobolearn.tools.bridges.bridge import Bridge
+
+
+class BridgeSpeechRecognizerWheeledRobot(Bridge):
+    r"""Bridge Speech Wheeled Robot
+
+    Bridge between the speech recognizer interface and a wheeled robot. You can give oral orders to the robot.
+    """
+
+    def __init__(self, interface, wheeled_robot, init_speed=1.):
+        if not isinstance(interface, SpeechRecognizerInterface):
+            raise TypeError("Expecting a speech recognizer interface")
+        if not isinstance(wheeled_robot, WheeledRobot):
+            raise TypeError("Expecting a wheeled robot")
+        super(BridgeSpeechRecognizerWheeledRobot, self).__init__(interface)
+        self.robot = wheeled_robot
+        self.speed = init_speed
+
+    def step(self):
+        data = self.interface.data
+        #print('data: {}'.format(data))
+        if data == 'stop':
+            self.robot.stop()
+        elif data == 'move forward':
+            self.robot.driveForward(self.speed)
+        elif data == 'move backward':
+            self.robot.driveBackward(self.speed)
+        elif data == 'turn right':
+            pass
+        elif data == 'turn left':
+            pass
+        elif data == 'faster':
+            self.speed *= 2.
+        elif data == 'slower':
+            self.speed /= 2.
+        elif data:
+            pass
+            #print('I do not know the meaning of {}'.format(data))
+
+
+class BridgeSpeechRecognizerAckermannWheeledRobot(Bridge):
+    r"""Bridge Speech Ackermann Wheeled Robot
+
+    Bridge between the speech recognizer interface and a wheeled robot (with ackermann steering). You can give oral
+    orders to the robot.
+    """
+
+    def __init__(self, interface, wheeled_robot, init_speed=1.):
+        if not isinstance(interface, SpeechRecognizerInterface):
+            raise TypeError("Expecting a speech recognizer interface")
+        if not isinstance(wheeled_robot, AckermannWheeledRobot):
+            raise TypeError("Expecting a wheeled robot of type Ackermann steering")
+        super(BridgeSpeechRecognizerAckermannWheeledRobot, self).__init__(interface)
+        self.robot = wheeled_robot
+        self.speed = init_speed
+        self.steering_angle = 0.
+
+    def step(self):
+        data = self.interface.data
+        #print('data: {}'.format(data))
+        if data == 'stop':
+            self.robot.stop()
+        elif data == 'move forward':
+            self.robot.driveForward(self.speed)
+        elif data == 'move backward':
+            self.robot.driveBackward(self.speed)
+        elif data == 'turn right':
+            self.robot.setSteering(np.deg2rad(-20))
+        elif data == 'turn left':
+            self.robot.setSteering(np.deg2rad(20))
+        elif data == 'faster':
+            self.speed *= 2.
+        elif data == 'slower':
+            self.speed /= 2.
+        elif data:
+            pass
+            #print('I do not know the meaning of {}'.format(data))

pyrobolearn/tools/bridges/bridge.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python
+"""Define the Bridge class.
+
+The Bridge class links an interface with something else. This can be the world (such as a robot, a camera,
+a controller), a state / action, an environment, or a task. This is the main parent class from which any other
+bridges inherit from.
+"""
+
+from pyrobolearn.tools.interfaces import Interface
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class Bridge(object):
+    r"""Bridge class
+
+    The Bridge class links an interface with something else in the world, such as a robot, a camera, a controller, etc.
+    This is the main parent class from which any other bridges inherit from.
+
+    More specifically, the interface can be run in a separate thread or in the same thread than the main one.
+    The interface collects the input data/events and/or outputs the given data/events, but the interface does not
+    know how to connect to the rest of the code. The bridge is the one that possesses this knowledge
+    (see Bridge/Adapter design pattern). There can be multiple interfaces running (like for the audio and webcam),
+    and each interface can have multiple bridges. However, each bridge links only one specific interface to something
+    else.
+
+    For instance, let's say we have an Xbox controller interface, and we want to control a quadcopter robot and
+    a wheeled robot. Everytime pushing the joystick forward results the interface to remember such event, but this
+    does not result in anything in the simulator. The associated bridge such as `BridgeXboxUAV` and
+    `BridgeUAVWheeled` knows what should be done if the joystick is moved forward. In the case of the quadcopter,
+    the bridge could for instance increase the speed of the propellers, and for the wheeled robot could result
+    to move this one forward. There could be other bridges linking the Xbox controller to other things in the world,
+    like the world camera, a visual object, etc. An interface can have multiple bridges, e.g. a bridge linking the
+    same Xbox controller to different cameras in the world. Pushing the joystick will result in the same movements
+    for all these cameras.
+    """
+
+    def __init__(self, interface, priority=None):
+        self.interface = interface
+        self.priority = priority
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def interface(self):
+        return self._interface
+
+    @interface.setter
+    def interface(self, interface):
+        if not isinstance(interface, Interface):
+            raise TypeError("Expecting interface to be an instance of Interface, instead got {}".format(interface))
+        self._interface = interface
+
+    @property
+    def priority(self):
+        return self._priority
+
+    @priority.setter
+    def priority(self, priority):
+        if priority is not None:
+            if not isinstance(priority, int):
+                raise TypeError("Expecting the priority to be an integer, instead got {}".format(priority))
+        self._priority = priority
+
+    ###########
+    # Methods #
+    ###########
+
+    def step(self, update_interface=False):
+        """Main function that has to be overwritten by the user."""
+        raise NotImplementedError
+
+    def __call__(self, update_interface=False):
+        self.step(update_interface=update_interface)

pyrobolearn/tools/bridges/camera/__init__.py

@@ -0,0 +1,8 @@
+
+## Bridge for camera interfaces ##
+
+# Bridge between camera and robot
+from robots import *
+
+# Bridge between camera and world
+from world import *

pyrobolearn/tools/bridges/controllers/__init__.py

@@ -0,0 +1,8 @@
+
+## Bridge for controller interfaces ##
+
+# Bridge between controller and robot
+from robots import *
+
+# Bridge between controller and world
+from world import *

pyrobolearn/tools/bridges/controllers/robots/__init__.py

@@ -0,0 +1,3 @@
+
+# bridge between controller and wheeled robot
+from bridge_controller_wheeled import *

pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py

@@ -0,0 +1,79 @@
+
+# Bridges between controller interface and wheeled robots
+
+from pyrobolearn.robots import WheeledRobot, AckermannWheeledRobot
+from pyrobolearn.tools.interfaces.controllers import XboxControllerInterface, XboxOneControllerInterface
+from pyrobolearn.tools.bridges.bridge import Bridge
+
+
+class BridgeXboxWheeledRobot(Bridge):
+    r"""Bridge Xbox Wheeled Robot
+
+    Bridge between the Xbox controller interface and a wheeled robot. You can move the robot using the joystick.
+    """
+
+    def __init__(self, interface, wheeled_robot):
+        # quick checks
+        if not isinstance(interface, XboxControllerInterface):
+            raise TypeError("Expecting a speech recognizer interface")
+        if not isinstance(wheeled_robot, WheeledRobot):
+            raise TypeError("Expecting a wheeled robot")
+
+        # call super class
+        super(BridgeXboxWheeledRobot, self).__init__(interface)
+        self.robot = wheeled_robot
+        self.speed = 1.
+
+    def step(self):
+        x,y = self.interface.LJ
+
+
+class BridgeXboxOneWheeledRobot(Bridge):
+    r"""Bridge Xbox Wheeled Robot
+
+    Bridge between the Xbox One controller interface and a wheeled robot. You can move the robot using the joystick.
+    """
+
+    def __init__(self, interface, wheeled_robot):
+        # quick check
+        if not isinstance(interface, XboxOneControllerInterface):
+            raise TypeError("Expecting a speech recognizer interface")
+        if not isinstance(wheeled_robot, WheeledRobot):
+            raise TypeError("Expecting a wheeled robot")
+
+        super(BridgeXboxOneWheeledRobot, self).__init__(interface)
+        self.robot = wheeled_robot
+        self.speed = 1.
+
+    def step(self):
+        x,y = self.interface.LJ
+
+
+class BridgeXboxOneAckermannWheeledRobot(Bridge):
+    r"""Bridge Xbox One Ackermann Wheeled Robot
+
+    Bridge between the Xbox One controller interface and a wheeled robot. You can move the robot using the joystick.
+    """
+
+    def __init__(self, interface, wheeled_robot):
+        if not isinstance(interface, XboxOneControllerInterface):
+            raise TypeError("Expecting a speech recognizer interface")
+        if not isinstance(wheeled_robot, AckermannWheeledRobot):
+            raise TypeError("Expecting a wheeled robot")
+        super(BridgeXboxOneAckermannWheeledRobot, self).__init__(interface)
+        self.robot = wheeled_robot
+        self.speed = 1.
+
+    def step(self):
+        x,y = self.interface.LJ
+        self.robot.setSteering(-x / 2.)
+        self.robot.driveForward(y * self.speed)
+
+        if self.interface.A:
+            print('increasing speed +1')
+            self.speed += 1.
+        if self.interface.B:
+            print('decreasing speed -1')
+            self.speed -= 1.
+            if self.speed < 1.:
+                self.speed = 1.

pyrobolearn/tools/bridges/mouse_keyboard/__init__.py

@@ -0,0 +1,4 @@
+
+# import
+from bridge_mousekeyboard_world import BridgeMouseKeyboardWorld
+from bridge_mousekeyboard_imitation_task import BridgeMouseKeyboardImitationTask

pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py

@@ -0,0 +1,375 @@
+#!/usr/bin/env python
+"""Define the Bridge between the mouse-keyboard interface and the world.
+
+Dependencies:
+- `pyrobolearn.utils`
+- `pyrobolearn.worlds` or `pyrobolearn.envs`
+- `pyrobolearn.tools.interfaces.MouseKeyboardInterface`
+- `pyrobolearn.tools.bridges.Bridge`
+"""
+
+from pyrobolearn.utils.bullet_utils import RGBColor, Key
+
+from pyrobolearn.simulators import Simulator
+from pyrobolearn.worlds import World
+from pyrobolearn.envs import Env
+
+from pyrobolearn.tools.interfaces import MouseKeyboardInterface
+# from pyrobolearn.tools.bridges import Bridge
+from bridge_mousekeyboard_world import BridgeMouseKeyboardWorld
+# from pyrobolearn.tasks.imitation_task import ILTask  # Warning: circular dependency
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class BridgeMouseKeyboardImitationTask(BridgeMouseKeyboardWorld):  # Bridge):
+    r"""Bridge Mouse-Keyboard Imitation Task
+
+    Bridge between the mouse-keyboard and the world.
+
+    Mouse:
+    * predefined in pybullet
+        * `scroll wheel`: zoom
+        * `ctrl`/`alt` + `scroll button`: move the camera using the mouse
+        * `ctrl`/`alt` + `left-click`: rotate the camera using the mouse
+        * `left-click` and drag: transport the object
+    * `left-click`: select/unselect an object (show bounding box, and print name/id in simulator)
+    * `right-click` and drag: perform IK on the selected link until the mouse button is released
+    * `ctrl` + `left-click`: select multiple object
+
+    Keyboard:
+    * predefined in pybullet:
+        * `w`: show the wireframe (collision shapes)
+        * `s`: show the reference system
+        * `v`: show bounding boxes
+        * `g`: show/hide parts of the GUI the side columns (check `sim.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)`)
+        * `esc`: quit the simulator
+    * `x`: change camera view such that it is perpendicular to the x-axis
+    * `y`: change camera view such that it is perpendicular to the y-axis
+    * `z`: change camera view such that it is perpendicular to the z-axis
+    * `j`: after selecting a robot, display the joint sliders to control them
+    * `t`: after selecting a robot link, display the cartesian sliders to control it using IK
+    * `b`: after selecting a link of the robot, display the bounding box around the selected link
+    * `d`: display what the robot sees (rgbd image)
+    * `u`: unselect any objects
+    * `r`: start/stop recording using the given recorder (if multiple recorders, select in the parameter column).
+           The recorders are the ones who know what/how to record and how to deal with the data. For instance,
+           if a policy needs to be trained, you have to interact with the recorder and not the interface.
+    * `f`: save recorded trajectory to file (append)
+    * `c`: clean and reset the given world
+    * `a`: apply force/torque on the selected link/joint using sliders (select the link/joint using the mouse)
+    * `h`: hide/show the complete GUI; enable/disable the rendering
+    * `m`: show/hide the workspace of the corresponding link (not implemented yet)
+    * `p`: show/hide recorded trajectories
+        * `shift`+`p`: remove recorded trajectories
+    * `o`: optimized the given policy to the demonstrated trajectories (if multiple policy, select one policy)
+    * `e`: test the optimized policy associated to the selected robot (if multiple policy, select one policy)
+    * `l`: change mode of locomotion (defined in the robot; select the controller/policy)
+    * `<number>`: select and perform corresponding action (10 actions possible, as <number> between 0 and 9)
+    * `top arrow`: move the selected robot forward
+    * `bottom arrow`: move the selected robot backward
+    * `left arrow`: turn the selected robot to the left
+    * `right arrow`: turn the selected robot to the right
+    * `space`: allow the robot to jump (if implemented)
+    """
+
+    def __init__(self, world, interface=None, imitation_task=None, priority=None, verbose=False):
+        """
+        Initialize the Bridge between a Mouse-Keyboard interface and an imitation learning task.
+
+        Args:
+            interface (MouseKeyboardInterface, Env, World): mouse keyboard interface.
+                If the interface is an instance of Env, World, or Simulator, it will create automatically
+                a mouse-keyboard interface.
+            priority (int): priority of the bridge.
+            verbose (bool): If True, print information on the standard output.
+        """
+        # check interface
+        if not isinstance(interface, MouseKeyboardInterface):
+            if interface is None:
+                sim = world.simulator
+            elif isinstance(interface, Env):
+                sim = interface.simulator
+            elif isinstance(interface, World):
+                sim = interface.simulator
+            elif isinstance(interface, Simulator):
+                sim = interface
+            else:
+                raise TypeError("Expecting interface to be an instance of MouseKeyboardInterface, or World, "
+                                "got instead {}".format(type(interface)))
+            interface = MouseKeyboardInterface(sim)
+
+        # call superclass
+        super(BridgeMouseKeyboardImitationTask, self).__init__(world, interface, priority, verbose=verbose)
+
+        # set task
+        self.task = imitation_task
+
+        # define few variables
+        self.pausing = False
+        self.recording = False
+        self.training = False
+        self.testing = False
+        self.visual_points = []  # {}
+        # self.display_trajectories = True
+
+        self.events_fn = {(Key.x,): self.change_camera_view_x,
+                          (Key.y,): self.change_camera_view_y,
+                          (Key.z,): self.change_camera_view_z,
+                          (Key.enter,): self.reset_camera_view,
+                          (Key.space,): self.pause,  # pause the simulator
+                          (Key.h,): self.gui,  # hide/enable (actually stop/play) GUI
+                          (Key.r,): self.reset_world,  # reset world
+                          (Key.j,): self.update_joint_sliders,  # add/remove joint (space) sliders
+                          (Key.t,): self.update_task_sliders,  # add/remove task (space) sliders
+                          (Key.p,): self.display_trajectories,  # display/hide trajectories
+                          (Key.ctrl, Key.p): self.reset_visual_points,  # remove trajectories
+                          (Key.u,): self.unselect,  # unselect robot/link
+                          (Key.ctrl, Key.r): self.record,  # record
+                          (Key.ctrl, Key.n): self.add_data_row_in_recorder,  # add a new data row in the recorder's
+                                                                             # data "matrix"
+                          (Key.shift, Key.r): self.end_recording,  # end recording
+                          (Key.ctrl, Key.s): self.save_recording,  # save what has been recorded into a file
+                          (Key.ctrl, Key.o): self.train,  # optimize/train the policy
+                          (Key.shift, Key.o): self.end_training,  # end training
+                          (Key.ctrl, Key.t): self.test,  # test the policy
+                          (Key.shift, Key.t): self.end_testing,  # end testing
+                          (Key.shift, Key.space): self.end_task,  # end task
+                          (Key.d,): lambda: None,  # display robot's camera
+                          (Key.m,): lambda: None,  # display link workspace
+                          (Key.b,): lambda: None,  # bounding box
+                          (Key.a,): lambda: None,  # apply force/torque on selected link/joint
+                          (Key.l,): lambda: None,
+                          (Key.n0,): lambda: None,
+                          (Key.n1,): lambda: None,
+                          (Key.n2,): lambda: None,
+                          (Key.n3,): lambda: None,
+                          (Key.n4,): lambda: None,
+                          (Key.n5,): lambda: None,
+                          (Key.n6,): lambda: None,
+                          (Key.n7,): lambda: None,
+                          (Key.n8,): lambda: None,
+                          (Key.n9,): lambda: None,
+                          (Key.top_arrow,): lambda: None,
+                          (Key.bottom_arrow,): lambda: None,
+                          (Key.left_arrow,): lambda: None,
+                          (Key.right_arrow,): lambda: None}
+
+        # define few variables
+        self.enable_training = False
+        self.enable_recording = False
+        self.enable_testing = False
+
+        # self.vs = sim.createVisualShape(sim.GEOM_SPHERE, radius=0.02, rgbaColor=(0, 0, 1, 1))
+        # self.vs1 = sim.createVisualShape(sim.GEOM_SPHERE, radius=0.2, rgbaColor=(1, 0, 0, 1))
+        # self.vs2 = sim.createVisualShape(sim.GEOM_SPHERE, radius=0.2, rgbaColor=(0, 1, 0, 1))
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def pausing(self):
+        """Return True if we need to pause the simulator."""
+        return self._pausing
+
+    @pausing.setter
+    def pausing(self, boolean):
+        """Specify if we should pause the simulator."""
+        self._pausing = boolean
+
+    @property
+    def recording(self):
+        """Return True if we are in recording mode."""
+        return self._recording
+
+    @recording.setter
+    def recording(self, boolean):
+        """Set recording mode."""
+        self._recording = boolean
+
+        # if you are recording, you can not train or test
+        if self._recording:
+            self._training = False
+            self._testing = False
+
+        # notify the task
+        if self.task is not None:
+            self.task.recording_enabled = boolean
+
+    @property
+    def training(self):
+        """Return True if we are in training mode."""
+        return self._training
+
+    @training.setter
+    def training(self, boolean):
+        """Set training mode."""
+        self._training = boolean
+
+        # if you are training, you can not record or test
+        if self._training:
+            self._recording = False
+            self._testing = False
+
+        # notify the task
+        if self.task is not None:
+            self.task.training_enabled = boolean
+
+    @property
+    def testing(self):
+        """Return True if we are in testing mode."""
+        return self._testing
+
+    @testing.setter
+    def testing(self, boolean):
+        """Set testing mode."""
+        self._testing = boolean
+
+        # if you are testing, you can not record or train
+        if self._testing:
+            self._recording = False
+            self._training = False
+
+        # notify the task
+        if self.task is not None:
+            self.task.testing_enabled = boolean
+
+    @property
+    def task(self):
+        """Return the task instance."""
+        return self._task
+
+    @task.setter
+    def task(self, task):
+        """Set the task."""
+        from pyrobolearn.tasks.imitation import ILTask  # to avoid circular dependency
+        if task is not None and not isinstance(task, ILTask):
+            raise("Expecting the imitation task to be an instance of ILTask, instead got {}".format(type(task)))
+        self._task = task
+
+    @property
+    def environment(self):
+        """Return the environment instance."""
+        return self.task.environment
+
+    # @property
+    # def world(self):
+    #     """Return the world instance."""
+    #     return self.task.world
+
+    # @property
+    # def simulator(self):
+    #     """Return the simulator instance."""
+    #     return self.task.simulator
+
+    @property
+    def recorders(self):
+        """Return the recorder."""
+        return self.task.recorders
+
+    ###########
+    # Methods #
+    ###########
+
+    def print_debug(self, str1, str2='', condition=True):
+        if self.debug:
+            if condition:
+                print("MouseKeyboardInterface: "+str1)
+            else:
+                print("MouseKeyboardInterface: "+str2)
+
+    # def step(self, update_interface=False):
+    #     """Perform a step: map the mouse-keyboard interface to the imitation task"""
+    #     # perform a step with the interface
+    #     if update_interface:
+    #         self.interface.step()
+    #
+    #     # # record (if specified)
+    #     # if self.recording:
+    #     #     for recorder in self.recorders:
+    #     #         recorder.record()
+
+    def pause(self):
+        """pause the simulation"""
+        self.pausing = not self.pausing
+        self.print_debug('pause the simulator', 'unpause the simulator', self.pausing)
+
+    def record(self):
+        """Start/Stop recording"""
+        # if self.recorders is not None:
+        self.recording = not self.recording
+        self.print_debug('start recording', 'stop recording', self.recording)
+
+    def add_data_row_in_recorder(self):
+        """Add a new data row in the recorder's data 'matrix'."""
+        if self.task is not None:
+            self.task.add_data_row_in_recorder()
+
+    def save_recording(self):
+        """save recording into file"""
+        if self.task is not None:
+            self.print_debug('saving what has been recorded')
+            self.task.save_recorders()
+
+    def reset_recorder(self):
+        """Reset the recorders."""
+        if self.task is not None:
+            self.print_debug('reset recorders')
+            self.task.reset_recorders()
+
+    def end_recording(self):
+        """End recording."""
+        if self.task is not None:
+            self.task.end_recording = True
+
+    def train(self):
+        """train the policies from the task"""
+        self.print_debug('train the task')
+        self.training = not self.training
+
+    def end_training(self):
+        """End training."""
+        if self.task is not None:
+            self.task.end_training = True
+
+    def test(self):
+        """test the policies from the task"""
+        self.print_debug('test the task')
+        self.testing = not self.testing
+
+    def end_testing(self):
+        """End testing."""
+        if self.task is not None:
+            self.task.end_testing = True
+
+    def end_task(self):
+        """End task."""
+        if self.task is not None:
+            self.task.end_task = True
+
+    def display_trajectories(self):
+        # if self.display_trajectories: # hide them (pybullet doesn't allow that for now, just remove body)
+        #    for key in self.visual_points:
+        #        self.sim.removeBody(self.visual_points[key])
+        # else:
+        #    for key in self.visual_points:
+        #        bodyId = self.sim.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs,
+        #                                          basePosition=key)
+        #        self.visual_points[key] = bodyId
+        # self.display_trajectories = not self.display_trajectories
+        for i in range(len(self.visual_points[:-1])):
+            self.simulator.addUserDebugLine(self.visual_points[i], self.visual_points[i + 1], RGBColor.red, 1., 2.)
+
+    def reset_visual_points(self):
+        # for key in self.visual_points:
+        #    self.sim.removeBody(self.visual_points[key])
+        # self.visual_points.clear()
+        self.visual_points = []

pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py

@@ -0,0 +1,437 @@
+#!/usr/bin/env python
+"""Define the Bridge between the mouse-keyboard interface and the world.
+
+Dependencies:
+- `pyrobolearn.utils`
+- `pyrobolearn.worlds` or `pyrobolearn.envs`
+- `pyrobolearn.tools.interfaces.MouseKeyboardInterface`
+- `pyrobolearn.tools.bridges.Bridge`
+"""
+
+import numpy as np
+
+from pyrobolearn.utils.bullet_utils import RGBColor, Key
+from pyrobolearn.utils.math_utils import Plane
+
+from pyrobolearn.worlds import World, WorldCamera
+from pyrobolearn.envs import Env
+from pyrobolearn.tools.interfaces import MouseKeyboardInterface
+from pyrobolearn.tools.bridges import Bridge
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class BridgeMouseKeyboardWorld(Bridge):
+    r"""Bridge Mouse-Keyboard World
+
+    Bridge between the mouse-keyboard and the world.
+
+    Mouse:
+    * predefined in pybullet
+        * `scroll wheel`: zoom
+        * `ctrl`/`alt` + `scroll button`: move the camera using the mouse
+        * `ctrl`/`alt` + `left-click`: rotate the camera using the mouse
+        * `left-click` and drag: transport the object
+    * `left-click`: select/unselect an object (show bounding box, and print name/id in simulator)
+    * `right-click` and drag: perform IK on the selected link until the mouse button is released
+    * `ctrl` + `left-click`: select multiple object
+
+    Keyboard:
+    * predefined in pybullet:
+        * `w`: show the wireframe (collision shapes)
+        * `s`: show the reference system
+        * `v`: show bounding boxes
+        * `g`: show/hide parts of the GUI the side columns (check `sim.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)`)
+        * `esc`: quit the simulator
+    * `x`: change camera view such that it is perpendicular to the x-axis
+    * `y`: change camera view such that it is perpendicular to the y-axis
+    * `z`: change camera view such that it is perpendicular to the z-axis
+    * `j`: after selecting a robot, display the joint sliders to control them
+    * `t`: after selecting a robot link, display the cartesian sliders to control it using IK
+    * `b`: after selecting a link of the robot, display the bounding box around the selected link
+    * `d`: display what the robot sees (rgbd image)
+    * `u`: unselect any objects
+    * `c`: clean and reset the given world
+    * `a`: apply force/torque on the selected link/joint using sliders (select the link/joint using the mouse)
+    * `h`: hide/show the complete GUI; enable/disable the rendering
+    * `m`: show/hide the workspace of the corresponding link (not implemented yet)
+    * `l`: change mode of locomotion (defined in the robot; select the controller/policy)
+    * `<number>`: select and perform corresponding action (10 actions possible, as <number> between 0 and 9)
+    * `top arrow`: move the selected robot forward
+    * `bottom arrow`: move the selected robot backward
+    * `left arrow`: turn the selected robot to the left
+    * `right arrow`: turn the selected robot to the right
+    * `space`: allow the robot to jump (if implemented)
+    """
+
+    def __init__(self, world, interface=None, priority=None, verbose=False):
+        """
+        Initialize the Bridge between a Mouse-Keyboard interface and the world.
+
+        Args:
+            world (World): world instance.
+            interface (None, MouseKeyboardInterface): mouse keyboard interface. If None, it will create one.
+            priority (int): priority of the bridge.
+            verbose (bool): If True, print information on the standard output.
+        """
+        # set world
+        self.world = world
+
+        # check interface
+        if not isinstance(interface, MouseKeyboardInterface):
+            interface = MouseKeyboardInterface(self.simulator)
+
+        # call superclass
+        super(BridgeMouseKeyboardWorld, self).__init__(interface, priority)
+
+        # camera
+        self.world_camera = WorldCamera(self.simulator)
+        self._camera = None
+        self.default_camera = None
+        self.plane, self.depth = None, None
+        self.bounding_box = False
+
+        self.robot, self.link_id = None, None
+        self.joint_sliders, self.task_sliders = False, {}
+        self.hiding_gui = False
+        self.pausing = False
+        self.debug = verbose
+
+        self.events_fn = {(Key.x,): self.change_camera_view_x,
+                          (Key.y,): self.change_camera_view_y,
+                          (Key.z,): self.change_camera_view_z,
+                          (Key.enter,): self.reset_camera_view,
+                          (Key.space,): self.pause,  # pause the simulator
+                          (Key.h,): self.gui,  # hide/enable (actually stop/play) GUI
+                          (Key.r,): self.reset_world,  # reset world
+                          (Key.j,): self.update_joint_sliders,  # add/remove joint (space) sliders
+                          (Key.t,): self.update_task_sliders,  # add/remove task (space) sliders
+                          (Key.p,): lambda: None,
+                          (Key.u,): self.unselect,  # unselect robot/link
+                          (Key.d,): lambda: None,  # display robot's camera
+                          (Key.m,): lambda: None,  # display link workspace
+                          (Key.b,): lambda: None,  # bounding box
+                          (Key.a,): lambda: None,  # apply force/torque on selected link/joint
+                          (Key.l,): lambda: None,
+                          (Key.n0,): lambda: None,
+                          (Key.n1,): lambda: None,
+                          (Key.n2,): lambda: None,
+                          (Key.n3,): lambda: None,
+                          (Key.n4,): lambda: None,
+                          (Key.n5,): lambda: None,
+                          (Key.n6,): lambda: None,
+                          (Key.n7,): lambda: None,
+                          (Key.n8,): lambda: None,
+                          (Key.n9,): lambda: None,
+                          (Key.top_arrow,): lambda: None,
+                          (Key.bottom_arrow,): lambda: None,
+                          (Key.left_arrow,): lambda: None,
+                          (Key.right_arrow,): lambda: None}
+
+        # self.vs = self.simulator.createVisualShape(self.simulator.GEOM_SPHERE, radius=0.02, rgbaColor=(0, 0, 1, 1))
+        # self.vs1 = self.simulator.createVisualShape(self.simulator.GEOM_SPHERE, radius=0.2, rgbaColor=(1, 0, 0, 1))
+        # self.vs2 = self.simulator.createVisualShape(self.simulator.GEOM_SPHERE, radius=0.2, rgbaColor=(0, 1, 0, 1))
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def world(self):
+        """Return the world instance."""
+        return self._world
+
+    @world.setter
+    def world(self, world):
+        """Set the world instance."""
+        if isinstance(world, Env):
+            world = world.world
+        if not isinstance(world, World):
+            raise TypeError("Expecting the world to be an instance of World or Env, instead got {}".format(type(world)))
+        self._world = world
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self.world.simulator
+
+    @property
+    def camera(self):
+        if self._camera is None:
+            self._camera = self.world_camera.getDebugVisualizerCamera(convert=False)
+            if self.default_camera is None:
+                self.default_camera = self._camera
+        return self._camera
+
+    ###########
+    # Methods #
+    ###########
+
+    def print_debug(self, str1, str2='', condition=True):
+        if self.debug:
+            if condition:
+                print("MouseKeyboardInterface: "+str1)
+            else:
+                print("MouseKeyboardInterface: "+str2)
+
+    def step(self, update_interface=False):
+        """Perform a step: map the mouse-keyboard interface to the world"""
+        # update interface
+        if update_interface:
+            self.interface()
+
+        # check keyboard events
+        self.check_key_events()
+            
+        # check mouse events and map to
+        self.check_mouse_events()
+
+        # update joint sliders if present (position control)
+        if self.joint_sliders:
+            self.robot.updateJointSlider()
+
+        # update task sliders if present (IK)
+        if self.task_sliders:
+            pass
+
+        # reset camera (the scrolling event is not detected)
+        self._camera = None
+
+        # step in the world
+        if not self.pausing:
+            self.world.step()
+
+    def change_camera_view_x(self):
+        """Change camera view X."""
+        self.print_debug('change camera view X')
+        dist, target = self.camera[-2:]
+        self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=90., cameraPitch=0.,
+                                                  cameraTargetPosition=target)
+
+    def change_camera_view_y(self):
+        """Change camera view Y."""
+        self.print_debug('change camera view Y')
+        dist, target = self.camera[-2:]
+        self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=180., cameraPitch=0.,
+                                                  cameraTargetPosition=target)
+
+    def change_camera_view_z(self):
+        """Change camera view Z."""
+        self.print_debug('change camera view Z')
+        dist, target = self.camera[-2:]
+        self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=-90., cameraPitch=-89.99,
+                                                  cameraTargetPosition=target)
+
+    def reset_camera_view(self):
+        """Reset camera view."""
+        self.print_debug('reset camera view')
+        if self.default_camera is not None:
+            yaw, pitch, dist, target = self.default_camera[-4:]
+            self.simulator.resetDebugVisualizerCamera(cameraDistance=dist, cameraYaw=yaw, cameraPitch=pitch,
+                                                      cameraTargetPosition=target)
+            self.default_camera = None
+
+    def pause(self):
+        """Pause the simulation."""
+        self.pausing = not self.pausing
+        self.print_debug('pause the simulator', 'unpause the simulator', self.pausing)
+
+    def gui(self):
+        """show/hide GUI."""
+        self.hiding_gui = not self.hiding_gui
+        self.simulator.configureDebugVisualizer(self.simulator.COV_ENABLE_GUI, self.hiding_gui)
+        self.print_debug('hide the GUI', 'enable the GUI', self.hiding_gui)
+
+    def reset_world(self):
+        """Reset the world."""
+        self.print_debug('reset the world')
+        self.world.resetRobots()
+        self.simulator.removeAllUserDebugItems()
+
+    def update_joint_sliders(self):
+        """Update joint sliders."""
+        if self.robot is not None:
+            if self.joint_sliders:  # remove joint sliders
+                self.robot.removeJointSlider()
+                self.print_debug('remove joint sliders')
+            else:  # add joint sliders
+                self.robot.addJointSlider()
+                self.print_debug('add joint sliders')
+            self.joint_sliders = not self.joint_sliders
+
+    def update_task_sliders(self):
+        """Update task sliders."""
+        if self.robot is not None and self.link_id is not None:
+            if self.link_id in self.task_sliders:  # remove task sliders
+                for idx in self.task_sliders[self.link_id]:
+                    self.simulator.removeUserDebugItem(self.task_sliders[self.link_id][idx])
+                self.task_sliders.pop(self.link_id)
+                self.print_debug('remove task sliders')
+            else:  # add task sliders
+                self.task_sliders[self.link_id] = {}
+                pos = self.robot.getLinkWorldPositions(self.link_id)
+                for i, name in zip(pos, ['x', 'y', 'z']):
+                    slider = self.simulator.addUserDebugParameter(name, i - 2., i + 2., i)
+                    self.task_sliders[self.link_id][name] = slider
+                self.print_debug('add task sliders')
+
+    def unselect(self):
+        """Unselect the robot and link id."""
+        self.print_debug('unselect robot/link')
+        self.robot, self.link_id = None, None
+
+    def add_world_text(self, string, position, color=(0.,0.,0.), size=1., lifetime=0.):
+        """Add world text."""
+        self.print_debug('add world text')
+        self.simulator.addUserDebugText(string, position, color, size, lifetime)
+
+    def add_screen_text(self, string, world_position, color=(0.,0.,0.), size=1., lifetime=0.):
+        """Add screen text."""
+        self.print_debug('add screen text')
+        V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.getMatrices(True)
+        position = self.world_camera.screenToWorld(world_position, Vp_inv, P_inv, V_inv)[:3]
+        self.simulator.addUserDebugText(string, position, color, size, lifetime)
+
+    def check_key_events(self):
+        # call function corresponding to key combination
+        if self.interface.key_pressed:
+            key = tuple(self.interface.key_pressed)
+            if key in self.events_fn:
+                self.events_fn[key]()
+
+    def check_mouse_events(self):
+        # check if the mouse has been released
+        if not self.interface.mouse_down:
+            self.plane, self.depth = None, None
+
+        # check what object we are trying to grab with the mouse by checking collision
+        if self.interface.mouse_pressed:
+            V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.getMatrices(True)
+            camera = self.world_camera.getDebugVisualizerCamera(convert=False)
+
+            # from the point (x,y) on the screen, get nearest and farthest point on the screen
+            x_screen_init = np.array([self.interface.mouse_x, self.interface.mouse_y, 1., 1.])
+            x_screen_final = np.array([self.interface.mouse_x, self.interface.mouse_y, 0., 1.])
+
+            # get the corresponding points in the world
+            x_world_init = self.world_camera.screenToWorld(x_screen_init, Vp_inv, P_inv, V_inv)
+            x_world_final = self.world_camera.screenToWorld(x_screen_final, Vp_inv, P_inv, V_inv)
+
+            # check if there is a collision
+            # print(x_world_init[:3], x_world_final[:3])
+            collision = self.simulator.rayTest(list(x_world_init[:3]), list(x_world_final[:3]))
+
+            # if collision, proceed the inverse operation to get the depth on the screen
+            if len(collision) > 0:
+                object_id, link_id, hit_frac, hit_pos, hit_normal = collision[0]
+                # self.simulator.addUserDebugLine(list(x_world_init[:3]), list(x_world_final[:3]), (0, 0, 1))
+                # bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs1,
+                #                                   basePosition=list(x_world_init[:3]))
+                # bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs2,
+                #                                   basePosition=list(x_world_final[:3]))
+                if object_id != -1 and self.world.isRobotId(object_id):  # valid object
+
+                    # Set robot and link_id
+                    self.robot, self.link_id = self.world.getRobot(object_id), link_id
+
+                    width, height = camera[:2]
+                    x_screen = np.array([width/2, height/10, 0.95, 1])
+                    pos = self.world_camera.screenToWorld(x_screen, Vp_inv, P_inv, V_inv)[:3]
+                    # self.simulator.addUserDebugText(str(self.robot) + ": " + self.robot.getLinkNames(self.link_id),
+                    #                                 pos, RGBColor.black, textSize=1)
+
+                    # calculate plane
+                    # 1. compute the initial point on the plane (collision point)
+                    if link_id == -1:  # no link
+                        x0 = np.array(self.simulator.getBasePositionAndOrientation(object_id)[0])
+                    else:  # link
+                        x0 = np.array(self.simulator.getLinkState(object_id, link_id)[0])
+
+                    # 2. calculate normal (=targetPosition - eyePosition) to the plane
+                    # normal = x_world_final[:3] - x_world_init[:3]
+                    yaw, pitch, dist, target = camera[-4:]
+                    yaw, pitch = np.deg2rad(yaw), np.deg2rad(pitch)
+                    normal = dist * np.array([np.cos(pitch) * np.sin(yaw),
+                                              -np.cos(pitch) * np.cos(yaw),
+                                              -np.sin(pitch)])
+
+                    # 3. create plane
+                    self.plane = Plane(x0, normal)
+
+                    # calculate associate depth on the screen (because perspective projection)
+                    hit_pos = np.array(list(hit_pos) + [1.])
+                    self.depth = self.world_camera.worldToScreen(hit_pos, V, P, Vp)[2]
+
+        elif self.interface.mouse_down and self.interface.mouse_moving and self.robot is not None:
+            V, P, Vp, V_inv, P_inv, Vp_inv = self.world_camera.getMatrices(True)
+            camera = self.world_camera.getDebugVisualizerCamera(convert=False)
+
+            if self.plane is not None:
+                # project the point on the screen to the world, and check where the line that starts from this point
+                # and is perpendicular to the plane (i.e. parallel to the normal) intersects with the aforementioned
+                # plane
+                x_screen = np.array([self.interface.mouse_x, self.interface.mouse_y, self.depth, 1])
+                x_world = self.world_camera.screenToWorld(x_screen, Vp_inv, P_inv, V_inv)[:3]
+                point = self.plane.getIntersectionPoint(x_world)
+
+            # # draw some spheres on the plane
+            # if self.display_trajectories:
+            #    bodyId = self.simulator.createMultiBody(baseMass=0, baseVisualShapeIndex=self.vs,
+            #                                      basePosition=point)
+            #    self.visual_points[tuple(point)] = bodyId
+            # else:
+            #    self.visual_points[tuple(point)] = None
+
+            # # draw trajectories
+            # if self.display_trajectories:
+            #     self.visual_points.append(point)
+            #     if len(self.visual_points) > 1:
+            #         self.simulator.addUserDebugLine(self.visual_points[-2], self.visual_points[-1],
+            #                                   RGBColor.red, 1., 2.)
+
+            # # perform inverse kinematics
+            # q = self.robot.calculateInverseKinematics(self.link, point)
+            # for i in range(self.robot.getNumberOfJoints()):
+            #     self.robot.setJointPositions(i, q[i])
+
+
+# Tests
+if __name__ == '__main__':
+    from pyrobolearn.simulators import BulletSim
+    from pyrobolearn.worlds import BasicWorld
+    import time
+    from itertools import count
+
+    # create simulator
+    sim = BulletSim()
+
+    # sim.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 1)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP, 0)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_RGB_BUFFER_PREVIEW, 0)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0)
+    # sim.configureDebugVisualizer(p.COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0)
+
+    # create World
+    world = BasicWorld(sim)
+
+    # load robot
+    robot = world.loadRobot('baxter', useFixedBase=True)
+
+    # create bridge/interface
+    bridge = BridgeMouseKeyboardWorld(world, verbose=True)
+
+    for _ in count():
+        bridge.step(update_interface=True)
+        # world.step()
+        time.sleep(1. / 100)

pyrobolearn/tools/bridges/vr/__init__.py

@@ -0,0 +1,8 @@
+
+## Bridge for VR interfaces ##
+
+# Bridge between VR and robot
+from robots import *
+
+# Bridge between VR and world
+from world import *

pyrobolearn/tools/interfaces/__init__.py

@@ -0,0 +1,30 @@
+
+# import the interfaces
+from interface import *
+
+# mouse-keyboard interface
+from mouse_keyboard import *
+
+# audio
+# from audio import *
+
+# camera
+# from camera import *
+
+# controllers
+# from controllers import *
+
+# bci
+# from bci import *
+
+# sensor suits
+# from suits import *
+
+# sensors (in general, EMG, etc)
+# from sensors import *
+
+# VR interfaces
+# from vr import *
+
+# robot interfaces
+# from robots import *

pyrobolearn/tools/interfaces/audio/__init__.py

@@ -0,0 +1,3 @@
+
+# import audio interfaces
+from audio import *

pyrobolearn/tools/interfaces/audio/audio.py

@@ -0,0 +1,340 @@
+import os
+
+from pyrobolearn.tools.interfaces.interface import Interface, InputInterface, OutputInterface, InputOutputInterface
+
+# To use microphone (using PyAudio). This also needed for the 'speech_recognition' module.
+try:
+    import pyaudio
+except ImportError as e:
+    # `pip install --allow-external pyaudio --allow-unverified pyaudio pyaudio` ??
+    string = "\nHint: try to install pyaudio by typing the following lines in the terminal: \n" \
+             "sudo apt-get install libasound-dev portaudio19-dev libportaudio2 libportaudiocpp0\n" \
+             "sudo apt-get install ffmpeg libav-tools\n" \
+             "pip install pyaudio\n"
+    raise ImportError(e.__str__() + string)
+
+# Speech recognition
+# Good tutorial: https://realpython.com/python-speech-recognition/#working-with-microphones
+try:
+    import speech_recognition as sr
+except ImportError as e:
+    string = "\nHint: try to install speech_recognition by typing the following lines in the terminal: \n" \
+             "sudo apt-get install libpulse-dev" \
+             "pip install pocketsphinx\n" \
+             "pip install google-cloud-speech" \
+             "pip install SpeechRecognition"
+    raise ImportError(e.__str__() + string)
+
+# Speech synthesis
+# Good tutorial: https://pythonprogramminglanguage.com/text-to-speech/
+# If Python3.3 or higher: https://pypi.org/project/google_speech/
+try:
+    from gtts import gTTS
+except ImportError as e:
+    string = "\nHint: try to install gTTS by typing: pip install gTTS"
+    raise ImportError(e.__str__() + string)
+
+# # Another one is `pyttsx3`, which is the best offline module (the problem is that it only supports english)
+# # Documentation (with examples): pyttsx3.readthedocs.io/en/latest/
+# try:
+#     import pyttsx3
+# except ImportError as e:
+#     string = "\nHint: try to install pyttsx3 by typing: pip install pyttsx3"
+#     raise ImportError(e.__str__() + string)
+
+# what I also checked: `pyttsx` and `pyvona`
+# import pyttsx
+# import pyvona
+
+
+# Translation
+# Github repo: https://github.com/ssut/py-googletrans
+# Tutorial: https://www.codeproject.com/Tips/1236705/How-to-Use-Google-Translator-in-Python
+try:
+    from googletrans import Translator
+except ImportError as e:
+    string = "\nHint: try to install googletrans by typing: pip install googletrans"
+    raise ImportError(e.__str__() + string)
+
+
+# ChatterBot
+# Github repo: https://github.com/gunthercox/ChatterBot
+# Documentation: https://chatterbot.readthedocs.io/en/stable
+# try:
+#     from chatterbot import ChatBot
+# except ImportError as e:
+#     string = "\nHint: try to install chatterbot by typing: pip install chatterbot"
+#     raise ImportError(e.__str__() + string)
+
+
+class AudioInterface(Interface):
+    r"""Audio Interface
+
+    References:
+        [1] https://gist.github.com/mabdrabo/8678538
+        [2] https://raspberrypi.stackexchange.com/questions/59852/pyaudio-does-not-detect-my-microphone-connected-via-usb-audio-adapter
+        [3] https://www.swharden.com/wp/2016-07-19-realtime-audio-visualization-in-python/
+        [4] https://www.programcreek.com/python/example/52624/pyaudio.PyAudio
+        [5] https://stackoverflow.com/questions/35970282/what-are-chunks-samples-and-frames-when-using-pyaudio
+
+        [6] https://realpython.com/python-speech-recognition/#working-with-microphones
+    """
+
+    def __init__(self):
+        super(AudioInterface, self).__init__()
+        self.port = pyaudio.PyAudio()
+        self.stream = self.port.open(format=pyaudio.paInt16, channels=2, rate=44100, input=True,
+                                     frames_per_buffer=1024) #input_device_index=)
+
+    def printInfo(self):
+        for i in range(self.port.get_device_count()):
+            info = self.port.get_device_info_by_index(i)
+            print("###############################################################")
+            print("Index: {} - name: {} - rate: {} ".format(i, info['name'], info['defaultSampleRate']))
+            print("Max input/output channels: {}, {}".format(info['maxInputChannels'], info['maxOutputChannels']))
+            print("Input latency (low, high): {}, {}".format(info['defaultLowInputLatency'],
+                                                             info['defaultHighInputLatency']))
+            print("Output latency (low, high): {}, {}".format(info['defaultLowOutputLatency'],
+                                                              info['defaultHighOutputLatency']))
+            print("Is an input device? {}".format(self.isInputDevice(info)))
+            print("Is an output device? {}".format(self.isOutputDevice(info)))
+
+    def isInputDevice(self, info):
+        return (info['maxInputChannels'] != 0)
+
+    def isOutputDevice(self, info):
+        return (info['maxOutputChannels'] != 0)
+
+    def step(self):
+        data = self.stream.read()
+
+    def __del__(self):
+        self.stream.stop_stream()
+        self.stream.close()
+
+
+class InputAudioInterface(InputInterface):
+    r"""Input Audio Interface.
+
+    See `pyAudio`: https://people.csail.mit.edu/hubert/pyaudio/
+
+    In pyAudio:
+    * 'Rate': sampling rate, i.e. the number of frames per second
+    * 'chunk': arbitrary chosen number of frames the signals are split into
+    """
+    pass
+
+
+class OutputAudioInterface(OutputInterface):
+    r"""Output Audio Interface
+
+    See `pyAudio`: https://people.csail.mit.edu/hubert/pyaudio/
+    """
+    pass
+
+
+class InputOutputAudioInterface(InputOutputInterface):
+    r"""Input and Output Audio Interface.
+
+    See `pyAudio`: https://people.csail.mit.edu/hubert/pyaudio/
+    """
+    pass
+
+
+class SpeechRecognizerInterface(InputInterface):
+    r"""Speech Recognizer Interface
+
+    References:
+        [1] Tutorial: https://realpython.com/python-speech-recognition/#working-with-microphones
+    """
+
+    def __init__(self, use_thread=False, lang='english', verbose=False):
+        self.recognizer = sr.Recognizer()
+        self.microphone = sr.Microphone()  # device_index=-1
+
+        languages = {'french': 'fr', 'english': 'en', 'american english': 'en-US', 'british english': 'en-GB',
+                     'indian english': 'en-IN', 'italian': 'it', 'japanese': 'ja', 'korean': 'ko', 'german': 'de',
+                     'dutch': 'nl', 'spanish': 'es', 'spanish (peru)': 'es-PE', 'chinese': 'zh-CN',
+                     'mandarin': 'zh-CN', 'polish': 'pl', 'portuguese': 'pt', 'russian': 'ru', 'greek': 'el-GR'}
+        # Check < https://gist.github.com/traysr/2001377 > for more
+        self.lang = languages[lang]
+
+        # string that is being said
+        self.data = ''
+
+        super(SpeechRecognizerInterface, self).__init__(use_thread=use_thread, verbose=verbose)
+
+    def run(self):
+        # listen to speech through the microphone
+        with self.microphone as source:
+            self.recognizer.adjust_for_ambient_noise(source)
+            print("I am listening...")
+            audio = self.recognizer.listen(source)  # listen to what is being said
+
+        # recognize speech (get the string from audio)
+        try:
+            print('Trying to understand what you just said...')
+            self.data = self.recognizer.recognize_google(audio, language=self.lang)
+        except sr.UnknownValueError:
+            print("Unable to recognize speech")
+        except sr.RequestError as e:
+            print("API unavailable".format(e))
+
+        if self.verbose:
+            print("You said: {}".format(self.data))
+
+
+class SpeechSynthesizerInterface(OutputInterface):
+    r"""Speech Synthesizer Interface
+
+    References:
+        [1] tutorial: https://pythonprogramminglanguage.com/text-to-speech/
+        [2] If Python3.3 or higher: https://pypi.org/project/google_speech/
+    """
+
+    def __init__(self, use_thread=False, lang='english', verbose=False):
+
+        languages = {'french': 'fr', 'english': 'en', 'american english': 'en-US', 'british english': 'en-GB',
+                     'indian english': 'en-IN', 'italian': 'it', 'japanese': 'ja', 'korean': 'ko', 'german': 'de',
+                     'dutch': 'nl', 'spanish': 'es', 'spanish (peru)': 'es-PE', 'chinese': 'zh-CN',
+                     'mandarin': 'zh-CN', 'polish': 'pl', 'portuguese': 'pt', 'russian': 'ru', 'greek': 'el-GR'}
+        # Check < https://gist.github.com/traysr/2001377 > for more
+        self.lang = languages[lang]
+
+        self.updated = False
+        self.data = ''
+
+        super(SpeechSynthesizerInterface, self).__init__(use_thread=use_thread, verbose=verbose)
+
+    def run(self):
+        if self.updated:
+            # tts = text-to-speech
+            tts = gTTS(text=self.data, lang=self.lang)
+            tts.save('tmp.mp3')
+            os.system('mpg321 tmp.mp3')
+            os.system('rm tmp.mp3')
+            self.updated = False
+
+    def update(self, data):
+        self.data = data
+        self.updated = True
+
+
+class SpeechTranslatorInterface(InputOutputInterface):
+    r"""Speech Translator Interface
+    """
+
+    def __init__(self, use_thread=False, target_lang='english', from_lang='auto'):
+        self.translator = Translator()
+
+        languages = {'french': 'fr', 'english': 'en', 'american english': 'en-US', 'british english': 'en-GB',
+                     'indian english': 'en-IN', 'italian': 'it', 'japanese': 'ja', 'korean': 'ko', 'german': 'de',
+                     'dutch': 'nl', 'spanish': 'es', 'spanish (peru)': 'es-PE', 'chinese': 'zh-CN',
+                     'mandarin': 'zh-CN', 'polish': 'pl', 'portuguese': 'pt', 'russian': 'ru', 'greek': 'el-GR',
+                     'auto': 'auto'}
+        # Check < https://gist.github.com/traysr/2001377 > for more
+        self.target_lang = languages[target_lang]
+        self.from_lang = languages[from_lang]
+
+        self.updated = False
+        self.input_data = ''
+        self.data = ''
+
+        super(SpeechTranslatorInterface, self).__init__(use_thread)
+
+    def run(self):
+        # translate
+        if self.updated and self.target_lang != self.from_lang:
+            translated = self.translator.translate(self.input_data, dest=self.target_lang, src=self.from_lang)
+            self.data = translated.text
+            self.updated = False
+
+    def update(self, data):
+        self.input_data = data
+        self.updated = True
+
+
+class SpeechInterface(InputOutputInterface):
+    r"""Speech Interface
+
+    This class performs speech recognition, translation, and synthesization.
+    """
+
+    def __init__(self, use_thread=False, target_lang='english', from_lang='english'):
+        self.recognizer = SpeechRecognizerInterface(use_thread=False, lang=from_lang)
+        self.translator = None
+        if target_lang != from_lang:
+            self.translator = SpeechTranslatorInterface(use_thread=False, target_lang=target_lang,
+                                                        from_lang=from_lang)
+        self.synthesizer = SpeechSynthesizerInterface(use_thread=False, lang=target_lang)
+
+        self.updated = False
+        self.input_data = ''
+        self.output_data = ''
+
+        super(SpeechInterface, self).__init__(use_thread)
+
+    def run(self):
+        pass
+
+    def update(self, data):
+        pass
+
+
+# Tests
+if __name__ == '__main__':
+    interface = AudioInterface()
+    interface.printInfo()
+
+    # recognize, translate and synthesize speech
+    english = set(['en', 'en-US', 'en-GB'])
+    languages = {'french': 'fr', 'english': 'en', 'american english': 'en-US', 'british english': 'en-GB',
+                 'indian english': 'en-IN', 'italian': 'it', 'japanese': 'ja', 'korean': 'ko', 'german': 'de',
+                 'dutch': 'nl', 'spanish': 'es', 'spanish (peru)': 'es-PE', 'chinese': 'zh-CN',
+                 'mandarin': 'zh-CN', 'polish': 'pl', 'portuguese': 'pt', 'russian': 'ru', 'greek': 'el-GR'}
+    # Check < https://gist.github.com/traysr/2001377 > for more
+    lang = languages['english']
+
+    recognizer = sr.Recognizer()
+    microphone = sr.Microphone()  # device_index=-1
+
+    # print(microphone.list_microphone_names())
+
+    # listen to speech through the microphone
+    with microphone as source:
+        recognizer.adjust_for_ambient_noise(source)
+        print("Say something!")
+        audio = recognizer.listen(source)
+
+    # recognize speech
+    print('processing...')
+    string = ''
+    try:
+        string = recognizer.recognize_google(audio, language=lang)
+    except sr.UnknownValueError:
+        print("Unable to recognize speech")
+    except sr.RequestError as e:
+        print("API unavailable".format(e))
+
+    print("You said: " + string)
+
+    # translate it if other language than english
+    if lang not in english:
+        translator = Translator()
+        translated = translator.translate(string)  # dest='en', src='auto')
+        print("which translates to: " + translated.text)
+
+    # produce speech
+    print('Let me try to repeat what you just said:')
+    tts = gTTS(text=string, lang=lang)
+    tts.save('tmp.mp3')
+    os.system('mpg321 tmp.mp3')
+    os.system('rm tmp.mp3')
+
+    # recognize speech using Sphinx
+    # try:
+    #     print("Sphinx thinks you said '" + recognizer.recognize_sphinx(audio) + "'")
+    # except sr.UnknownValueError:
+    #     print("Sphinx could not understand audio")
+    # except sr.RequestError as e:
+    #     print("Sphinx error; {0}".format(e))

pyrobolearn/tools/interfaces/audio/microphone.py

@@ -0,0 +1,22 @@
+
+from audio import InputAudioInterface
+
+
+class MicrophoneInterface(InputAudioInterface):
+    r"""Microphone Interface.
+
+    References:
+        [1] https://github.com/castorini/honk
+        [2] https://github.com/llSourcell/tensorflow_speech_recognition_demo/blob/master/speech_data.py
+        [3] https://github.com/SeanNaren/deepspeech.pytorch
+        [4] https://github.com/awni/speech
+        [5] https://github.com/tugstugi/pytorch-speech-commands
+        [6] https://cmusphinx.github.io/
+        [7] https://realpython.com/python-speech-recognition/
+    """
+    def __init__(self):
+        super(MicrophoneInterface, self).__init__()
+
+
+if __name__ == '__main__':
+    pass

pyrobolearn/tools/interfaces/audio/speaker.py

@@ -0,0 +1,8 @@
+
+from audio import OutputAudioInterface
+
+
+class SpeakerInterface(OutputAudioInterface):
+    r"""Speaker Interface
+    """
+    pass

pyrobolearn/tools/interfaces/bci/bci.py

@@ -0,0 +1,19 @@
+
+from pyrobolearn.tools.interfaces.interface import InputInterface
+
+
+class BCIInterface(InputInterface):
+    r"""Brain-Computer Interface
+
+    This class defines the Brain-Computer Interface which allows to read from a EEG headset.
+
+    Check the following references:
+    - OpenBCI:
+        - http://docs.openbci.com/OpenBCI%20Software/05-OpenBCI_Python
+        - https://github.com/OpenBCI/OpenBCI_Python
+    - OpenViBE:
+        - http://openvibe.inria.fr/
+        - http://openvibe.inria.fr/tutorial-using-python-with-openvibe/#The+Python+Scripting+box
+        - https://github.com/dojeda/openvibe-python-tutorial
+    """
+    pass

pyrobolearn/tools/interfaces/camera/__init__.py

@@ -0,0 +1,18 @@
+
+# General import
+from camera import CameraInterface
+
+# Webcam
+from webcam import WebcamInterface
+
+# Asus Xtion
+from asus_xtion import AsusXtionInterface
+
+# Kinect
+from kinect import *
+
+# FER
+from fer import FERInterface
+
+# OpenPose
+from openpose import OpenPoseInterface

pyrobolearn/tools/interfaces/camera/asus_xtion.py

@@ -0,0 +1,220 @@
+
+import numpy as np
+
+try:
+    # References:
+    # - Installation: https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-openni-nite.md
+    # - OpenNI: https://structure.io/openni
+    # - Github repo: https://github.com/occipital/openni2
+    # - Python wrappers: https://github.com/severin-lemaignan/openni-python
+    # - OpenNI2-FreenectDriver: https://github.com/OpenKinect/libfreenect/tree/master/OpenNI2-FreenectDriver
+
+    # from primesense import openni2, nite2
+    from openni import openni2, nite2
+    from openni.utils import InitializationError
+
+    try:
+        openni2.initialize()
+        openni2.unload()
+    except InitializationError as e:
+        raise InitializationError(repr(e) + '\nYou have to export the path to the folder which contains '
+                                            'the library libOpenNI2.so. Depending on your architecture, try '
+                                            'to type one of the following command in the terminal: '
+                                            '\nexport OPENNI2_REDIST=<path/to/libOpenNI2.so/folder>'
+                                            '\nexport OPENNI2_REDIST64=<path/to/libOpenNI2.so/folder>')
+
+except ImportError as e:
+    raise ImportError(repr(e) + '\nTry to install `openni`: pip install openni'
+                                '\nTo install the OpenNI2-Freenect driver, install libfreenect manually, and '
+                                'check the README in the `libfreenect/OpenNI2-FreenectDriver` folder')
+
+
+from camera import CameraInterface
+
+# check https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-openni-nite.md
+
+
+class AsusXtionInterface(CameraInterface):
+    r"""Asus Xtion Interface
+
+    Check OpenNI. Check also ROS.
+
+    If using `openni`, connect the Asus Xtion to your computer, and type the following in the terminal:
+    $ NiViewer
+    to check if it is correctly detected and working properly.
+
+    References:
+        [1] https://github.com/danielelic/PyOpenNI2-Utility
+        [2] https://github.com/kanishkaganguly/OpenNIMultiSensorCapture
+        [3] https://docs.opencv.org/3.1.0/d7/d6f/tutorial_kinect_openni.html
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0., verbose=False, use_rgb=True, use_depth=True, use_ir=False):
+
+        # quick check
+        if use_rgb and use_ir:
+            raise ValueError("It is not possible to stream RGB images at the same as IR images, set one to False")
+
+        # set variables
+        self.use_rgb = use_rgb
+        self.use_depth = use_depth
+        self.use_ir = use_ir
+
+        # initialize openni2; you can give the path to the library as an argument. Otherwise, it will look for
+        # OPENNI2_REDIST and OPENNI2_REDIST64 environment variables.
+        openni2.initialize()
+
+        # open all the devices
+        devices = openni2.Device.open_all()
+
+        # get the correct device (PrimeSense)
+        self.device = None
+        for device in devices:
+            info = device.get_device_info()
+            if info.vendor == 'PrimeSense': # Asus Xtion Interface
+                self.device = device
+                break
+
+        # If didn't find it, return an error
+        if self.device is None:
+            devices = [device.get_device_info() for device in devices]
+            raise ValueError("No Asus devices were detected; we found these devices instead: {}".format(devices))
+
+        if verbose:
+            print(self.device.get_device_info())
+
+        # create RGB, IR, and depth streams
+        self.streams = []
+        if self.use_rgb:
+            self.rgb_stream = self.device.create_color_stream()
+            self.streams.append(self.rgb_stream)
+
+        if self.use_ir:
+            self.ir_stream = self.device.create_ir_stream()
+            self.streams.append(self.ir_stream)
+
+        if self.use_depth:
+            self.depth_stream = self.device.create_depth_stream()
+            self.streams.append(self.depth_stream)
+
+        # start each stream
+        for stream in self.streams:
+            stream.start()
+
+        # data
+        self.rgb = None
+        self.ir = None
+        self.depth = None
+
+        super(AsusXtionInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+    def run(self):
+        data = []
+        if self.use_rgb:
+            # read frame
+            frame = self.rgb_stream.read_frame()
+            buffer = frame.get_buffer_as_uint8()
+            img = np.frombuffer(buffer, dtype=np.uint8)
+            img = img.reshape(frame.height, frame.width, 3)
+            self.rgb = img
+            data.append(self.rgb)
+
+        if self.use_ir:
+            # read frame
+            frame = self.ir_stream.read_frame()
+            buffer = frame.get_buffer_as_uint16()
+            img = np.frombuffer(buffer, dtype=np.uint16)
+            img = img.reshape(frame.height, frame.width)
+            self.ir = img
+            data.append(self.ir)
+
+        if self.use_depth:
+            frame = self.depth_stream.read_frame()
+            buffer = frame.get_buffer_as_uint16()
+            img = np.frombuffer(buffer, dtype=np.uint16)
+            img = img.reshape(frame.height, frame.width)
+            self.depth = img
+            data.append(self.depth)
+
+        return data
+
+    def __del__(self):
+        # close all the streams
+        for stream in self.streams:
+            stream.close()
+
+        # unload openni2
+        openni2.unload()
+
+
+if __name__ == '__main__':
+    # https://structure.io/openni
+    # https://github.com/occipital/OpenNI2
+    # https://www.reddit.com/r/ROS/comments/6qejy0/openni_kinect_installation_on_kinetic_indigo/
+    # https://github.com/cjcase/openGeppetto/wiki/Installing-OpenNI-on-Ubuntu
+    # https://github.com/jmendeth/PyOpenNI/blob/c7fa4fa01de3bb717ece5d036daaa343fe1c2ca9/examples/record.py
+    # https://pypi.org/project/openni/
+    # https://roboram.wordpress.com/asus-xtion-pro-live-ubuntu-14-04-installation/
+    # https://docs.opencv.org/3.1.0/d7/d6f/tutorial_kinect_openni.html
+    # http://euanfreeman.co.uk/pyopenni-and-opencv/
+    # WARNINGS: THERE ARE 2 OPENNI: One is pyopenni and the other one is openni
+    # Use opencv with openni: https://gist.github.com/joinAero/1f76844278f141cea8338d1118423648
+    # `sudo apt-get install libopenni-dev libopenni2-dev libopenni-sensor-primesense-dev`
+    # https://github.com/jmendeth/PyOpenNI/wiki/Building-on-Linux
+
+    # capture = cv2.VideoCapture(cv2.CAP_OPENNI)
+    # print(capture.get(cv2.CAP_PROP_OPENNI_GENERATOR_PRESENT))
+    # capture.set(cv2.CAP_OPENNI_IMAGE_GENERATOR_OUTPUT_MODE, cv2.CAP_OPENNI_VGA_30HZ)
+    # print(type(capture))
+    # time.sleep(1.)
+    # ret = capture.grab()
+    # print(ret)
+    # ret, frame = capture.read()
+    # print(ret)
+    # ret, depth = capture.retrieve(cv2.CAP_OPENNI_DEPTH_MAP)
+    #
+    # print(depth.shape)
+    # plt.imshow(depth)
+    # plt.show()
+
+    from itertools import count
+    import matplotlib.pyplot as plt
+
+    # set what we want to use (note you can not get RGB and IR images at the same time)
+    use_rgb, use_depth, use_ir = False, True, True
+
+    # create interface
+    interface = AsusXtionInterface(use_thread=False, sleep_dt=1. / 10, verbose=True, use_rgb=use_rgb,
+                                   use_depth=use_depth, use_ir=use_ir)
+
+    # plotting using matplotlib in interactive mode
+    fig, axes = plt.subplots(1,2)
+    plots  = [None]*2
+    titles = []
+    if use_rgb: titles.append('RGB')
+    if use_ir: titles.append('IR')
+    if use_depth: titles.append('Depth')
+    plt.ion()  # interactive mode on
+
+    for _ in count():
+        # if don't use thread call `step` or `run`
+        data = interface.run()
+
+        # get the frame and plot it with matplotlib
+        if plots[0] is None:
+            for i in range(len(plots)):
+                plots[i] = axes[i].imshow(data[i])
+                axes[i].set_title(titles[i])
+        else:
+            for plot, img in zip(plots, data):
+                plot.set_data(img)
+
+            # pause a bit
+            plt.pause(0.01)
+
+        # check if the figure is closed, and if so, get out of the loop
+        if not plt.fignum_exists(fig.number):
+            break
+
+    plt.ioff()  # interactive mode off
+    plt.show()

pyrobolearn/tools/interfaces/camera/camera.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Define the main basic Camera interface
+
+This defines the main basic camera interface from which all other interfaces which uses a camera inherit from.
+"""
+
+from pyrobolearn.tools.interfaces.interface import InputInterface
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class CameraInterface(InputInterface):
+    r"""Camera Interface.
+
+    This is the abstract class Camera Interface which is inherited from all the interfaces that use cameras
+    such as webcams, kinects, asus xtion, etc.
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        super(CameraInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)

pyrobolearn/tools/interfaces/camera/description.md

@@ -0,0 +1,2 @@
+In this folder, we use the Python Wrapper for OpenPose, and apply it on pictures
+streamed from a webcam or kinect.

pyrobolearn/tools/interfaces/camera/fer.py

@@ -0,0 +1,16 @@
+
+from camera import CameraInterface
+
+
+class FERInterface(CameraInterface):
+    r"""Facial Expression Recognition (FER) Interface
+
+    References:
+        [1] EmoPy - A deep neural net toolkit for emotion analysis via Facial Expression Recognition:
+            https://github.com/thoughtworksarts/EmoPy
+        [2] http://sefiks.com/2018/01/01/facial-expression-recognition-with-keras/
+        [3] https://github.com/a514514772/Real-Time-Facial-Expression-Recognition-with-DeepLearning
+    """
+
+    def __init__(self):
+        super(FERInterface, self).__init__()

pyrobolearn/tools/interfaces/camera/kinect.py

@@ -0,0 +1,357 @@
+
+# import kinect library
+
+# select which library we want from {freenect, openni, pykinect}
+# - freenect is a lower-level library which, in addition to access depth and color images, it also allows you to
+#   access to various the Kinect hardware (such as the sensors (e.g. accelerometer) and actuators (e.g. LED control)
+#   on the Kinect).
+# - openni is a higher-level library which, in addition to depth and color images, it allows you to perform high-level
+#   tasks such as skeleton tracking, segmentation, gesture recognition, and others. It also allows you to use other
+#   devices such as the asus xtion as well.
+#   --> Note that there is an `OpenNI2-FreenectDriver` in the libfreenect repo, which is a bridge to libfreenect
+#       implemented as an OpenNI2 driver. It allows OpenNI2 to use Kinect hardware on Linux and OSX.
+# - pykinect is a Windows library that allows the user to access the kinect (but cannot be used on Unix systems)
+#
+# For more info, check the following links:
+# - https://robotics.stackexchange.com/questions/565/kinect-libfreenect-vs-opennisensorkinect
+# - https://stackoverflow.com/questions/19181332/libfreenect-vs-openni
+
+import numpy as np
+
+# by default, use `openni` (optionally with the freenect driver) as it seems to be the most complete library
+KINECT_LIBRARY = 'freenect'
+
+if KINECT_LIBRARY[-8:] == 'freenect': # 'libfreenect' or 'freenect'
+    # References:
+    # - OpenKinect: https://openkinect.org/wiki/Getting_Started
+    # - tuto: naman5.wordpress.com/2014/06/24/experimenting-with-kinect-using-opencv-python-and-open-kinect-libfreenect/
+
+    try:
+        import freenect
+    except ImportError as e:
+        raise ImportError(repr(e) + '\nTry to install `libfreenect` (manually in order to have the python wrappers):'
+                                    '\n# install dependencies'
+                                    '\nsudo apt-get install git-core cmake libglut3-dev pkg-config build-essential '
+                                    'libxmu-dev libxi-dev libusb-1.0-0-dev'
+                                    '\n# clone the repo'
+                                    '\ngit clone git://github.com/OpenKinect/libfreenect.git'
+                                    '\nsudo python setup.py install'
+                                    '\n# build the repo and install it'
+                                    '\ncd libfreenect; mkdir build; cd build;'
+                                    '\ncmake ..; make'
+                                    '\nsudo make install'
+                                    '\n\n# Install Python wrappers'
+                                    '\ncd ../wrappers/python'
+                                    '\nsudo python setup.py install')
+
+    # If using `freenect`, connect the kinect to your computer, and type the following in the terminal:
+    # $ freenect-glview
+    # to check if it is correctly detected and working properly.
+
+elif KINECT_LIBRARY == 'openni':
+    # References:
+    # - Installation: https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-openni-nite.md
+    # - OpenNI: https://structure.io/openni
+    # - Github repo: https://github.com/occipital/openni2
+    # - Python wrappers: https://github.com/severin-lemaignan/openni-python
+    # - OpenNI2-FreenectDriver: https://github.com/OpenKinect/libfreenect/tree/master/OpenNI2-FreenectDriver
+    if __name__ == '__main__':
+        try:
+            from openni import openni2, nite2
+            from openni.utils import InitializationError
+            try:
+                openni2.initialize()
+                openni2.unload()
+            except InitializationError as e:
+                raise InitializationError(repr(e) + '\nYou have to export the path to the folder which contains '
+                                                    'the library libOpenNI2.so. Depending on your architecture, try '
+                                                    'to type one of the following command in the terminal: '
+                                                    '\nexport OPENNI2_REDIST=<path/to/libOpenNI2.so/folder>'
+                                                    '\nexport OPENNI2_REDIST64=<path/to/libOpenNI2.so/folder>')
+        except ImportError as e:
+            raise ImportError(repr(e) + '\nTry to install `openni`: pip install openni'
+                                        '\nTo install the OpenNI2-Freenect driver, install libfreenect manually, and '
+                                        'check the README in the `libfreenect/OpenNI2-FreenectDriver` folder')
+
+    # Checks:
+    # - To check if `OpenNI` is correctly installed and work, check `NiViewer` binary application in the `OpenNI`
+    #   package
+    # - To check if `Nite` is correctly installed and work, check `UserViewer` and `HandViewer` binaries in
+    #   the `Nite2` package
+
+    # Troubleshootings:
+    # - sudo ln -s /lib/x86_64 ..
+    # - Not detected
+
+
+elif KINECT_LIBRARY == 'pykinect':  # WARNING: ONLY WORKS ON WINDOWS
+    # Reference:
+    # - https://github.com/Microsoft/PTVS/wiki/PyKinect
+    # - https://possiblywrong.wordpress.com/2012/11/04/kinect-skeleton-tracking-with-visual-python/
+    try:
+        import pykinect
+    except ImportError as e:
+        raise ImportError(repr(e) + '\nTry to install `pykinect`: pip install pykinect')
+
+else:
+    raise ValueError("The given KINECT_LIBRARY variable is not known, please select between {freenect, openni, "
+                     "pykinect}")
+
+
+# import interface
+from camera import CameraInterface
+
+
+class KinectInterface(CameraInterface):
+    r"""Kinect Interface
+
+    This defines the kinect interface class.
+
+    There are 3 different libraries `freenect`, `openni`, and `pykinect` that can be used.
+    - `freenect`: it is a lower-level library which, in addition to access depth and color images, it also allows you
+    to access to various the Kinect hardware (such as the sensors (e.g. accelerometer) and actuators (e.g. LED control)
+    on the Kinect).
+    - `openni`: it is a higher-level library which, in addition to depth and color images, it allows you to perform
+    high-level tasks such as skeleton tracking, segmentation, gesture recognition, and others. Note that there is an
+    `OpenNI2-FreenectDriver` in the libfreenect repo, which is a bridge to libfreenect implemented as an OpenNI2
+    driver. It allows OpenNI2 to use Kinect hardware on Linux and OSX.
+    - `pykinect`: it is a Windows library that allows the user to access the kinect (but cannot be used on Unix
+    systems)
+
+    References:
+        [1] libfreenect:
+            - Wiki: https://openkinect.org/wiki/Getting_Started
+            - Github repo: https://github.com/OpenKinect/libfreenect
+        [2] OpenNI:
+            - Homepage: https://structure.io/openni
+            - Github repo: https://github.com/occipital/openni2
+            - Openni-Python: https://github.com/severin-lemaignan/openni-python
+            - OpenNI2-FreenectDriver: https://github.com/OpenKinect/libfreenect/tree/master/OpenNI2-FreenectDriver
+        [3] PyKinect: https://github.com/Microsoft/PTVS/wiki/PyKinect
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0., verbose=False):
+        super(KinectInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+
+class FreenectKinectInterface(KinectInterface):
+    r"""Freenect Kinect Interface
+
+    Kinect interface using the `freenect` library.
+
+    References:
+        [1] libfreenect:
+            - Wiki: https://openkinect.org/wiki/Getting_Started
+            - Github repo: https://github.com/OpenKinect/libfreenect
+        [2] Tutorial: naman5.wordpress.com/2014/06/24/experimenting-with-kinect-using-opencv-python\
+                       -and-open-kinect-libfreenect/
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0., verbose=False):
+
+        # data
+        self.rgb = None
+        self.depth = None
+
+        super(FreenectKinectInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+    def get_image(self, convertTo=None):  # cv2.COLOR_RGB2BGR):
+        array, _ = freenect.sync_get_video()
+        if convertTo is not None:
+            array = cv2.cvtColor(array, convertTo)
+        return array
+
+    def get_depth(self):
+        array, _ = freenect.sync_get_depth()
+        array = array.astype(np.uint8)
+        return array
+
+    def run(self):
+        self.rgb = self.get_image()
+        self.depth = self.get_depth()
+
+        return self.rgb, self.depth
+
+
+class OpenNIKinectInterface(KinectInterface):
+    r"""OpenNI Kinect Interface
+
+    Kinect interface using the `openni` library.
+
+    References:
+        [1] https://github.com/danielelic/PyOpenNI2-Utility
+        [2] https://github.com/kanishkaganguly/OpenNIMultiSensorCapture
+        [3] https://docs.opencv.org/3.1.0/d7/d6f/tutorial_kinect_openni.html
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0., verbose=False):
+
+        # initialize openni2; you can give the path to the library as an argument. Otherwise, it will look for
+        # OPENNI2_REDIST and OPENNI2_REDIST64 environment variables.
+        openni2.initialize()
+
+        # open all the devices
+        devices = openni2.Device.open_all()
+
+        # get the correct device (Microsoft Kinect)
+        self.device = None
+        for device in devices:
+            info = device.get_device_info()
+            if info.vendor == 'Microsoft' and info.name == 'Kinect':  # Kinect Interface
+                self.device = device
+                break
+
+        # If didn't find it, return an error
+        if self.device is None:
+            devices = [device.get_device_info() for device in devices]
+            raise ValueError("No Asus devices were detected; we found these devices instead: {}".format(devices))
+
+        if verbose:
+            print(self.device.get_device_info())
+
+        # create RGB and depth streams
+        self.rgb_stream = self.device.create_color_stream()
+        self.depth_stream = self.device.create_depth_stream()
+
+        # start the streams
+        self.rgb_stream.start()
+        self.depth_stream.start()
+
+        # data
+        self.rgb = None
+        self.depth = None
+
+        super(OpenNIKinectInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+    def run(self):
+        # read frames
+        rgb_frame = self.rgb_stream.read_frame()
+        depth_frame = self.depth_stream.read_frame()
+
+        # get buffers
+        rgb = rgb_frame.get_buffer_as_uint8()
+        depth = depth_frame.get_buffer_as_uint16()
+
+        # convert from buffers to images and reshape them
+        rgb = np.frombuffer(rgb, dtype=np.uint8)
+        rgb = rgb.reshape(rgb_frame.height, rgb_frame.width, 3)
+        depth = np.frombuffer(depth, dtype=np.uint16)
+        depth = depth.reshape(depth_frame.height, depth_frame.width)
+
+        # save images and return them
+        self.rgb, self.depth = rgb, depth
+        return self.rgb, self.depth
+
+    def __del__(self):
+        # close all the streams
+        self.rgb_stream.close()
+        self.depth_stream.close()
+
+        # unload openni2
+        openni2.unload()
+
+
+# TODO: add pose and gesture types
+class KinectSkeletonTrackingInterface(KinectInterface):
+    r"""Skeleton tracking
+
+    Skeleton tracking using the openni and nite libraries.
+
+    References:
+        [1] https://github.com/severin-lemaignan/openni-python
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0., verbose=False, track_hand=False):
+
+        # initialize openni2 and nite2; you can give the path to the library as an argument.
+        # Otherwise, it will look for OPENNI2_REDIST / OPENNI2_REDIST64 and NITE2_REDIST / NITE2_REDIST64 environment
+        # variables.
+        openni2.initialize()
+        nite2.initialize()
+
+        # open all the devices
+        devices = openni2.Device.open_all()
+
+        # get the correct device (Microsoft Kinect)
+        self.device = None
+        for device in devices:
+            info = device.get_device_info()
+            if info.vendor == 'Microsoft' and info.name == 'Kinect':  # Kinect Interface
+                self.device = device
+                break
+
+        # If didn't find it, return an error
+        if self.device is None:
+            devices = [device.get_device_info() for device in devices]
+            raise ValueError("No Asus devices were detected; we found these devices instead: {}".format(devices))
+
+        if verbose:
+            print(self.device.get_device_info())
+
+        # create tracker for the hand or user depending on the given parameter
+        if track_hand:
+            self.tracker = nite2.HandTracker(self.device)
+        else:
+            self.tracker = nite2.UserTracker(self.device)
+
+        # data
+        self.joints = ['head', 'neck', 'torso', 'left_shoulder', 'left_elbow', 'left_hand', 'left_hip', 'left_knee',
+                       'left_foot', 'right_shoulder', 'right_elbow', 'right_hand', 'right_hip', 'right_knee',
+                       'right_foot']
+        joint = nite2.JointType
+        self.nite_joints = [joint.NITE_JOINT_HEAD, joint.NITE_JOINT_NECK, joint.NITE_JOINT_TORSO,
+                            joint.NITE_JOINT_LEFT_SHOULDER, joint.NITE_JOINT_LEFT_ELBOW, joint.NITE_JOINT_LEFT_HAND,
+                            joint.NITE_JOINT_LEFT_HIP, joint.NITE_JOINT_LEFT_KNEE, joint.NITE_JOINT_LEFT_FOOT,
+                            joint.NITE_JOINT_RIGHT_SHOULDER, joint.NITE_JOINT_RIGHT_ELBOW, joint.NITE_JOINT_RIGHT_HAND,
+                            joint.NITE_JOINT_RIGHT_HIP, joint.NITE_JOINT_RIGHT_KNEE, joint.NITE_JOINT_RIGHT_FOOT]
+
+        self.data = {}
+
+        super(KinectSkeletonTrackingInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt,
+                                                              verbose=verbose)
+
+    def run(self):
+        # read frame
+        frame = self.tracker.read_frame()
+
+        # check if users in the frame
+        if frame.users:
+            # for each user in the frame
+            for user in frame.users:
+                # check if it is a new one
+                if user.is_new():
+                    if self.verbose:
+                        print("New user detected! Calibrating...")
+                    self.tracker.start_skeleton_tracking(user.id)
+                    self.data = {user.id: {}}
+
+                # check that the state has been correctly tracked
+                elif user.skeleton.state == nite2.SkeletonState.NITE_SKELETON_TRACKED:
+
+                    # go through each joint and update the user data
+                    for joint, nite_joint in zip(self.joints, self.nite_joints):
+                        j = user.skeleton.joints[nite_joint]
+                        self.data[user.id][joint] = (j.position.x, j.position.y, j.position.z, j.positionConfidence)
+
+                    # self.data[user.id]['updated'] = True
+
+        return self.data
+
+    def __del__(self):
+        # unload nite2 and openni2
+        nite2.unload()
+        openni2.unload()
+
+
+class ROSKinectInterface(CameraInterface):
+    r"""ROS Kinect Interface
+
+    References:
+        [1] http://wiki.ros.org/openni_camera
+    """
+    pass
+
+
+# Tests
+if __name__ == '__main__':
+    pass

pyrobolearn/tools/interfaces/camera/openpose.py

@@ -0,0 +1,224 @@
+#!/usr/bin/env python
+"""Define the Openpose Interface
+
+This extracts the human skeleton from an image or stream of images (from a webcam for instance) using the openpose
+library. See `https://github.com/CMU-Perceptual-Computing-Lab/openpose` for more info about openpose.
+"""
+
+import os
+import cv2
+# import PyOpenPose as OP
+try:
+    import pyopenpose
+except ImportError as e:
+    raise ImportError(repr(e) + '\nTry to install `pyopenpose` by installing the openpose library. Check the script '
+                                '`pyrobolearn/scripts/install_openpose.sh` to install the library and the associated '
+                                'python wrapper.')
+
+from camera import CameraInterface
+from webcam import WebcamInterface
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class OpenPoseInterface(CameraInterface):
+    r"""OpenPose interface
+
+    This class defines the OpenPose interface which uses a camera interface like a webcam or kinect to get the (2D or
+    3D) pictures and map them to the human kinematic skeleton.
+
+    References:
+        [1] OpenPose: github.com/CMU-Perceptual-Computing-Lab/openpose
+        [2] PyOpenPose (official): github.com/CMU-Perceptual-Computing-Lab/openpose/blob/master/doc/modules/python_module.md
+        [3] OpenPose output format: github.com/CMU-Perceptual-Computing-Lab/openpose/blob/master/doc/output.md
+        [4] PyOpenPose (python wrappers): github.com/FORTH-ModelBasedTracker/PyOpenPose
+    """
+
+    def __init__(self, camera=None, detect_face=False, detect_hands=False, openpose_path=None,
+                 use_thread=False, sleep_dt=0, verbose=False):
+
+        # save variables
+        self.detect_face = detect_face
+        self.detect_hands = detect_hands
+
+        # Check the given camera
+        if camera is None:
+            # If None, get pictures from a webcam
+            camera = WebcamInterface(use_thread=False, convertTo=None, verbose=False)
+            self.camera_in_openpose = True
+        else:
+            self.camera_in_openpose = False
+
+        self.camera = camera
+
+        # Define the JOINTS
+        # define the 25 BODY joints
+        self.body_joints = ['Nose', 'Neck', 'RShoulder', 'RElbow', 'RWrist', 'LShoulder', 'LElbow', 'LWrist', 'MidHip',
+                            'RHip', 'RKnee', 'RAnkle', 'LHip', 'LKnee', 'LAnkle', 'REye', 'LEye', 'REar', 'LEar',
+                            'LBigToe', 'LSmallToe', 'LHeel', 'RBigToe', 'RSmallToe', 'RHeel', 'Background']
+        self.body_joint_names_to_ids = dict(zip(self.body_joints, range(len(self.body_joints))))
+
+        # define the 21 HAND joints
+        # for each of the 4 main finger(s) (without thumb), there are 4 joints (proximal, middle, distal, tip)
+        self.hand_joints = ['Palm'] + ['Thumb' + str(i) for i in range(4)] + ['Index' + str(i) for i in range(4)] + \
+                           ['Middle' + str(i) for i in range(4)] + ['Ring' + str(i) for i in range(4)] + \
+                           ['Little' + str(i) for i in range(4)]
+        self.hand_joint_names_to_ids = dict(zip(self.hand_joints, range(len(self.hand_joints))))
+
+        # define the 70 FACE joints
+        # Note that in github.com/CMU-Perceptual-Computing-Lab/openpose/blob/master/doc/media/keypoints_face.png,
+        # the joints are not symmetric but are read from left to right.
+        # The joints are defined as:
+        # face (beard): [0, 16]
+        # right eyebrow: [17, 21]
+        # left eyebrow: [22,26]
+        # nose: [27, 35]
+        # right eye: [36, 41] + 68
+        # left eye: [42, 47] + 69
+        # mouth: [48, 67]
+        self.face_joints = ['Face' + str(i) for i in range(17)] + ['REyebrow' + str(i-17) for i in range(17, 22)] + \
+                           ['LEyebrow' + str(i-22) for i in range(22, 27)] + \
+                           ['Nose' + str(i-27) for i in range(27, 36)] + \
+                           ['REye' + str(i-36) for i in range(36, 42)] + \
+                           ['LEye' + str(i-42) for i in range(42, 48)] + \
+                           ['Mouth' + str(i-48) for i in range(48, 68)] + ['REye6'] + ['LEye6']
+        self.face_joint_names_to_ids = dict(zip(self.face_joints, range(len(self.face_joints))))
+
+        # Check the path to the openpose folder (which contains various models and test images)
+        if openpose_path is None:
+            if 'OPENPOSE_PATH' not in os.environ:
+                raise ValueError("The OPENPOSE_PATH environment variable has not been set properly. Please "
+                                 "then provide the path to the openpose folder by specifying the `openpose_path` "
+                                 "argument")
+            openpose_path = os.environ['OPENPOSE_PATH']
+        self.openpose_path = openpose_path
+
+        # specify the parameters
+        params = dict()
+        params["model_folder"] = path + "models/"
+        if detect_face:
+            params["face"] = True
+        if detect_hands:
+            params["hands"] = True
+
+        # configure openpose
+        self.openpose = pyopenpose.WrapperPython()
+        self.openpose.configure(params)
+        self.openpose.start()
+
+        # define data holder
+        self.datum = pyopenpose.Datum()
+
+        # define frame
+        self.frame = None
+
+        # call superclass constructor
+        super(OpenPoseInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+    @property
+    def num_bodies(self):
+        return len(self.datum.poseKeypoints)
+
+    @property
+    def body_keypoints(self):
+        """Return the keypoints (x,y,confidence) for each body part for each person"""
+        return self.datum.poseKeypoints
+
+    @property
+    def left_hand_keypoints(self):
+        return self.datum.handKeypoints[0]
+
+    @property
+    def right_hand_keypoints(self):
+        return self.datum.handKeypoints[1]
+
+    @property
+    def hand_keypoints(self):
+        return self.left_hand_keypoints, self.right_hand_keypoints
+
+    @property
+    def face_keypoints(self):
+        return self.datum.faceKeypoints
+
+    @property
+    def keypoints(self):
+        return self.body_keypoints, self.face_keypoints, self.left_hand_keypoints, self.right_hand_keypoints
+
+    @property
+    def heatmap(self):
+        return None
+
+    @property
+    def input_image(self):
+        return self.datum.cvInputData
+
+    @property
+    def output_image(self):
+        return self.datum.cvOutputData
+
+    @property
+    def num_gpus(self):
+        return pyopenpose.get_gpu_number()
+
+    def run(self, input_frame=None):
+        if input_frame is None:
+            # read image
+            if self.camera_in_openpose:
+                self.camera.run()
+            img = self.camera.frame
+        else:
+            if isinstance(input_frame, str):
+                img = cv2.imread(input_frame)
+            else:
+                img = input_frame
+
+        # define data holder
+        self.datum = pyopenpose.Datum()
+
+        # process image
+        self.datum.cvInputData = img
+        self.openpose.emplaceAndPop([self.datum])
+
+        # save frame
+        self.frame = self.datum.cvOutputData
+
+        # define dictionary of keypoints
+        keypoints = dict()
+        keypoints['body'] = self.body_keypoints
+        if self.detect_face:
+            keypoints['face'] = self.face_keypoints
+        if self.detect_hands:
+            keypoints['left_hand'] = self.left_hand_keypoints
+            keypoints['right_hand'] = self.right_hand_keypoints
+
+        # display image if specified
+        if self.verbose:
+            cv2.imshow('OpenPose frame', self.frame)
+
+            # quit display if 'esc' button is pressed
+            key = cv2.waitKey(15) & 0xFF
+            if key == 27:
+                self.verbose = False
+                cv2.destroyWindow('frame')
+
+        # return frame and the associated keypoints
+        return self.frame, keypoints
+
+
+# Tests
+if __name__ == '__main__':
+    path = '/home/brian/repos/openpose/'
+    interface = OpenPoseInterface(openpose_path=path)  # , use_thread=False, sleep_dt=1./10, verbose=True)
+
+    while True:
+        frame, keypoints = interface.run()
+        cv2.imshow("OpenPose 1.4.0 - Tutorial Python API", frame)
+        cv2.waitKey(15)

pyrobolearn/tools/interfaces/camera/webcam.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python
+"""Define the Webcam Interface
+
+This provides the main interface to get pictures from the specified webcam.
+"""
+
+
+import cv2  # OpenCV to capture image from webcam
+import os
+
+from camera import CameraInterface
+
+# to close correctly the webcam once we run
+os.environ["OPENCV_VIDEOIO_PRIORITY_MSMF"] = "0"
+
+# For multithreading, check also:
+# - http://blog.blitzblit.com/2017/12/24/asynchronous-video-capture-in-python-with-opencv/
+# - https://nrsyed.com/2018/07/05/multithreading-with-opencv-python-to-improve-video-processing-performance/
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class WebcamInterface(CameraInterface):
+    r"""Webcam Interface
+
+    This class defines the webcam interface. It gets pictures from the webcam, which can then be processed by another
+    tool (such as openpose).
+
+    References:
+        [1] https://docs.opencv.org/3.0-beta/doc/py_tutorials/py_gui/py_video_display/py_video_display.html
+    """
+
+    def __init__(self, webcamId=0, saveVideo=False, filename='output.avi', fps=20, frameSize=(640,480), codec='XVID',
+                 convertTo=cv2.COLOR_BGR2RGB, use_thread=False, sleep_dt=0, verbose=False):
+
+        # create video capture object
+        self.capture = cv2.VideoCapture(webcamId)
+
+        # create video writer
+        if saveVideo:
+            # define codec (DIVX, XVID, MJPG, X264, WMV1, WMV2)
+            codec = cv2.VideoWriter_fourcc(*codec)
+            # define video writer
+            self.writer = cv2.VideoWriter(filename, codec, fps, frameSize)
+        else:
+            self.writer = None
+
+        # variables
+        self.convertTo = convertTo
+        self.verbose = False
+
+        # camera image
+        self.frame = self.run()
+
+        # call superclass constructor
+        super(WebcamInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+    def run(self):  # , display=True, convertToGray=False):
+        # get the frame from the webcam
+        return_code, frame = self.capture.read()
+
+        if not return_code:
+            raise ValueError("There was an error when reading the frame from the webcam")
+
+        # convert to gray (if specified)
+        if self.convertTo == cv2.COLOR_BGR2GRAY:
+            frame = cv2.cvtColor(frame, self.convertTo)
+
+        # display image if specified
+        if self.verbose:
+            cv2.imshow('frame', frame)
+
+            # quit display if 'esc' button is pressed
+            key = cv2.waitKey(1) & 0xFF
+            if key == 27:
+                self.verbose = False
+                cv2.destroyWindow('frame')
+
+        # convert to the specified format
+        if self.convertTo is not None:
+            frame = cv2.cvtColor(frame, self.convertTo)
+
+        # save the frame for the video
+        if self.writer is not None:
+            self.writer.write(frame)
+
+        # set and return the frame
+        self.frame = frame
+        return frame
+
+    def __del__(self):
+        self.capture.release()
+        if self.writer is not None:
+            self.writer.release()
+        # cv2.destroyAllWindows()
+
+
+# Test
+if __name__ == '__main__':
+    from itertools import count
+    import matplotlib.pyplot as plt
+
+    # create interface
+    interface = WebcamInterface(use_thread=True, sleep_dt=1./10, verbose=False)
+
+    # plotting using matplotlib in interactive mode
+    fig = plt.figure()
+    plot = None
+    plt.ion()   # interactive mode on
+
+    for _ in count():
+        # # if don't use thread call `step` or `run` (note that `run` returns the frame but not
+        # interface.step()
+
+        # get the frame and plot it with matplotlib
+        frame = interface.frame
+        if plot is None:
+            plot = plt.imshow(frame)
+        else:
+            plot.set_data(frame)
+            plt.pause(0.01)
+
+        # check if the figure is closed, and if so, get out of the loop
+        if not plt.fignum_exists(fig.number):
+            break
+
+    plt.ioff()  # interactive mode off
+    plt.show()

pyrobolearn/tools/interfaces/controllers/__init__.py

@@ -0,0 +1,9 @@
+
+# import general game controller interface
+from controller import GameControllerInterface
+
+# Xbox controller interface
+from xbox import *
+
+# Playstation controller interface
+from playstation import *

pyrobolearn/tools/interfaces/controllers/controller.py

@@ -0,0 +1,11 @@
+
+from pyrobolearn.tools.interfaces.interface import InputOutputInterface
+
+
+class GameControllerInterface(InputOutputInterface):
+    r"""Game Controller Interface
+
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        super(GameControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)

pyrobolearn/tools/interfaces/controllers/playstation.py

@@ -0,0 +1,272 @@
+#!/usr/bin/env python
+"""Define the PlayStation controller interface
+
+This provides the interfaces for the PlayStation controllers (PS3 and PS4) using the `inputs` library.
+"""
+
+try:
+    from inputs import devices, get_gamepad
+except ImportError as e:
+    raise ImportError(repr(e) + '\nTry to install `inputs`: pip install inputs')
+
+from controller import GameControllerInterface
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class PSControllerInterface(GameControllerInterface):
+    r"""PlayStation Controller Interface
+
+    This provides the code for the PlayStation Controller interface. We use the `inputs` Python library [1, 2].
+    If the PS controller is not detected, please install the necessary drivers.
+
+    References:
+        [1] Python library `inputs`: https://inputs.readthedocs.io
+        [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html
+    """
+
+    def __init__(self, use_thread=False, controller_name='Sony Interactive Entertainment Wireless Controller'):
+        # Check if some gamepads are connected to the computer
+        gamepads = devices.gamepads
+        if len(gamepads) == 0:
+            raise ValueError("No gamepads/controllers were detected.")
+
+        # Check if the specified gamepad is detected
+        self.gamepad = None
+        for gamepad in gamepads:
+            if controller_name in gamepad.name:
+                self.gamepad = gamepad
+                break
+
+        if self.gamepad is None:
+            raise ValueError("The specified gamepad/controller was not detected.")
+
+        # translation
+        buttons = ['BTN_SOUTH', 'BTN_EAST', 'BTN_WEST', 'BTN_NORTH', 'BTN_THUMBL', 'BTN_THUMBR', 'BTN_TL', 'BTN_TL2',
+                   'BTN_TR', 'BTN_TR2', 'BTN_START', 'BTN_SELECT', 'BTN_MODE', 'ABS_HAT0X', 'ABS_HAT0Y', 'ABS_Z', 'ABS_RZ',
+                   'ABS_X', 'ABS_Y', 'ABS_RX', 'ABS_RY']
+        ps4_buttons = ['X', 'O', 'S', 'T', 'LJB', 'RJB', 'L1', 'L2', 'R1', 'R2', 'options', 'share', 'PS', 'L', 'R', 'RT',
+                       'LJX', 'LJY', 'RJX', 'RJY']
+        self.map = dict(zip(buttons, ps4_buttons))
+        self.inv_map = dict(zip(ps4_buttons, buttons))
+
+        # buttons and their values
+        self.buttons = dict(zip(ps4_buttons[:12], [0] * 12))
+        self.buttons.update(dict(zip(['Dpad', 'LJ', 'RJ'], [[0, 0]] * 3)))
+
+        # last updated button
+        self.last_updated_button = None
+
+        super(PSControllerInterface, self).__init__(use_thread)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def X(self):
+        """Button X"""
+        return self.buttons['X']
+
+    @property
+    def O(self):
+        """Button O (circle)"""
+        return self.buttons['O']
+
+    # alias (C = circle)
+    C = O
+
+    @property
+    def S(self):
+        """Button Square"""
+        return self.buttons['S']
+
+    @property
+    def T(self):
+        """Button Triangle"""
+        return self.buttons['T']
+
+    @property
+    def LJB(self):
+        """Left Joystick Button"""
+        return self.buttons['LJB']
+
+    @property
+    def RJB(self):
+        """Right Joystick Button"""
+        return self.buttons['RJB']
+
+    @property
+    def LB(self):
+        """left bumper; button for left index finger"""
+        return self.buttons['LB']
+
+    @property
+    def RB(self):
+        """right bumper; button for right index finger"""
+        return self.buttons['RB']
+
+    @property
+    def menu(self):
+        """menu button"""
+        return self.buttons['menu']
+
+    @property
+    def view(self):
+        """view button"""
+        return self.buttons['view']
+
+    @property
+    def LT(self):
+        """Left trigger; button for left middle finger"""
+        return self.buttons['LT']
+
+    @property
+    def RT(self):
+        """Right trigger; button for right middle finger"""
+        return self.buttons['RT']
+
+    @property
+    def Dpad(self):
+        """Directional pad"""
+        return self.buttons['Dpad']
+
+    @property
+    def LJ(self):
+        """Left Joystick"""
+        return self.buttons['LJ']
+
+    @property
+    def RJ(self):
+        """Right Joystick"""
+        return self.buttons['RJ']
+
+    # aliases
+    left_joystick = LJ
+    right_joystick = RJ
+
+    ###########
+    # Methods #
+    ###########
+
+    def run(self):
+        # print('running')
+        events = self.gamepad.read()  # blocking=False) # get_gamepad()
+        for event in events:
+            event_type, code, state = event.ev_type, event.code, event.state
+            self.__setitem(event_type, self.map.get(code), state)
+
+            # display info
+            if self.verbose:
+                print("Pushed button {} - state = {}".format(self.last_updated_button,
+                                                             self.buttons[self.last_updated_button]))
+
+    def setLeftVibration(self, time_msec):
+        """Set the vibration for the left motor"""
+        self.gamepad.set_vibration(1, 0, time_msec)
+        # display info
+        if self.verbose:
+            print("Set vibration to the left motor for {} msec".format(time_msec))
+
+    def setRightVibration(self, time_msec):
+        """Set the vibration for the right motor"""
+        self.gamepad.set_vibration(0, 1, time_msec)
+        # display info
+        if self.verbose:
+            print("Set vibration to the right motor for {} msec".format(time_msec))
+
+    def setVibration(self, time_msec):
+        """Set the vibration for both motors"""
+        self.gamepad.set_vibration(1, 1, time_msec)
+        # display info
+        if self.verbose:
+            print("Set vibration to both motors for {} msec".format(time_msec))
+
+    def __getitem__(self, key):
+        """Return the specified button"""
+        if key is not None:
+            return self.buttons[key]
+
+    def __setitem(self, event_type, key, value):
+
+        if event_type == 'Absolute':
+            if key == 'LJX':
+                self.buttons['LJ'][0] = value / 32768.  # values between [-32768, 32767]
+                self.last_updated_button = 'LJ'
+            elif key == 'LJY':
+                self.buttons['LJ'][1] = -1. * value / 32768.  # values between [-32767, 32768]
+                self.last_updated_button = 'LJ'
+            elif key == 'RJX':
+                self.buttons['RJ'][0] = value / 32768.  # values between [-32768, 32767]
+                self.last_updated_button = 'RJ'
+            elif key == 'RJY':
+                self.buttons['RJ'][1] = -1. * value / 32768.  # values between [-32767, 32768]
+                self.last_updated_button = 'RJ'
+            elif key == 'DpadX':
+                self.buttons['Dpad'][0] = value  # left (-1) and right (1)
+                self.last_updated_button = 'Dpad'
+            elif key == 'DpadY':
+                self.buttons['Dpad'][1] = -1 * value  # down (-1) and high (1)
+                self.last_updated_button = 'Dpad'
+            elif key == 'LT' or key == 'RT':  # max 1023
+                self.buttons[key] = value / 1023.
+                self.last_updated_button = key
+        elif event_type == 'Key':
+            self.buttons[key] = value
+            self.last_updated_button = key
+
+
+class PS3ControllerInterface(PSControllerInterface):
+    r"""PlayStation 3 Controller Interface
+
+    This provides the code for the PS3 Controller interface.
+
+    In order to make this code works, make sure you installed the `inputs` Python library [1,2].
+    If the PS4 controller is not detected, please install the necessary drivers.
+
+    References:
+        [1] Python library `inputs`: https://inputs.readthedocs.io
+        [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html
+    """
+
+    def __init__(self, use_thread=False):
+        super(PS3ControllerInterface, self).__init__(use_thread=use_thread,
+                                                     controller_name='Sony PLAYSTATION(R)3 Controller')
+
+
+class PS4ControllerInterface(PSControllerInterface):
+    r"""PlayStation 4 Controller Interface
+
+    This provides the code for the PS4 Controller interface.
+
+    In order to make this code works, make sure you installed the `inputs` Python library [1,2].
+    If the PS4 controller is not detected, please install the necessary drivers.
+
+    References:
+        [1] Python library `inputs`: https://inputs.readthedocs.io
+        [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html
+    """
+
+    def __init__(self, use_thread=False):
+        super(PS4ControllerInterface, self).__init__(use_thread=use_thread,
+                                                     controller_name='Sony Interactive Entertainment Wireless Controller')
+
+
+# Tests
+if __name__ == '__main__':
+    device = devices.gamepads[0]
+    while True:
+        events = device.read()  # blocking=False) # get_gamepad()
+        for event in events:
+            event_type, code, state = event.ev_type, event.code, event.state
+            if event_type != 'Absolute':
+                if code != 'SYN_REPORT':
+                    print(code, state)

pyrobolearn/tools/interfaces/controllers/xbox.py

@@ -0,0 +1,267 @@
+
+try:
+    from inputs import devices
+    # TODO: update the library inputs to make it non-blocking
+    # solution: use the fcntl to read in a non-blocking mode, change `InputDevice._get_data(self, read_size)`,
+    # instead of using `read`, use the fcntl library
+    # solution1: use threads
+    # References:
+    # [1] https://github.com/zeth/inputs/pull/9/commits/e1356b945c8f47667fe2f0b4f13b9e8e7b83238a  (doesn't work)
+    # [2] https://github.com/rakshit97/usb-controller-as-mouse  (gives a pointer)
+except ImportError as e:
+    raise ImportError(repr(e) + '\nTry to install `inputs`: pip install inputs')
+
+from controller import GameControllerInterface
+
+
+class XboxControllerInterface(GameControllerInterface):
+    r"""Xbox Controller Interface.
+
+    This provides the code for the Xbox Controller interface.
+
+    In order to make this code works, make sure you installed the `inputs` Python library [1]. If the Xbox controller
+    is not detected, please install the necessary driver. On Ubuntu 16.04, you can install the `xpad` driver by typing
+    the following commands (see [3]):
+
+    ```bash
+    sudo apt-get install git
+    sudo apt-get install dkms
+    sudo git clone https://github.com/paroj/xpad.git /usr/src/xpad-0.4
+    sudo dkms install -m xpad -v 0.4
+    ```
+
+    Notes: I tried the `xboxdrv` driver (https://github.com/xboxdrv/xboxdrv) on Ubuntu 16.04 (kernel 4.1* and 4.4),
+    which was necessary for the following code repos:
+    * https://github.com/FRC4564/Xbox
+    * https://github.com/linusg/xbox360controller  (Note that this requires at least Python 3.3)
+    but it didn't work. For more info, check [3]
+
+    References:
+        [1] Python library `inputs`: https://inputs.readthedocs.io
+        [2] Hardware support: https://inputs.readthedocs.io/en/latest/user/hardwaresupport.html
+        [3] https://askubuntu.com/questions/783587/how-do-i-get-an-xbox-one-controller-to-work-with-16-04-not-steam
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False, controller_name='X-Box One'):
+        # Check if some gamepads are connected to the computer
+        gamepads = devices.gamepads
+        if len(gamepads) == 0:
+            raise ValueError("No gamepads/controllers were detected.")
+
+        # Check if the specified gamepad is detected
+        self.gamepad = None
+        for gamepad in gamepads:
+            if controller_name in gamepad.name:
+                self.gamepad = gamepad
+                break
+
+        if self.gamepad is None:
+            raise ValueError("The specified gamepad/controller was not detected.")
+
+        # translation
+        buttons = ['BTN_SOUTH', 'BTN_EAST', 'BTN_WEST', 'BTN_NORTH', 'BTN_THUMBL', 'BTN_THUMBR', 'BTN_TL', 'BTN_TR',
+                   'BTN_START', 'BTN_SELECT', 'ABS_Z', 'ABS_RZ', 'ABS_HAT0X', 'ABS_HAT0Y', 'ABS_X', 'ABS_Y', 'ABS_RX',
+                   'ABS_RY']
+        xbox_buttons = ['A', 'B', 'Y', 'X', 'LJB', 'RJB', 'LB', 'RB', 'menu', 'view', 'LT', 'RT', 'DpadX', 'DpadY',
+                        'LJX', 'LJY', 'RJX', 'RJY']
+        self.map = dict(zip(buttons, xbox_buttons))
+
+        # buttons and their values
+        self.buttons = dict(zip(xbox_buttons[:12], [0]*12))
+        self.buttons.update(dict(zip(['Dpad', 'LJ', 'RJ'], [[0,0]]*3)))
+
+        # last updated button
+        self.last_updated_button = None
+
+        super(XboxControllerInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def A(self):
+        """Button A"""
+        return self.buttons['A']
+
+    @property
+    def B(self):
+        """Button B"""
+        return self.buttons['B']
+
+    @property
+    def Y(self):
+        """Button Y"""
+        return self.buttons['Y']
+
+    @property
+    def X(self):
+        """Button X"""
+        return self.buttons['X']
+
+    @property
+    def LJB(self):
+        """Left Joystick Button"""
+        return self.buttons['LJB']
+
+    @property
+    def RJB(self):
+        """Right Joystick Button"""
+        return self.buttons['RJB']
+
+    @property
+    def LB(self):
+        """left bumper; button for left index finger"""
+        return self.buttons['LB']
+
+    @property
+    def RB(self):
+        """right bumper; button for right index finger"""
+        return self.buttons['RB']
+
+    @property
+    def menu(self):
+        """menu button"""
+        return self.buttons['menu']
+
+    @property
+    def view(self):
+        """view button"""
+        return self.buttons['view']
+
+    @property
+    def LT(self):
+        """Left trigger; button for left middle finger"""
+        return self.buttons['LT']
+
+    @property
+    def RT(self):
+        """Right trigger; button for right middle finger"""
+        return self.buttons['RT']
+
+    @property
+    def Dpad(self):
+        """Directional pad"""
+        return self.buttons['Dpad']
+
+    @property
+    def LJ(self):
+        """Left Joystick"""
+        return self.buttons['LJ']
+
+    @property
+    def RJ(self):
+        """Right Joystick"""
+        return self.buttons['RJ']
+
+    # aliases
+    left_joystick = LJ
+    right_joystick = RJ
+
+    ###########
+    # Methods #
+    ###########
+
+    def run(self):
+        #print('running')
+        events = self.gamepad.read() #blocking=False) # get_gamepad()
+        for event in events:
+            event_type, code, state = event.ev_type, event.code, event.state
+            self.__setitem(event_type, self.map.get(code), state)
+
+            # display info
+            if self.verbose:
+                print("Pushed button {} - state = {}".format(self.last_updated_button,
+                                                             self.buttons[self.last_updated_button]))
+
+    def setLeftVibration(self, time_msec):
+        """Set the vibration for the left motor"""
+        self.gamepad.set_vibration(1, 0, time_msec)
+        # display info
+        if self.verbose:
+            print("Set vibration to the left motor for {} msec".format(time_msec))
+
+    def setRightVibration(self, time_msec):
+        """Set the vibration for the right motor"""
+        self.gamepad.set_vibration(0, 1, time_msec)
+        # display info
+        if self.verbose:
+            print("Set vibration to the right motor for {} msec".format(time_msec))
+
+    def setVibration(self, time_msec):
+        """Set the vibration for both motors"""
+        self.gamepad.set_vibration(1, 1, time_msec)
+        # display info
+        if self.verbose:
+            print("Set vibration to both motors for {} msec".format(time_msec))
+
+    def __getitem__(self, key):
+        """Return the specified button"""
+        if key is not None:
+            return self.buttons[key]
+
+    def __setitem(self, event_type, key, value):
+
+        if event_type == 'Absolute':
+            if key == 'LJX':
+                self.buttons['LJ'][0] = value / 32768.          # values between [-32768, 32767]
+                self.last_updated_button = 'LJ'
+            elif key == 'LJY':
+                self.buttons['LJ'][1] = -1. * value / 32768.    # values between [-32767, 32768]
+                self.last_updated_button = 'LJ'
+            elif key == 'RJX':
+                self.buttons['RJ'][0] = value / 32768.          # values between [-32768, 32767]
+                self.last_updated_button = 'RJ'
+            elif key == 'RJY':
+                self.buttons['RJ'][1] = -1. * value / 32768.    # values between [-32767, 32768]
+                self.last_updated_button = 'RJ'
+            elif key == 'DpadX':
+                self.buttons['Dpad'][0] = value         # left (-1) and right (1)
+                self.last_updated_button = 'Dpad'
+            elif key == 'DpadY':
+                self.buttons['Dpad'][1] = -1 * value    # down (-1) and high (1)
+                self.last_updated_button = 'Dpad'
+            elif key == 'LT' or key == 'RT':  # max 1023
+                self.buttons[key] = value / 1023.
+                self.last_updated_button = key
+        elif event_type == 'Key':
+            self.buttons[key] = value
+            self.last_updated_button = key
+
+
+class Xbox360ControllerInterface(XboxControllerInterface):
+    r"""Xbox 360 Controller Interface
+
+    """
+
+    def __init__(self, use_thread=False):
+        super(Xbox360ControllerInterface, self).__init__(use_thread=use_thread, controller_name='X-Box 360')
+
+
+class XboxOneControllerInterface(XboxControllerInterface):
+    r"""Xbox One Controller Interface
+
+    """
+
+    def __init__(self, use_thread=False):
+        super(XboxOneControllerInterface, self).__init__(use_thread=use_thread, controller_name='X-Box One')
+
+
+# Tests
+if __name__ == '__main__':
+    import time
+    from itertools import count
+
+    # create interface
+    xbox = XboxOneControllerInterface()
+
+    for _ in count():
+        # run one step with the interface
+        xbox.run()  # same as `step()` if we are not using threads
+
+        # get the last update and print it
+        b = xbox.last_updated_button
+        print("Last updated button: {} with value: {}".format(b, xbox[b]))
+
+        # sleep a bit
+        time.sleep(0.01)

pyrobolearn/tools/interfaces/interface.py

@@ -0,0 +1,140 @@
+#!/usr/bin/env python
+"""Define the abstract interfaces
+
+This defines the Input/Output abstract interfaces. They allowed to get information from input systems (such as
+the mouse, keyboard, microphone, webcam/kinect, VR/AR tools, game controllers, and so on), and/or send information
+to output systems (such as speakers, game controllers, VR/AR tools, etc).
+These interfaces are independent from the rest of the code; they are not coupled to the simulator, robots, or world.
+The code that connects the interfaces to the simulator or elements inside this last one are the `bridges` defined
+in `pyrobolearn/tools/bridges`.
+"""
+
+import threading
+import time
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+def thread_loop(run):
+    """decorator to make the function run in a loop if it is a thread"""
+    def fct(self, *args, **kwargs):
+        if self.use_thread:
+            while True:
+                run(*args, **kwargs)
+        else:
+            run(*args, **kwargs)
+    return fct
+
+
+class Interface(object):
+    r"""Interface (abstract class)
+
+    The interface links input systems (such as the mouse, keyboard, microphone, webcam/kinect, VR/AR tools, game
+    controllers, and so on), and/or output systems (such as speakers, game controllers, VR/AR tools, etc) to the
+    simulator. Specifically, it can connects such systems to the `world`, `simulator`, or `robot`.
+
+    They can be divided into 3 classes:
+    * Input interfaces: these interfaces interpret the signals received by an input system, and act on the world.
+                        Examples include the mouse, keyboard, microphone, webcam/kinect, joysticks, and others.
+                        For more info, see the `InputInterface` class.
+    * Output interfaces: these interfaces received signals from the world, and output these through an output system.
+                         For instance, a robot could 'speak' in the world, and this signal could be redirected to
+                         computer speakers or headphones. For more info, see the `OutputInterface` class.
+    * Input and Output interfaces: these interfaces connects with a system that can be used as inputs and outputs.
+                                   Such instances include controllers with feedback (using vibration or force),
+                                   VR/AR tools, phones, etc. For more info, see the `InputOutputInterface` class.
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        self.use_thread = use_thread
+        self.dt = sleep_dt
+        self.data = None
+        self.verbose = verbose
+
+        if self.use_thread:
+            self.thread = threading.Thread(target=self._run)
+            self.thread.start()
+
+    def step(self):
+        """
+        Perform one step with the interface; the interface checks the events (if it is not run in a separate thread)
+        """
+        if not self.use_thread:
+            return self._run()
+
+    def _run(self, *args, **kwargs):
+        """
+        Code that calls the `run` method implemented by the user, and loop over it if we are in a thread.
+        """
+        if self.use_thread:
+            while True:
+                self.run(*args, **kwargs)
+                time.sleep(self.dt)
+        else:
+            return self.run(*args, **kwargs)
+
+    # @thread_loop
+    def run(self, *args, **kwargs):
+        """
+        Code to be run by the interface. This needs to be implemented by the user
+        """
+        pass
+
+    def close(self):
+        """
+        Stop and close the interface.
+        """
+        pass
+
+    def __call__(self):
+        self.step()
+
+    def __del__(self):
+        self.close()
+
+
+class InputInterface(Interface):
+    r"""Input Interface (abstract class)
+
+    These interfaces interpret the signals received by an input system (such as a mouse, keyboard, microphone,
+    webcam/kinect, game controllers, VR/AR controllers), and act on the world.
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        super(InputInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+
+class OutputInterface(Interface):
+    r"""Output Interface (abstract class)
+
+    These interfaces received signals from the world, and output these through an output system. For instance,
+    a robot could 'speak' in the world, and this signal could be redirected to computer speakers or headphones.
+    The screen of your computer is another obvious output system.
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        super(OutputInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)
+
+
+class InputOutputInterface(Interface):
+    r"""Input and Output interface (abstract class)
+
+    These interfaces can receive and interpret signals from certain systems, and redirect or produce signals to be
+    sent to these same systems. Such instances include controllers with feedback (using vibration or force),
+    VR/AR tools, phones, and others.
+
+    Specifically, you could send images captured by a camera in the world/simulator, stream these to a phone or VR/AR
+    headsets. At the same time, you could capture inputs from the screen (or other sensors) of the phone, or from VR
+    controllers, and stream these to this interface that will act on the world/simulator.
+    """
+
+    def __init__(self, use_thread=False, sleep_dt=0, verbose=False):
+        super(InputOutputInterface, self).__init__(use_thread=use_thread, sleep_dt=sleep_dt, verbose=verbose)

pyrobolearn/tools/interfaces/mouse_keyboard/__init__.py

@@ -0,0 +1,3 @@
+
+# import mouse keyboard interface
+from mousekeyboard import MouseKeyboardInterface

pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py

@@ -0,0 +1,228 @@
+#!/usr/bin/env python
+"""Define the mouse-keyboard interface.
+
+Dependencies:
+- `pyrobolearn.tools.interfaces`
+- `pyrobolearn.simulators` (only for the mouse keyboard interface; we get the events from the simulator)
+"""
+
+import numpy as np
+
+from pyrobolearn.utils.bullet_utils import Key, Mouse
+from pyrobolearn.tools.interfaces import InputInterface
+from pyrobolearn.simulators import Simulator
+# from pyrobolearn.worlds import World
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2018, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "MIT"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class MouseKeyboardInterface(InputInterface):
+    r"""Mouse Keyboard Interface for the simulator
+
+    Provide the mouse keyboard interface which can for instance allows the user to interact with the world
+    (and thus the robots) using the mouse and keyboard.
+
+    For IK, you can perform it:
+    * from one link to another link: select the first link by a `left-click` then the second link that you wish to move
+                                     with a `right-click`
+    * using the full-body: do not select any objects, and just `right-click` on the corresponding link
+    You can also use the sliders to move the corresponding link. When using the mouse, it will move the selected link
+    in the plane which is parallel to the camera (i.e. perpendicular to the axis that goes from the camera lens to
+    the link), and contains the CoM of the selected link.
+
+    Mouse:
+    * predefined in pybullet
+        * `scroll wheel`: zoom
+        * `ctrl`/`alt` + `scroll button`: move the camera using the mouse
+        * `ctrl`/`alt` + `left-click`: rotate the camera using the mouse
+        * `left-click` and drag: transport the object
+
+    Keyboard:
+    * predefined in pybullet:
+        * `w`: show the wireframe (collision shapes)
+        * `s`: show the reference system
+        * `v`: show bounding boxes
+        * `g`: show/hide parts of the GUI the side columns (check `sim.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)`)
+        * `esc`: quit the simulator
+
+    For programmers:
+        In Pybullet, `sim.getMouseEvents()` return a list of mouse events in the following format:
+        [(eventType, x, y, buttonIndex, buttonState)]
+        where
+        * `eventType` is 1 if the mouse is moving, 2 if a button has been pressed or released
+        * `(x,y)` are the mouse position on the screen (expressed in pixel, and returned as float (they should be int))
+        * `buttonIndex` is -1 if nothing, 0 if left button, 1 if scroll wheel (pressed), 2 if right button
+        * `buttonState` is 0 if nothing, 3 if the button has been pressed, 4 is the button has been released,
+            1 if the key is down (never observed), 2 if the key has been triggered (never observed).
+
+        In Pybullet, `sim.getKeyboardEvents()` return a dictionary of key events in the following format:
+            {keyID: keyState}
+        where
+        * `keyID` is an integer (ascii code) representing the key. Some special keys like shift, arrows, and others are
+        are defined in pybullet such as `B3G_SHIFT`, `B3G_LEFT_ARROW`, `B3G_UP_ARROW`,...
+        * `keyState` is an integer. 3 if the button has been pressed, 1 if the key is down, 2 if the key has been
+        triggered, 4 if it has been released.
+    """
+
+    def __init__(self, simulator, verbose=False):
+        """
+        Initialize the Mouse-Keyboard Interface. This interface is a little bit special in the sense that we use
+        the simulator to provide the mouse and keyboard events instead of using an external library.
+
+        Args:
+            simulator (Simulator): simulator instance from which we capture mouse and keyboard events.
+            verbose (bool): If True, print information on the standard output.
+        """
+        super(MouseKeyboardInterface, self).__init__(use_thread=False, sleep_dt=0, verbose=verbose)
+
+        # set simulator
+        self.simulator = simulator
+
+        # define variables for mouse events
+        # Note: the difference between pressed and moment, is that a key is pressed during a short instant,
+        # while the key can be down for a long period of time.
+        self.mouse_moving = False
+        self.mouse_pressed = False
+        self.left_click_pressed = False
+        self.right_click_pressed = False
+        self.left_click_down = False
+        self.right_click_down = False
+        self.mouse_x, self.mouse_y = 0, 0
+
+        # define variables for key events
+        self.key_pressed = []
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def simulator(self):
+        """Return the simulator instance."""
+        return self._simulator
+
+    @simulator.setter
+    def simulator(self, simulator):
+        """Set the simulator instance."""
+        # if isinstance(simulator, Simulator):
+        #     pass
+        # elif isinstance(simulator, World):
+        #     simulator = simulator.simulator
+        # elif isinstance(simulator, Env):
+        #     simulator = simulator.world.simulator
+        # else:
+        # if not isinstance(simulator, Simulator):
+        #     raise TypeError("Expecting the simulator to be an instance of Simulator, "
+        #                     "got instead {}".format(type(simulator)))
+        # if isinstance(simulator, World):
+        #     simulator = simulator.simulator
+        self._simulator = simulator
+
+    @property
+    def mouse_pressed(self):
+        return self.left_click_pressed or self.right_click_pressed
+
+    @mouse_pressed.setter
+    def mouse_pressed(self, pressed):
+        self.left_click_pressed = pressed
+        self.right_click_pressed = pressed
+
+    @property
+    def mouse_down(self):
+        return self.left_click_down or self.right_click_down
+
+    ###########
+    # Methods #
+    ###########
+
+    def check_key_events(self):
+        # get key events
+        events = self.simulator.getKeyboardEvents()
+
+        # create new list of key pressed
+        self.key_pressed = []
+        if Key.shift in events:
+            self.key_pressed.append(Key.shift)
+        if Key.alt in events:
+            self.key_pressed.append(Key.alt)
+        if Key.ctrl in events:
+            self.key_pressed.append(Key.ctrl)
+
+        # go through each keyboard event
+        for key, state in events.items():
+            if state == Key.pressed:  # or state == Key.down:  # the key is pressed or down
+                self.key_pressed.append(key)
+
+    def check_mouse_events(self):
+        # get mouse events
+        events = self.simulator.getMouseEvents()
+
+        # reset mouse events
+        self.mouse_moving, self.mouse_pressed = False, False
+
+        # go through each mouse event
+        for event in events:
+            eventType, x, y, idx, state = event
+            self.mouse_x, self.mouse_y = x, y
+
+            # check if mouse is moving
+            if eventType == Mouse.moving:
+                self.mouse_moving = True
+
+            # check if button has been pressed or released
+            elif eventType == Mouse.button:
+                # check left click
+                if idx == Mouse.left_click:
+                    if state == Mouse.pressed:
+                        self.left_click_pressed = True
+                        self.left_click_down = True
+                    elif state == Mouse.released:
+                        self.left_click_pressed = False
+                        self.left_click_down = False
+
+                # check right click
+                elif idx == Mouse.right_click:
+                    if state == Mouse.pressed:
+                        self.right_click_pressed = True
+                        self.right_click_down = True
+                    elif state == Mouse.released:
+                        self.right_click_pressed = False
+                        self.right_click_down = False
+
+    def step(self):
+        """Perform a step with the interface."""
+        # check key events
+        self.check_key_events()
+
+        # check mouse events
+        self.check_mouse_events()
+
+        if self.verbose:
+            print("\nKey pressed: {}".format(self.key_pressed))
+            print("Mouse moving? {}".format(self.mouse_moving))
+            print("Mouse pressed? {}".format(self.mouse_pressed))
+            print("Mouse down? {}".format(self.mouse_down))
+
+
+# Tests
+if __name__ == '__main__':
+    from pyrobolearn.simulators import BulletSim
+    import time
+    from itertools import count
+
+    # create simulator
+    sim = BulletSim()
+
+    # create interface
+    interface = MouseKeyboardInterface(sim, verbose=True)
+
+    for _ in count():
+        interface.step()
+        time.sleep(1. / 2)

pyrobolearn/tools/interfaces/robots/__init__.py

@@ -0,0 +1,3 @@
+
+# General import
+from robot import RobotInterface

pyrobolearn/tools/interfaces/robots/robot.py

@@ -0,0 +1,11 @@
+
+# Import general interface
+from pyrobolearn.tools.interfaces.interface import Interface, InputInterface, OutputInterface, InputOutputInterface
+
+
+class RobotInterface(Interface):
+    r"""Robot Interface
+
+    This interfaces receives data from the real robot, and cache it here.
+    """
+    pass

pyrobolearn/tools/interfaces/sensors/__init__.py

@@ -0,0 +1,7 @@
+
+
+# general imports
+from sensor import *
+
+# EMG sensor
+from emg import EMGInterface

pyrobolearn/tools/interfaces/sensors/emg.py

@@ -0,0 +1,9 @@
+
+from sensor import BioSensorInterface
+
+
+class EMGInterface(BioSensorInterface):
+    r"""EMG sensor interface
+
+    """
+    pass

pyrobolearn/tools/interfaces/sensors/sensor.py

@@ -0,0 +1,16 @@
+
+from pyrobolearn.tools.interfaces import InputInterface
+
+
+class SensorInterface(InputInterface):
+    r"""Sensor Interface
+
+    """
+    pass
+
+
+class BioSensorInterface(InputInterface):
+    r"""Bio-Sensor Interface
+
+    """
+    pass

pyrobolearn/tools/interfaces/suits/__init__.py

@@ -0,0 +1,6 @@
+
+# general import
+from suit import SuitInterface
+
+# Xsens suit
+from xsens import XsensSuitInterface

pyrobolearn/tools/interfaces/suits/suit.py

@@ -0,0 +1,9 @@
+
+from pyrobolearn.tools.interfaces import InputInterface
+
+
+class SuitInterface(InputInterface):
+    r"""Suit Interface
+
+    """
+    pass

pyrobolearn/tools/interfaces/suits/xsens.py

@@ -0,0 +1,9 @@
+
+from suit import SuitInterface
+
+
+class XsensSuitInterface(SuitInterface):
+    r"""Xsens suit interface
+
+    """
+    pass

pyrobolearn/tools/interfaces/vr/__init__.py

@@ -0,0 +1,11 @@
+
+# general interface
+from vr import VRInterface
+
+## VR interfaces ##
+
+# Oculus
+from oculus import OculusInterface
+
+# HTC
+from htc import HTCViveInterface

pyrobolearn/tools/interfaces/vr/htc.py

@@ -0,0 +1,23 @@
+
+from pyrobolearn.tools.interfaces.vr import VRInterface
+
+
+class HTCViveInterface(VRInterface):
+    r"""HTC Vive Interface
+
+    To install:
+    * SteamVR
+    * OpenVR
+    * PyOpenVR
+
+    References:
+        [1] OpenVR: https://github.com/ValveSoftware/openvr
+        [2] PyOpenVR: https://github.com/cmbruns/pyopenvr
+            with wiki: https://github.com/cmbruns/pyopenvr/wiki/API-Documentation
+        [3] https://github.com/osudrl/CassieVrControls/wiki/OpenVR-Quick-Start
+        [4] https://github.com/osudrl/OpenVR-Tracking-Example
+        [5] How to use the HTC Vive Trackers in Ubuntu using Python 3.6:
+            https://gist.github.com/DanielArnett/c9a56c9c7cc0def20648480bca1f6772
+    """
+    def __init__(self, world, use_controllers=True, use_headset=True):
+        super(HTCViveInterface, self).__init__()

pyrobolearn/tools/interfaces/vr/oculus.py

@@ -0,0 +1,319 @@
+# Define the OculusTouch class which communicates with Unity (on Windows) using TCP.
+# Currently, Oculus has only support for Windows systems. However, several
+# libraries such as ROS only runs on Linux systems. Thus, we can run Unity on
+# a Windows system, associate the Unity scripts with the Oculus GameObjects, and then
+# run this file on a Unix system (such as Linux or MacOSX) which will communicate by TCP.
+# The scripts for Unity can be found in the `unity-scripts` folder.
+#
+# Currently, this code is the server while the code running in Unity on Windows is the client.
+
+import Queue
+import socket
+import struct
+from threading import Thread
+
+import cv2
+
+from pyrobolearn.utils.bullet_utils import RGBAColor
+from pyrobolearn.tools.interfaces.vr import VRInterface
+# from pyrobolearn.worlds.world import BasicWorld
+
+
+class OculusInterface(VRInterface):
+    r"""Oculus VR Interface
+
+    Hardware: Oculus Rift headset + Touch controllers
+    """
+
+    def __init__(self, world, ip=True, port=5111, use_controllers=True, use_headset=False, use_threading=False,
+                 rate=None):
+        """Initialize the Oculus interface.
+
+        Args:
+            world (World, Env): the world
+            use_controllers (bool): True if you want to use the controllers (stream commands from the controllers to
+                the simulator, and send feedback
+            use_headset (bool): True if you want to use the headsets (stream the pictures from the simulator to the
+                headset, and the headset position/orientation to the simulator)
+            ip (str): IP of the system to stream and receive the data. This is useful if the VR hardware is not
+                supported on the current OS. For instance, if you are running on Linux and the VR hardware only runs
+                on Windows, then by specifying the Windows system IP, the data will be streamed between these 2 systems.
+                Note that when streaming between 2 systems, they have to agree about the data they are receiving and
+                sending. Thus, the (code of the) interface on the Windows system is completely dependent on this
+                interface. Use the corresponding code available in the `vr` folder, and follow the set-up instructions.
+                Be sure that you use the correct corresponding interface on the other system, which will depend on the
+                two previous arguments `use_controllers` and `use_headset`.
+                Note that we currently use `Unity` as the interface between the VR hardware and the Windows system, as
+                much of the code is already provided. A global overview of the set-up can be depicted as:
+                pyrobolearn (Linux/Mac) <--TCP/UDP--> Unity (Windows) <----> VR hardware
+            port (int): Port of the system to stream the data
+            use_threading (bool): True if you want to use threads to send the pictures
+            rate (int): acquisition rate of the camera images
+        """
+        super(OculusInterface, self).__init__()
+        self.MSG_SIZE = 0
+
+        self.use_headset = use_headset # TODO: if we use the headset, we automatically use threads
+        self.use_threading = use_threading
+
+        if use_controllers:
+            self.MSG_SIZE = 304
+
+        # Simulator world
+        self.world = world
+        self.sim = self.world.simulator
+        self.task = None
+
+        # create visual spheres in the world for the hands
+        self.worldCamera = self.world.getMainCamera()
+        V, P, Vp, V_inv, P_inv, Vp_inv = self.worldCamera.getMatrices(True)
+        camera = self.worldCamera.getDebugVisualizerCamera(convert=False)
+        width, height = camera[:2]
+        posL = np.array([width / 2 - 20, height / 2, 0.95, 1])
+        posR = np.array([width / 2 + 20, height / 2, 0.95, 1])
+        posL = self.worldCamera.screenToWorld(posL, Vp_inv, P_inv, V_inv)[:3]
+        posR = self.worldCamera.screenToWorld(posR, Vp_inv, P_inv, V_inv)[:3]
+        self.leftSphere = self.world.loadVisualSphere(posL, radius=0.1, color=RGBAColor.red)  # red
+        self.rightSphere = self.world.loadVisualSphere(posR, radius=0.1, color=RGBAColor.blue)  # blue
+
+        # Check IP: get IP of this computer if not provided
+        if ip:
+            s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+            s.connect(("8.8.8.8", 80))
+            ip = s.getsockname()[0]
+
+        # Threads
+        self.running = True
+        self.queue = Queue.Queue(10)
+        self.threads = []
+        if self.use_headset:
+            thread = Thread(target=self.runThread, args=(ip, port + 1))
+            self.threads.append(thread)
+
+        # Connection over the network for joysticks
+        print('Creating socket')
+        self.sock = socket.socket(socket.AF_INET,  # Internet
+                                  socket.SOCK_STREAM)  # TCP = SOCK_STREAM
+        self.server_address = (ip, port)
+        self.sock.bind(self.server_address)
+        self.sock.listen(1)  ## Only for TCP
+        print('Waiting for connection...')
+        self.connection, self.client_address = self.sock.accept()
+
+        # VR
+        self.head, self.leftHand, self.rightHand = [], [], []
+        self.leftJoystick, self.rightJoystick = [], []
+        self.leftVibration, self.rightVibration = 0, 0
+        self.vibrationTime = 1
+        self.prevOculusHeadPos = None
+
+        # Camera images
+        if rate is None:
+            rate = np.inf
+        self.cnt, self.rate = 0, rate
+        self.width, self.height = 400, 400
+        self.encode_params = [int(cv2.IMWRITE_JPEG_QUALITY), 80]
+
+        self.leftCollided = False
+        self.rightCollided = False
+
+    def recv(self):
+        data = self.connection.recvfrom(self.MSG_SIZE)
+        poses = data[0].split(";")
+        for pose in poses:
+            name, value = pose.split("=")
+            values = np.array(value.split(","))
+            values = np.asfarray(values, float)
+            # TODO: replace the if-else by dict of functions
+            if name == "H": # head: position + quaternion
+                self.head = [values[:3], values[3:]]
+            elif name == "L": # left hand: position + quaternion
+                self.leftHand = [values[:3], values[3:]]
+            elif name == "R": # right hand: position + quaternion
+                self.rightHand = [values[:3], values[3:]]
+            elif name == 'JL': # left joystick [touch, button, lateral, forward]
+                self.leftJoystick = [values[0], values[1], values[-2:]]
+            elif name == 'JR': # right joystick: [touch, button, lateral, forward]
+                self.rightJoystick = [values[0], values[1], values[-2:]]
+                # move the camera by rotating
+                yaw, pitch = values[-2:]
+                #self.worldCamera.addYawPitch(yaw, pitch, radian=False)
+                #print(pitch, yaw)
+                pos = self.worldCamera.targetPosition
+                dist = self.worldCamera.dist
+            elif name == 'BA': # button A: [touch, button]
+                pass
+            elif name == 'BB': # button B: [touch, button]
+                pass
+            elif name == 'BX': # button X: [touch, button]
+                pass
+            elif name == 'BY': # button Y: [touch, button]
+                pass
+            elif name == 'BS': # button start: button
+                pass
+            elif name == 'BLT': # button left index trigger: [button, trigger]
+                pass
+            elif name == 'BLH': # button left hand trigger: trigger
+                pass
+            elif name == 'BRT': # button right index trigger: [button, trigger]
+                pass
+            elif name == 'BRH': # button right hand trigger: trigger
+                pass
+
+        ### update world ###
+
+        headWorldPos = self.worldCamera.position
+        targetPos = self.worldCamera.targetPosition
+        forwardVec, upVec, lateralVec = self.worldCamera.getVectors()
+        if self.prevOculusHeadPos is None:
+            self.prevOculusHeadPos = self.head[0]
+
+        # update world camera position and orientation based on Oculus headset and joysticks
+        #R = np.array(self.sim.getMatrixFromQuaternion(self.head[1])).reshape(3,3)
+        #targetPos = R.dot(targetPos)
+        #targetPos += (self.head[0] - self.prevOculusHeadPos)
+        lateral, forward = self.leftJoystick[-1] # move the camera by translating
+        targetPos += 0.1 * (forward * forwardVec + lateral * lateralVec)
+        self.worldCamera.targetPosition = targetPos
+
+        # update hand positions in world
+        leftHandWorldPos = headWorldPos + (self.leftHand[0] - self.head[0])
+        rightHandWorldPos = headWorldPos + (self.rightHand[0] - self.head[0])
+        #self.world.moveObject(self.leftSphere, self.leftHand[0], (0, 0, 0, 1))
+        #self.world.moveObject(self.rightSphere, self.rightHand[0], (0, 0, 0, 1))
+        self.world.moveObject(self.leftSphere, leftHandWorldPos, (0, 0, 0, 1))
+        self.world.moveObject(self.rightSphere, rightHandWorldPos, (0, 0, 0, 1))
+
+        # change color if hands collide with an object
+        self.leftCollided = self.updateSphereColor(self.leftSphere, self.leftCollided,
+                                                   RGBAColor.orange, RGBAColor.red)
+        self.rightCollided = self.updateSphereColor(self.rightSphere, self.rightCollided,
+                                                    RGBAColor.green, RGBAColor.blue)
+
+        # get pictures for the eyes
+        if self.use_headset and (self.cnt % self.rate) == 0:
+            self.cnt = 0
+            # get picture for the eyes
+            leftPic = self.getEyeRGBImage(headWorldPos, targetPos, lateralVec, beta=-0.32)
+            rightPic = self.getEyeRGBImage(headWorldPos, targetPos, lateralVec, beta=0.32)
+            #if self.use_threading:
+            # add them to the queue
+            self.queue.put((leftPic, rightPic))
+            #else:
+            #    # compress pictures and send them over the network
+            #    self.compressAndSendPicture(leftPic, self.connection)
+            #    self.compressAndSendPicture(rightPic, self.connection)
+
+        self.cnt += 1
+
+    def updateSphereColor(self, sphere, hasCollidedPreviously, collisionColor, freeColor):
+        aabb = self.world.getObjectAABB(sphere)
+
+        if len(self.world.getObjectIdsInAABB(aabb[0], aabb[1])) > 1:
+            update = not hasCollidedPreviously
+        else:
+            update = hasCollidedPreviously
+
+        if update:
+            hasCollidedPreviously = not hasCollidedPreviously
+            if hasCollidedPreviously:
+                self.world.changeObjectColor(sphere, color=collisionColor)
+            else:
+                self.world.changeObjectColor(sphere, color=freeColor)
+
+        return hasCollidedPreviously
+
+    def runThread(self, ip, port):
+        # create socket for image
+        sock = socket.socket(socket.AF_INET,  # Internet
+                             socket.SOCK_STREAM)  # UDP = SOCK_DGRAM / TCP = SOCK_STREAM
+        server_address = (ip, port) # ip and port
+        sock.bind(self.server_address)
+        sock.listen(1)  ## Only for TCP
+        print('Thread: waiting for connection...')
+        connection, client_address = sock.accept()
+        print('Thread: connected')
+
+        # run thread
+        while self.running:
+            # get images added in the queue
+            images = self.queue.get(block=True)
+            # compress pictures and send them over the network
+            for image in images:
+                self.compressAndSendPicture(image, connection)
+            #time.sleep(1./60)
+
+    def getEyeRGBImage(self, headWorldPos, targetPos, lateralVec, beta=0.32):
+        eyePos = headWorldPos + beta * lateralVec
+        eyeTargetPos = targetPos + beta * lateralVec
+        V = self.sim.computeViewMatrix(cameraEyePosition=eyePos, cameraTargetPosition=eyeTargetPos,
+                                       cameraUpVector=(0,0,1))
+        pic = np.array(self.sim.getCameraImage(self.width, self.height, viewMatrix=V)[2])
+        pic = pic.reshape(self.width, self.height, 4)[:, :, :3]
+        return pic
+
+    def compressAndSendPicture(self, image, connection):
+        retval, image = cv2.imencode('.jpg', image, self.encode_params)
+        image = image.tostring()
+        connection.sendall(struct.pack('<i', len(image)))
+        connection.sendall(image)
+
+    def send(self):
+        msg = "VL=" + format(self.leftVibration, '03') + "," + format(self.vibrationTime, '03') + \
+              ";VR=" + format(self.rightVibration, '03') + "," + format(self.vibrationTime, '03')
+        self.connection.sendall(msg)
+
+    def step(self):
+        self.recv()
+        self.send()
+
+    # alias
+    update = step
+
+    def printState(self):
+        print("Head: {}".format(self.head))
+        print("Left hand: {}".format(self.leftHand))
+        print("Right hand: {}".format(self.rightHand))
+
+    def setVibration(self, left=0, right=0, vibrationTime=1):
+        """
+        Set the level of vibration on the corresponding oculus touch for the specified number of iterations/time.
+        The level of vibration of each controller is between 0 and 255.
+        """
+        self.leftVibration, self.rightVibration = min(int(left), 255), min(int(right), 255)
+        self.vibrationTime = min(int(vibrationTime), 200)
+
+    def __del__(self):
+        # stop threads
+        self.running = False
+        for t in self.threads:
+            t.join()
+        # stop connection
+        self.connection.close()
+        self.sock.close()
+
+
+# Test
+if __name__ == "__main__":
+    import pybullet as p
+    from pybullet_envs.bullet.bullet_client import BulletClient
+    import time
+    import numpy as np
+    from itertools import count
+
+    # create simulator
+    sim = BulletClient(connection_mode=p.GUI)
+
+    # create world
+    world = BasicWorld(sim)
+
+    # create interface
+    interface = OculusInterface(world, port=5111)
+
+    # run simulation
+    for t in count():
+        interface.step()
+        #interface.printState()
+        # step in the simulation
+        world.step()
+        time.sleep(1./60)

pyrobolearn/tools/interfaces/vr/oculus_windows/meshes/LICENSE.txt

@@ -0,0 +1,6 @@
+Licensed under the Creative Commons Attribution 4.0
+Copyright © 2017 Oculus VR, LLC. All rights reserved.
+
+The text of this may be found at: https://creativecommons.org/licenses/by/4.0/
+
+License (the "License"). Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.

pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/Loom.cs

@@ -0,0 +1,121 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Threading;
+using UnityEngine;
+using System.Linq;
+
+
+public class Loom : MonoBehaviour {
+    // taken from: http://www.programering.com/a/MzM1UzNwATg.html
+    // The important functions to use are:
+    // - RunAsync(Action) which runs a set of statements on another Thread
+    // - QueueOnMainThread(Action, [optional] float time) - which runs a set of statements on the main thread (with an optional delay)
+
+    public static int maxThreads = 8;
+    static int numThreads;
+
+    private static Loom _current;
+    private int _count;
+    static bool initialized;
+
+    private List<Action> _actions = new List<Action>();
+    private List<DelayedQueueItem> _delayed = new List<DelayedQueueItem>();
+    List<DelayedQueueItem> _currentDelayed = new List<DelayedQueueItem>();
+    List<Action> _currentActions = new List<Action>();
+
+    public struct DelayedQueueItem {
+        public float time;
+        public Action action;
+    }
+    
+    public static Loom Current {
+        get {
+            Initialize();
+            return _current;
+        }
+    }
+
+    void Awake() {
+        _current = this;
+        initialized = true;
+    }
+
+    static void Initialize() {
+        if (!initialized) {
+            if (!Application.isPlaying)
+                return;
+            initialized = true;
+            var g = new GameObject("Loom");
+            _current = g.AddComponent<Loom>();
+        }
+    }
+
+    public static void QueueOnMainThread(Action action) {
+        QueueOnMainThread(action, 0f);
+    }
+
+    public static void QueueOnMainThread(Action action, float time) {
+        if (time != 0) {
+            lock (Current._delayed) {
+                Current._delayed.Add(new DelayedQueueItem { time = Time.time + time, action = action });
+            }
+        } else {
+            lock (Current._actions) {
+                Current._actions.Add(action);
+            }
+        }
+    }
+
+    public static Thread RunAsync(Action a) {
+        Initialize();
+        while (numThreads >= maxThreads) {
+            Thread.Sleep(1);
+        }
+        Interlocked.Increment(ref numThreads);
+        ThreadPool.QueueUserWorkItem(RunAction, a);
+        return null;
+    }
+
+    private static void RunAction(object action) {
+        try {
+            ((Action)action)();
+        } catch {
+        } finally {
+            Interlocked.Decrement(ref numThreads);
+        }
+
+    }
+
+    void OnDisable() {
+        if (_current == this) {
+
+            _current = null;
+        }
+    }
+
+    // Use this for initialization
+    void Start() {
+    }
+
+    // Update is called once per frame
+    void Update() {
+        lock (_actions) {
+            _currentActions.Clear();
+            _currentActions.AddRange(_actions);
+            _actions.Clear();
+        }
+        foreach (var a in _currentActions) {
+            a();
+        }
+        lock (_delayed) {
+            _currentDelayed.Clear();
+            _currentDelayed.AddRange(_delayed.Where(d => d.time <= Time.time));
+            foreach (var item in _currentDelayed)
+                _delayed.Remove(item);
+        }
+        foreach (var delayed in _currentDelayed) {
+            delayed.action();
+        }
+    }
+}

pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/OculusHaptics.cs

@@ -0,0 +1,124 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public enum VibrationForce
+{
+    Light,
+    Medium,
+    Hard,
+}
+
+
+public class OculusHaptics : MonoBehaviour
+{
+    OVRInput.Controller controllerMask;
+
+    private OVRHapticsClip clipLight;
+    private OVRHapticsClip clipMedium;
+    private OVRHapticsClip clipHard;
+
+    public float lowViveHaptics { get; private set; }
+    public float mediumViveHaptics { get; private set; }
+    public float hardViveHaptics { get; private set; }
+
+    private byte[] noize;
+
+
+    private void Start()
+    {
+        InitializeOVRHaptics();
+        //byte[] noize = { 250 };
+        //clipHard = new OVRHapticsClip(noize, 1);
+    }
+
+    private void InitializeOVRHaptics()
+    {
+
+        //int cnt = 10;
+        //clipLight = new OVRHapticsClip(cnt);
+        //clipMedium = new OVRHapticsClip(cnt);
+        ////clipHard = new OVRHapticsClip(cnt);
+        //for (int i = 0; i < cnt; i++)
+        //{
+        //    clipLight.Samples[i] = i % 2 == 0 ? (byte)0 : (byte)45;
+        //    clipMedium.Samples[i] = i % 2 == 0 ? (byte)0 : (byte)100;
+        //    //clipHard.Samples[i] = i % 2 == 0 ? (byte)0 : (byte)180;
+        //}
+
+        //clipLight = new OVRHapticsClip(clipLight.Samples, clipLight.Samples.Length);
+        //clipMedium = new OVRHapticsClip(clipMedium.Samples, clipMedium.Samples.Length);
+        //clipHard = new OVRHapticsClip(clipHard.Samples, clipHard.Samples.Length);
+    }
+
+
+    void OnEnable()
+    {
+        InitializeOVRHaptics();
+    }
+
+    public void Vibrate(VibrationForce vibrationForce)
+    {
+        //var channel = OVRHaptics.RightChannel;
+        //if (controllerMask == OVRInput.Controller.LTouch)
+        //    channel = OVRHaptics.LeftChannel;
+
+        //channel = OVRHaptics.Channels[1];
+
+        //switch (vibrationForce)
+        //{
+        //    case VibrationForce.Light:
+        //        channel.Preempt(clipLight);
+        //        break;
+        //    case VibrationForce.Medium:
+        //        channel.Preempt(clipMedium);
+        //        break;
+        //    case VibrationForce.Hard:
+        //        channel.Preempt(clipHard);
+        //        break;
+        //}
+
+
+        OVRHaptics.Channels[0].Clear();
+        OVRHaptics.Channels[1].Clear();
+
+        if (vibrationForce == VibrationForce.Light)
+            noize = new byte[10] { 60, 60, 60, 60, 60, 60, 60, 60, 60, 60 };
+        else if (vibrationForce == VibrationForce.Medium)
+            noize = new byte[10] { 120, 120, 120 , 120, 120, 120, 120, 120, 120, 120 };
+        else
+            noize = new byte[10] { 240, 240, 240, 240, 240, 240, 240, 240, 240, 240 };
+
+        OVRHaptics.Channels[1].Preempt(new OVRHapticsClip(noize, 10));
+        OVRHaptics.Process();
+    }
+
+    public IEnumerator VibrateTime(VibrationForce force, float time)
+    {
+        //bool forcedHaptic = true;
+        var channel = OVRHaptics.RightChannel;
+        if (controllerMask == OVRInput.Controller.LTouch)
+            channel = OVRHaptics.LeftChannel;
+
+        for (float t = 0; t <= time; t += Time.deltaTime)
+        {
+            switch (force)
+            {
+                case VibrationForce.Light:
+                    channel.Queue(clipLight);
+                    break;
+                case VibrationForce.Medium:
+                    channel.Queue(clipMedium);
+                    break;
+                case VibrationForce.Hard:
+                    channel.Queue(clipHard);
+                    break;
+            }
+        }
+        yield return new WaitForSeconds(time);
+        channel.Clear();
+        //forcedHaptic = false;
+        yield return null;
+
+    }
+}

pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/PlayMovieTextureOnUI.cs

@@ -0,0 +1,43 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.UI;
+
+public class PlayMovieTextureOnUI : MonoBehaviour {
+
+    public RawImage rawimage;
+    public bool createWebcam;
+    Texture2D texture;
+    TCPImage tcpImage;
+
+    // Use this for initialization
+    void Start () {
+        createWebcam = false;
+        if (createWebcam)
+        {
+            Debug.Log("Create Webcam");
+            WebCamTexture webcamTexture = new WebCamTexture();
+            //rawimage.texture = webcamTexture;
+            rawimage.material.mainTexture = webcamTexture;
+            Debug.Log("Play Webcam");
+            webcamTexture.Play();
+            Debug.Log("Play Webcam");
+        }
+
+        //rawimage.material.mainTexture = texture;
+
+        String host = "10.255.24.97"; //"10.255.24.140";
+        Int32 port = 5112;
+        tcpImage = new TCPImage();
+        //tcpImage.setupSocket(Host, Port);
+        //tcpImage.setTexture(rawimage.material.mainTexture);
+        //tcpImage.setRawImage(rawimage);
+        tcpImage.init(host, port, rawimage);
+    }
+	
+	// Update is called once per frame
+	void Update () {
+        //tcpImage.readThread();
+    }
+}

pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TCP.cs

@@ -0,0 +1,71 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System.Collections;
+using System;
+using System.IO;
+using System.Net.Sockets;
+
+
+public class TCP : MonoBehaviour {
+
+    internal Boolean socketReady = false;
+    TcpClient mySocket;
+    NetworkStream theStream;
+    StreamWriter theWriter;
+    StreamReader theReader;
+    //String Host = "localhost";
+    //Int32 Port = 5111;
+    void Start()
+    {
+    }
+    void Update()
+    {
+    }
+    // **********************************************
+    public void setupSocket(String Host, Int32 Port)
+    {
+        try
+        {
+            mySocket = new TcpClient(Host, Port);
+            theStream = mySocket.GetStream();
+            theWriter = new StreamWriter(theStream);
+            theReader = new StreamReader(theStream);
+            socketReady = true;
+        }
+        catch (Exception e)
+        {
+            Debug.Log("Socket error: " + e);
+        }
+    }
+    public void writeSocket(string theLine)
+    {
+
+        if (!socketReady)
+            return;
+        //Debug.Log("socket:" + theLine);
+        String foo = theLine + "\r\n";
+        theWriter.Write(foo);
+        theWriter.Flush();
+    }
+    public String readSocket()
+    {
+        if (!socketReady)
+            return "";
+        //Debug.Log("reader");
+        if (theStream.DataAvailable)
+            return theReader.ReadLine();
+        //Debug.Log("reader2");
+        return "";
+    }
+    public void closeSocket()
+    {
+        if (!socketReady)
+            return;
+        theWriter.Close();
+        theReader.Close();
+        mySocket.Close();
+        socketReady = false;
+    }
+
+}

pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TCPImage.cs

@@ -0,0 +1,245 @@
+using System;
+using System.Net.Sockets;
+using System.Text;
+using System.Threading;
+using UnityEngine;
+using UnityEngine.UI;
+
+public class TCPImage : MonoBehaviour {
+    // mainly inspired by: https://stackoverflow.com/questions/42717713/unity-live-video-streaming
+
+    // std variables
+    public bool enableLog = false;
+    private bool stop = false;
+    //private bool displayImage = false;
+
+    // network variables
+    private TcpClient client;
+    private String host;
+    private Int32 port;
+    private bool socketReady = false;
+    NetworkStream stream;
+
+    // Image variables
+    // nb of bytes used to specify the length of the image (needs to be the same on the server side)
+    private const int IMAGE_LENGTH = 4; // 4 bytes are enough (=int32)
+    Texture2D imageTexture;
+    RawImage rawImage;
+
+    // Thread
+    Thread thread;
+    static readonly object lockObj = new object();
+    byte[] imageBytes;
+
+
+    // Use this for initialization
+    void Start () {
+	}
+	
+	// Update is called once per frame
+	void Update () {
+        // display image
+        //if (displayImage) {
+        //    bool loaded = false;
+        //    lock (lockObj) {
+        //        loaded = imageTexture.LoadImage(imageBytes);
+        //        log("Image loaded: " + loaded);
+        //    }
+        //    rawImage.texture = imageTexture;
+        //    displayImage = false;
+        //    if (loaded)
+        //        Debug.Log("Image loaded!");
+        //}
+	}
+
+    public void init(String host, Int32 port, RawImage image) {
+        // set variables
+        this.imageTexture = new Texture2D(0, 0);
+        this.rawImage = image;
+        this.host = host;
+        this.port = port;
+
+        // create new thread that will update rawimage with the new incoming images
+        //thread = new Thread(readThread);
+        //thread.IsBackground = true;
+        //thread.Start();
+        Loom.RunAsync(() => {
+            readThread();
+        });
+    }
+
+    public void readThread() {
+        // create client (connect to server)
+        Debug.Log("Setup TCP Image connection");
+        this.client = new TcpClient(this.host, this.port);
+
+        // thread main loop: read images from the network
+        while (!this.stop) {
+            // read image size
+            int dim = readImageSize(IMAGE_LENGTH);
+            log("Dimension of image to be received: " + dim);
+
+            // read image
+            if (dim > 0) {
+                readImage(dim);
+            } else {
+                logWarning("Lost Connection!!!");
+            }
+        }
+    }
+
+    /**Read the specified number of bytes from the network.
+     * param: bytes - this will be filled by what is received from the network.
+     */
+    private bool read(byte[] bytes) {
+        stream = this.client.GetStream();
+        int total = 0;
+        int size = bytes.Length;
+        while (total != size) {
+            int nbBytesRead = stream.Read(bytes, total, size - total);
+            if (nbBytesRead == 0) // disconnected
+                return false;
+            total += nbBytesRead;
+        }
+        return true;
+    }
+
+    /**Read the size of the image that will sent through the network.
+     */
+    private int readImageSize(int size) {
+        byte[] imageSizeBytes = new byte[size];
+        bool connected = read(imageSizeBytes);
+        if (!connected)
+            return -1;
+        int imageSize = BitConverter.ToInt32(imageSizeBytes, 0);
+        return imageSize;
+    }
+
+    /**Read the image from the network using the specified size.
+     */
+    //private bool readImage(int size) {
+     private void readImage(int size) {
+        // read the image from the network
+        //byte[] imageBytes = new byte[size];
+        bool connected = false;
+        //lock (lockObj) {
+            imageBytes = new byte[size];
+            connected = read(imageBytes);
+            log("image loaded in bytes!");
+        //displayImage = true;
+        //}
+
+        bool ready = false;
+
+        // displayImage
+        if (connected) {
+            Loom.QueueOnMainThread(() => {
+                displayImage(imageBytes);
+                ready = true;
+            });
+        }
+
+        // wait until old Image is displayed
+        while (!ready)
+            System.Threading.Thread.Sleep(1);
+
+        //return connected;
+    }
+
+    private void displayImage(byte[] imageBytes) {
+        bool loaded = imageTexture.LoadImage(imageBytes);
+        log("Image loaded: " + loaded);
+        this.rawImage.texture = imageTexture;
+    }
+
+    private void log(string msg) {
+        if (enableLog)
+            Debug.Log(msg);
+    }
+
+    private void logWarning(string msg) {
+        if (enableLog)
+            Debug.LogWarning(msg);
+    }
+
+    private void OnApplicationQuit() {
+        logWarning("Quitting application!");
+        this.stop = true;
+        if (this.client != null)
+            this.client.Close();
+    }
+
+
+
+
+
+
+    public void setupSocket(String Host, Int32 Port) {
+        imageTexture = new Texture2D(0, 0);
+        try {
+            Debug.Log("Setup TCP IMAGE connection");
+            client = new TcpClient(Host, Port);
+            //buffer = new byte[client.ReceiveBufferSize];
+            //memStream = new MemoryStream();
+            //stream = client.GetStream();
+            socketReady = true;
+        }
+        catch (Exception e)
+        {
+            Debug.Log("Socket error: " + e);
+        }
+        if (socketReady) {
+            
+        }
+    }
+
+    public void setRawImage(RawImage image) {
+        rawImage = image;
+    }
+
+    //public String readThread() {
+    //    if(socketReady) {
+    //        //while(true) { // keep reading
+    //        //Debug.Log("Collecting data...");
+    //        memStream = new MemoryStream();
+    //        while (true) { //(stream.DataAvailable) { // read an image
+    //            int len = stream.Read(buffer, 0, buffer.Length);
+    //            if (len <= 0)
+    //                break;
+    //            //Debug.Log(len);
+    //            //Debug.Log(stream.DataAvailable);
+    //            count += 1;
+    //            Debug.Log(count);
+    //            memStream.Write(buffer, 0, len);
+    //        }
+    //        count = 0;
+    //        //Debug.Log("Data collected!");
+    //        // memStream.Seek(0, SeekOrigin.Begin);
+    //        //StreamReader reader = new StreamReader(memStream);
+    //        //string str = reader.ReadToEnd(); //Encoding.ASCII.GetString(memStream.ToArray());
+    //        string str = Encoding.ASCII.GetString(memStream.ToArray());
+    //        //Debug.Log(str.Length);
+    //        //memStream.SetLength(0);
+    //        return str;
+    //    } else {
+    //        Debug.log("socket not ready");
+    //        return "";
+    //    }
+    //}
+
+    //public void readThread() {
+    //    if (socketReady) {
+    //        int dim = readImageSize(IMAGE_LENGTH);
+    //        Debug.Log("Dimension of image to be received: " + dim);
+    //        string str = "";
+    //        if (dim > 0) {
+    //            str = readImage(dim);
+    //            Debug.Log("Length of image received: " + str.Length);
+    //        } else
+    //            Debug.Log("Lost Connection!!!");
+    //    } else {
+    //        Debug.Log("socket not ready!");
+    //    }
+    //}
+
+}

pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TestOculus.cs

@@ -0,0 +1,165 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using System;
+
+public class TestOculus : MonoBehaviour {
+    TCP tcp = new TCP();
+    //UDP udp;
+    OVRCameraRig camera_rig;
+    OVRPlayerController player_controller;
+    OVRManager manager;
+    public bool enableLog = false;
+    public bool connectTCP = false;
+    //public bool connectUDP = false;
+    public AudioClip clip;
+    private List<string> dataList;
+    private int total;
+
+    OculusHaptics haptics;
+
+    //TCPImage tcpImage;
+
+    // Use this for initialization
+    void Start () {
+        connectTCP = true;
+        //connectUDP = false;
+        dataList = new List<string>();
+
+        log("->Start()");
+
+        String Host = "10.255.24.97"; //"10.255.24.140";
+        Int32 Port = 5113;
+        if (connectTCP)
+            tcp.setupSocket(Host, Port);
+
+        //if (connectUDP)
+        //{
+        //    Debug.Log("Setting UDP");
+        //    udp = new UDP();
+        //    udp.setupUDP(Port);
+        //    Debug.Log("UDP set");
+        //}
+
+        //tcpImage = new TCPImage();
+        //tcpImage.setupSocket(Host, Port);
+
+
+        camera_rig = GameObject.FindObjectOfType<OVRCameraRig>();
+        player_controller = GameObject.FindObjectOfType<OVRPlayerController>();
+        manager = GameObject.FindObjectOfType<OVRManager>();
+
+        log("Start()->");
+
+        // haptics
+        haptics = new OculusHaptics();
+    }
+	
+
+
+
+
+
+    // Update is called once per frame
+    void Update()
+    {
+        OVRInput.Update();
+        log("->Update()!");
+
+        Transform root_anchor = camera_rig.trackingSpace;
+        Transform centerEyeAnchor = camera_rig.centerEyeAnchor;
+        Transform leftHandAnchor = camera_rig.leftHandAnchor;
+        Transform rightHandAnchor = camera_rig.rightHandAnchor;
+
+
+        Vector3 eye_P_right_hand = rightHandAnchor.position; // - center_eye_anchor.position;
+
+        Quaternion rightHandQuat = rightHandAnchor.rotation;
+        Quaternion leftHandQuat = leftHandAnchor.rotation;
+        Quaternion headQuat = centerEyeAnchor.rotation;
+
+        string msg;
+
+        msg = string.Format("{0:N5}", -headQuat[2]) + ',' + string.Format("{0:N5}", headQuat[0]) + ',' + string.Format("{0:N5}", -headQuat[1]) + ',' + string.Format("{0:N5}", headQuat[3]);
+        //msg = string.Format("{0:N5}", eye_P_right_hand[2]) + ',' + string.Format("{0:N5}", -eye_P_right_hand[0]) + ',' + string.Format("{0:N5}", eye_P_right_hand[1]) + ',';
+        //msg += string.Format("{0:N5}", rightHandQuat[2]) + ',' + string.Format("{0:N5}", rightHandQuat[0]) + ',' + string.Format("{0:N5}", rightHandQuat[1]) + ',' + string.Format("{0:N5}", rightHandQuat[3]);
+        //msg += string.Format("{0:N5}", -right_hand_quat[2]) + ',' + string.Format("{0:N5}", right_hand_quat[3]) + ',' + string.Format("{0:N5}", right_hand_quat[1]) + ',' + string.Format("{0:N5}", right_hand_quat[0]);
+
+
+        log(msg);
+
+        if (connectTCP)
+        {
+            //Debug.Log("tcp");
+            //Debug.Log(msg);
+            tcp.writeSocket(msg);
+            //String str = tcp.readSocket();
+            //Debug.Log(str);
+            //Debug.Log(str.Length);
+            //if (str.Equals("end")) {
+            //    Debug.Log("End");
+            //    string data = string.Join("", dataList.ToArray());
+            //    dataList.Clear();
+            //    total = 0;
+            //    Debug.Log("Total length: ");
+            //    Debug.Log(data.Length);
+            //} else {
+            //    if (str.Length != 0) {
+            //        total += str.Length;
+            //        Debug.Log(total);
+            //        dataList.Add(str);
+            //    }
+            //}
+        }
+
+        //String s = tcpImage.readThread();
+        //if (s.Length != 0) {
+        //    Debug.Log("Length of image received: " + s.Length);
+        //    //Debug.Log("First letters: " + s.Substring(0, 20));
+        //}
+        //if (connectUDP) {
+        //    string data = udp.readSocket();
+        //    Debug.Log(data.Length);
+        //}
+
+        // haptics
+        // WARNING: To get it working, you have to wear the headset or activate its proximity sensor!!
+        if (OVRInput.Get(OVRInput.Button.One))
+        {
+
+            Debug.Log("Button A pressed!");
+            haptics.Vibrate(VibrationForce.Light);
+        }
+
+        if (OVRInput.Get(OVRInput.Button.Two))
+        {
+
+            Debug.Log("Button B pressed!");
+            haptics.Vibrate(VibrationForce.Medium);
+        }
+
+        if (OVRInput.Get(OVRInput.Button.Three))
+        {
+
+            Debug.Log("Button X pressed!");
+            haptics.Vibrate(VibrationForce.Hard);
+        }
+
+        log("  Update()->");
+    }
+
+    private void log(string msg) {
+        if (enableLog)
+            Debug.Log(msg);
+    }
+
+    private void logWarning(string msg) {
+        if (enableLog)
+            Debug.LogWarning(msg);
+    }
+
+
+
+
+}
+

pyrobolearn/tools/interfaces/vr/vr.py

@@ -0,0 +1,8 @@
+
+from pyrobolearn.tools.interfaces.interface import InputOutputInterface
+
+
+class VRInterface(InputOutputInterface):
+    r"""Virtual Reality Interface
+    """
+    pass

setup.py

@@ -25,7 +25,7 @@ setup(
     author_email='briandelhaisse@gmail.com',
     maintainer='Brian Delhaisse',
     maintainer_email='Brian Delhaisse',
-    license='(c) Brian Delhaisse',
+    license='MIT',
     url='https://github.com/robotlearn/pyrobolearn',
     platforms=['Linux Ubuntu'],
     python_requires='2.7'