diff --git a/pyrobolearn/algos/__init__.py b/pyrobolearn/algos/__init__.py new file mode 100644 index 0000000..6dd7eb0 --- /dev/null +++ b/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 diff --git a/pyrobolearn/algos/bo.py b/pyrobolearn/algos/bo.py new file mode 100644 index 0000000..2e01faa --- /dev/null +++ b/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 diff --git a/pyrobolearn/algos/cem.py b/pyrobolearn/algos/cem.py new file mode 100755 index 0000000..8648f47 --- /dev/null +++ b/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 diff --git a/pyrobolearn/algos/cmaes.py b/pyrobolearn/algos/cmaes.py new file mode 100755 index 0000000..ca14cd0 --- /dev/null +++ b/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) diff --git a/pyrobolearn/algos/fd.py b/pyrobolearn/algos/fd.py new file mode 100755 index 0000000..4992bfb --- /dev/null +++ b/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) diff --git a/pyrobolearn/algos/neat_algo.py b/pyrobolearn/algos/neat_algo.py new file mode 100755 index 0000000..0688811 --- /dev/null +++ b/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) diff --git a/pyrobolearn/algos/power.py b/pyrobolearn/algos/power.py new file mode 100755 index 0000000..0aba55e --- /dev/null +++ b/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) diff --git a/pyrobolearn/models/nn/ae.py b/pyrobolearn/models/nn/ae.py index 55fd34c..55cbfc5 100644 --- a/pyrobolearn/models/nn/ae.py +++ b/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" diff --git a/pyrobolearn/models/nn/cnn.py b/pyrobolearn/models/nn/cnn.py index e7bb442..12d264e 100644 --- a/pyrobolearn/models/nn/cnn.py +++ b/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" diff --git a/pyrobolearn/models/nn/dnn.py b/pyrobolearn/models/nn/dnn.py index e3e7c2f..7f25b67 100644 --- a/pyrobolearn/models/nn/dnn.py +++ b/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" diff --git a/pyrobolearn/models/nn/gan.py b/pyrobolearn/models/nn/gan.py index c3aeee6..b33f64a 100644 --- a/pyrobolearn/models/nn/gan.py +++ b/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" diff --git a/pyrobolearn/models/nn/mlp.py b/pyrobolearn/models/nn/mlp.py index 439e86a..fdea93d 100644 --- a/pyrobolearn/models/nn/mlp.py +++ b/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" diff --git a/pyrobolearn/models/nn/neat_model.py b/pyrobolearn/models/nn/neat_model.py index 6fd4435..5959cd4 100644 --- a/pyrobolearn/models/nn/neat_model.py +++ b/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" diff --git a/pyrobolearn/models/nn/rcnn.py b/pyrobolearn/models/nn/rcnn.py index da54863..ab2914d 100644 --- a/pyrobolearn/models/nn/rcnn.py +++ b/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" diff --git a/pyrobolearn/models/nn/rnn.py b/pyrobolearn/models/nn/rnn.py index e73e339..f65da33 100644 --- a/pyrobolearn/models/nn/rnn.py +++ b/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" diff --git a/pyrobolearn/models/nn/vae.py b/pyrobolearn/models/nn/vae.py index f9bde3c..0f14261 100644 --- a/pyrobolearn/models/nn/vae.py +++ b/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" diff --git a/pyrobolearn/robots/actuators/actuator.py b/pyrobolearn/robots/actuators/actuator.py index 4f6406f..3576d64 100644 --- a/pyrobolearn/robots/actuators/actuator.py +++ b/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" diff --git a/pyrobolearn/robots/actuators/joints.py b/pyrobolearn/robots/actuators/joints.py index 18844d4..b47646f 100644 --- a/pyrobolearn/robots/actuators/joints.py +++ b/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" diff --git a/pyrobolearn/robots/base.py b/pyrobolearn/robots/base.py index 3ecc77a..c2b1067 100644 --- a/pyrobolearn/robots/base.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/camera.py b/pyrobolearn/robots/sensors/camera.py index 820705e..dfeb10f 100644 --- a/pyrobolearn/robots/sensors/camera.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/contact.py b/pyrobolearn/robots/sensors/contact.py index bb8ff4f..456ec89 100644 --- a/pyrobolearn/robots/sensors/contact.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/force_torque.py b/pyrobolearn/robots/sensors/force_torque.py index bed13ca..fdf80e4 100644 --- a/pyrobolearn/robots/sensors/force_torque.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/imu.py b/pyrobolearn/robots/sensors/imu.py index 8fa6826..f91aed2 100644 --- a/pyrobolearn/robots/sensors/imu.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/joints.py b/pyrobolearn/robots/sensors/joints.py index 8c9dd02..24e1ae5 100644 --- a/pyrobolearn/robots/sensors/joints.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/light.py b/pyrobolearn/robots/sensors/light.py index 58dcae8..de2f008 100644 --- a/pyrobolearn/robots/sensors/light.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/links.py b/pyrobolearn/robots/sensors/links.py index f8a5480..2bb5b42 100644 --- a/pyrobolearn/robots/sensors/links.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/misc.py b/pyrobolearn/robots/sensors/misc.py index 84fa7f7..77ea626 100644 --- a/pyrobolearn/robots/sensors/misc.py +++ b/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" diff --git a/pyrobolearn/robots/sensors/sensor.py b/pyrobolearn/robots/sensors/sensor.py index d16b1fc..88b434d 100644 --- a/pyrobolearn/robots/sensors/sensor.py +++ b/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" diff --git a/pyrobolearn/tools/README.md b/pyrobolearn/tools/README.md new file mode 100644 index 0000000..bc022ff --- /dev/null +++ b/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. \ No newline at end of file diff --git a/pyrobolearn/tools/__init__.py b/pyrobolearn/tools/__init__.py new file mode 100644 index 0000000..bbde068 --- /dev/null +++ b/pyrobolearn/tools/__init__.py @@ -0,0 +1,6 @@ + +# import interfaces +# import interfaces + +# import bridges +# import bridges diff --git a/pyrobolearn/tools/bridges/__init__.py b/pyrobolearn/tools/bridges/__init__.py new file mode 100644 index 0000000..b1ed603 --- /dev/null +++ b/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 * diff --git a/pyrobolearn/tools/bridges/audio/__init__.py b/pyrobolearn/tools/bridges/audio/__init__.py new file mode 100644 index 0000000..6ff5ddf --- /dev/null +++ b/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 * \ No newline at end of file diff --git a/pyrobolearn/tools/bridges/audio/robots/__init__.py b/pyrobolearn/tools/bridges/audio/robots/__init__.py new file mode 100644 index 0000000..33f9f0d --- /dev/null +++ b/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 * \ No newline at end of file diff --git a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_rotatory_uav.py b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_rotatory_uav.py new file mode 100644 index 0000000..bcad30e --- /dev/null +++ b/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)) diff --git a/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py b/pyrobolearn/tools/bridges/audio/robots/bridge_speech_wheeled.py new file mode 100644 index 0000000..3049dd7 --- /dev/null +++ b/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)) \ No newline at end of file diff --git a/pyrobolearn/tools/bridges/audio/world/__init__.py b/pyrobolearn/tools/bridges/audio/world/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/bridges/bridge.py b/pyrobolearn/tools/bridges/bridge.py new file mode 100644 index 0000000..c8c1886 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/bridges/camera/__init__.py b/pyrobolearn/tools/bridges/camera/__init__.py new file mode 100644 index 0000000..9082627 --- /dev/null +++ b/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 * \ No newline at end of file diff --git a/pyrobolearn/tools/bridges/camera/robots/__init__.py b/pyrobolearn/tools/bridges/camera/robots/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/bridges/camera/world/__init__.py b/pyrobolearn/tools/bridges/camera/world/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/bridges/controllers/__init__.py b/pyrobolearn/tools/bridges/controllers/__init__.py new file mode 100644 index 0000000..e7dae3b --- /dev/null +++ b/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 * \ No newline at end of file diff --git a/pyrobolearn/tools/bridges/controllers/robots/__init__.py b/pyrobolearn/tools/bridges/controllers/robots/__init__.py new file mode 100644 index 0000000..771f249 --- /dev/null +++ b/pyrobolearn/tools/bridges/controllers/robots/__init__.py @@ -0,0 +1,3 @@ + +# bridge between controller and wheeled robot +from bridge_controller_wheeled import * diff --git a/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py b/pyrobolearn/tools/bridges/controllers/robots/bridge_controller_wheeled.py new file mode 100644 index 0000000..826ad8f --- /dev/null +++ b/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. diff --git a/pyrobolearn/tools/bridges/controllers/world/__init__.py b/pyrobolearn/tools/bridges/controllers/world/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/__init__.py b/pyrobolearn/tools/bridges/mouse_keyboard/__init__.py new file mode 100644 index 0000000..475444b --- /dev/null +++ b/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 diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_imitation_task.py new file mode 100644 index 0000000..730cee3 --- /dev/null +++ b/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) + * ``: select and perform corresponding action (10 actions possible, as 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 = [] diff --git a/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py b/pyrobolearn/tools/bridges/mouse_keyboard/bridge_mousekeyboard_world.py new file mode 100644 index 0000000..1721263 --- /dev/null +++ b/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) + * ``: select and perform corresponding action (10 actions possible, as 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) diff --git a/pyrobolearn/tools/bridges/vr/__init__.py b/pyrobolearn/tools/bridges/vr/__init__.py new file mode 100644 index 0000000..0040712 --- /dev/null +++ b/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 * \ No newline at end of file diff --git a/pyrobolearn/tools/bridges/vr/robots/__init__.py b/pyrobolearn/tools/bridges/vr/robots/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/bridges/vr/world/__init__.py b/pyrobolearn/tools/bridges/vr/world/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/interfaces/__init__.py b/pyrobolearn/tools/interfaces/__init__.py new file mode 100644 index 0000000..15fb30f --- /dev/null +++ b/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 * diff --git a/pyrobolearn/tools/interfaces/audio/__init__.py b/pyrobolearn/tools/interfaces/audio/__init__.py new file mode 100644 index 0000000..474549f --- /dev/null +++ b/pyrobolearn/tools/interfaces/audio/__init__.py @@ -0,0 +1,3 @@ + +# import audio interfaces +from audio import * diff --git a/pyrobolearn/tools/interfaces/audio/audio.py b/pyrobolearn/tools/interfaces/audio/audio.py new file mode 100644 index 0000000..61c0f7a --- /dev/null +++ b/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)) diff --git a/pyrobolearn/tools/interfaces/audio/microphone.py b/pyrobolearn/tools/interfaces/audio/microphone.py new file mode 100644 index 0000000..ab57454 --- /dev/null +++ b/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 diff --git a/pyrobolearn/tools/interfaces/audio/speaker.py b/pyrobolearn/tools/interfaces/audio/speaker.py new file mode 100644 index 0000000..e0a60e3 --- /dev/null +++ b/pyrobolearn/tools/interfaces/audio/speaker.py @@ -0,0 +1,8 @@ + +from audio import OutputAudioInterface + + +class SpeakerInterface(OutputAudioInterface): + r"""Speaker Interface + """ + pass diff --git a/pyrobolearn/tools/interfaces/bci/__init__.py b/pyrobolearn/tools/interfaces/bci/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/tools/interfaces/bci/bci.py b/pyrobolearn/tools/interfaces/bci/bci.py new file mode 100644 index 0000000..c7e81a8 --- /dev/null +++ b/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 diff --git a/pyrobolearn/tools/interfaces/camera/__init__.py b/pyrobolearn/tools/interfaces/camera/__init__.py new file mode 100644 index 0000000..478f9d1 --- /dev/null +++ b/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 diff --git a/pyrobolearn/tools/interfaces/camera/asus_xtion.py b/pyrobolearn/tools/interfaces/camera/asus_xtion.py new file mode 100644 index 0000000..21139f0 --- /dev/null +++ b/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=' + '\nexport OPENNI2_REDIST64=') + +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() diff --git a/pyrobolearn/tools/interfaces/camera/camera.py b/pyrobolearn/tools/interfaces/camera/camera.py new file mode 100644 index 0000000..da03e9e --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/camera/description.md b/pyrobolearn/tools/interfaces/camera/description.md new file mode 100644 index 0000000..17ac345 --- /dev/null +++ b/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. diff --git a/pyrobolearn/tools/interfaces/camera/fer.py b/pyrobolearn/tools/interfaces/camera/fer.py new file mode 100644 index 0000000..5d5d19b --- /dev/null +++ b/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__() diff --git a/pyrobolearn/tools/interfaces/camera/kinect.py b/pyrobolearn/tools/interfaces/camera/kinect.py new file mode 100644 index 0000000..8f7f3f0 --- /dev/null +++ b/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=' + '\nexport OPENNI2_REDIST64=') + 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 diff --git a/pyrobolearn/tools/interfaces/camera/openpose.py b/pyrobolearn/tools/interfaces/camera/openpose.py new file mode 100644 index 0000000..a1af618 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/camera/webcam.py b/pyrobolearn/tools/interfaces/camera/webcam.py new file mode 100644 index 0000000..64cadb5 --- /dev/null +++ b/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() diff --git a/pyrobolearn/tools/interfaces/controllers/__init__.py b/pyrobolearn/tools/interfaces/controllers/__init__.py new file mode 100644 index 0000000..822d35e --- /dev/null +++ b/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 * diff --git a/pyrobolearn/tools/interfaces/controllers/controller.py b/pyrobolearn/tools/interfaces/controllers/controller.py new file mode 100644 index 0000000..fb7ec61 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/controllers/playstation.py b/pyrobolearn/tools/interfaces/controllers/playstation.py new file mode 100755 index 0000000..8b45f87 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/controllers/xbox.py b/pyrobolearn/tools/interfaces/controllers/xbox.py new file mode 100644 index 0000000..e791bac --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/interface.py b/pyrobolearn/tools/interfaces/interface.py new file mode 100644 index 0000000..f2d53db --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/mouse_keyboard/__init__.py b/pyrobolearn/tools/interfaces/mouse_keyboard/__init__.py new file mode 100644 index 0000000..b90e724 --- /dev/null +++ b/pyrobolearn/tools/interfaces/mouse_keyboard/__init__.py @@ -0,0 +1,3 @@ + +# import mouse keyboard interface +from mousekeyboard import MouseKeyboardInterface diff --git a/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py b/pyrobolearn/tools/interfaces/mouse_keyboard/mousekeyboard.py new file mode 100644 index 0000000..16c74d9 --- /dev/null +++ b/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) diff --git a/pyrobolearn/tools/interfaces/robots/__init__.py b/pyrobolearn/tools/interfaces/robots/__init__.py new file mode 100644 index 0000000..33db34b --- /dev/null +++ b/pyrobolearn/tools/interfaces/robots/__init__.py @@ -0,0 +1,3 @@ + +# General import +from robot import RobotInterface diff --git a/pyrobolearn/tools/interfaces/robots/robot.py b/pyrobolearn/tools/interfaces/robots/robot.py new file mode 100644 index 0000000..d317169 --- /dev/null +++ b/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 diff --git a/pyrobolearn/tools/interfaces/sensors/__init__.py b/pyrobolearn/tools/interfaces/sensors/__init__.py new file mode 100644 index 0000000..9599ab0 --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/__init__.py @@ -0,0 +1,7 @@ + + +# general imports +from sensor import * + +# EMG sensor +from emg import EMGInterface diff --git a/pyrobolearn/tools/interfaces/sensors/emg.py b/pyrobolearn/tools/interfaces/sensors/emg.py new file mode 100644 index 0000000..9c92d3f --- /dev/null +++ b/pyrobolearn/tools/interfaces/sensors/emg.py @@ -0,0 +1,9 @@ + +from sensor import BioSensorInterface + + +class EMGInterface(BioSensorInterface): + r"""EMG sensor interface + + """ + pass diff --git a/pyrobolearn/tools/interfaces/sensors/sensor.py b/pyrobolearn/tools/interfaces/sensors/sensor.py new file mode 100644 index 0000000..b4a46d4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/pyrobolearn/tools/interfaces/suits/__init__.py b/pyrobolearn/tools/interfaces/suits/__init__.py new file mode 100644 index 0000000..82edbeb --- /dev/null +++ b/pyrobolearn/tools/interfaces/suits/__init__.py @@ -0,0 +1,6 @@ + +# general import +from suit import SuitInterface + +# Xsens suit +from xsens import XsensSuitInterface diff --git a/pyrobolearn/tools/interfaces/suits/suit.py b/pyrobolearn/tools/interfaces/suits/suit.py new file mode 100644 index 0000000..f13fe1e --- /dev/null +++ b/pyrobolearn/tools/interfaces/suits/suit.py @@ -0,0 +1,9 @@ + +from pyrobolearn.tools.interfaces import InputInterface + + +class SuitInterface(InputInterface): + r"""Suit Interface + + """ + pass diff --git a/pyrobolearn/tools/interfaces/suits/xsens.py b/pyrobolearn/tools/interfaces/suits/xsens.py new file mode 100644 index 0000000..6a00171 --- /dev/null +++ b/pyrobolearn/tools/interfaces/suits/xsens.py @@ -0,0 +1,9 @@ + +from suit import SuitInterface + + +class XsensSuitInterface(SuitInterface): + r"""Xsens suit interface + + """ + pass diff --git a/pyrobolearn/tools/interfaces/vr/__init__.py b/pyrobolearn/tools/interfaces/vr/__init__.py new file mode 100644 index 0000000..5b72fc8 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/pyrobolearn/tools/interfaces/vr/htc.py b/pyrobolearn/tools/interfaces/vr/htc.py new file mode 100644 index 0000000..da91543 --- /dev/null +++ b/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__() \ No newline at end of file diff --git a/pyrobolearn/tools/interfaces/vr/oculus.py b/pyrobolearn/tools/interfaces/vr/oculus.py new file mode 100644 index 0000000..573ef72 --- /dev/null +++ b/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(' _actions = new List(); + private List _delayed = new List(); + List _currentDelayed = new List(); + List _currentActions = new List(); + + 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(); + } + } + + 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(); + } + } +} diff --git a/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/OculusHaptics.cs b/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/OculusHaptics.cs new file mode 100755 index 0000000..d87b87b --- /dev/null +++ b/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; + + } +} diff --git a/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/PlayMovieTextureOnUI.cs b/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/PlayMovieTextureOnUI.cs new file mode 100755 index 0000000..853f3e8 --- /dev/null +++ b/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(); + } +} diff --git a/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TCP.cs b/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TCP.cs new file mode 100755 index 0000000..40e853d --- /dev/null +++ b/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; + } + +} diff --git a/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TCPImage.cs b/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TCPImage.cs new file mode 100755 index 0000000..2f067e3 --- /dev/null +++ b/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!"); + // } + //} + +} diff --git a/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TestOculus.cs b/pyrobolearn/tools/interfaces/vr/oculus_windows/unity-scripts/TestOculus.cs new file mode 100755 index 0000000..55287a4 --- /dev/null +++ b/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 dataList; + private int total; + + OculusHaptics haptics; + + //TCPImage tcpImage; + + // Use this for initialization + void Start () { + connectTCP = true; + //connectUDP = false; + dataList = new List(); + + 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(); + player_controller = GameObject.FindObjectOfType(); + manager = GameObject.FindObjectOfType(); + + 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); + } + + + + +} + diff --git a/pyrobolearn/tools/interfaces/vr/vr.py b/pyrobolearn/tools/interfaces/vr/vr.py new file mode 100644 index 0000000..9fbf83b --- /dev/null +++ b/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 diff --git a/setup.py b/setup.py index 63610d5..e22117f 100755 --- a/setup.py +++ b/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'