diff --git a/examples/README.md b/examples/README.md index b9869dc..edada3d 100644 --- a/examples/README.md +++ b/examples/README.md @@ -5,3 +5,5 @@ In this folder, you will find different examples on how to use the framework. You can check the following folders: - `gym/cartpole`: policies are trained with different algorithms on the gym Cartpole environment. - `robots`: check how to load a specific robot into the world. +- `states`: how to query the states / observations. + diff --git a/pyrobolearn/approximators/__init__.py b/pyrobolearn/approximators/__init__.py index 83c8ec5..b1675d4 100644 --- a/pyrobolearn/approximators/__init__.py +++ b/pyrobolearn/approximators/__init__.py @@ -1,3 +1,13 @@ # import function approximators from .approximator import * + +# import basic function approximators (random, linear, polynomial) +from .basic_approximator import * + +# import nn function approximators +from .nn_approximator import * + +# import gp function approximators +# from .gp_approximator import * + diff --git a/pyrobolearn/approximators/approximator.py b/pyrobolearn/approximators/approximator.py index a3915ce..1720b6c 100644 --- a/pyrobolearn/approximators/approximator.py +++ b/pyrobolearn/approximators/approximator.py @@ -10,17 +10,15 @@ Dependencies: - `pyrobolearn.actions` """ -from abc import ABCMeta, abstractmethod import collections import numpy as np import torch from pyrobolearn.states import State from pyrobolearn.actions import Action - from pyrobolearn.models import Model -from pyrobolearn.models.linear import Linear -from pyrobolearn.models.nn import NN, MLP, NEATModel +from pyrobolearn.processors import Processor + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -36,14 +34,14 @@ class Approximator(object): r"""Function Approximator (abstract) class The function approximator is a wrapper around the inner learning base model, and connects a learning model to - its state/action inputs and outputs. That is, this class described how to connect a learning model with - states/actions, and thus allows the inner models to be independent from the notions of states and actions. + its state / action inputs and outputs. That is, this class described how to connect a learning model with + states / actions, and thus allows the inner models to be independent from the notions of states and actions. This class and all the children inheriting from this one will be used internally by several classes such as policies, dynamic models, value estimators, and others. This enables to not duplicate and write the same code - for these various different concepts which share similar features. - For instance, policies are function approximators where the inputs are states, and the outputs are actions. - Dynamic models are extended models where the inputs are states and actions, and the outputs are the next state. + for these various different concepts which share similar features. For instance, policies are function + approximators where the inputs are states, and the outputs are actions while dynamic transition models are + approximators where the inputs are states and actions, and the outputs are the next state. Often the learning model can be constructed automatically by knowing the input and output dimensions, along with few other optional parameters. This is useful if one does not wish to change the learning model but just to scale @@ -53,27 +51,22 @@ class Approximator(object): * Policy: approximator mapping states to actions * Value estimator: approximator mapping states (or states and actions) to a value, or mapping states to actions (in the case of discrete actions) - * Actor-Critic: + * Actor-Critic: combination of policy and value function approximators * Dynamic: approximator mapping states and actions to the next states * Transformation mappings: approximator mapping states to states - - Example:: - - states = JntPosState(robot) + JntVelState(robot) - actions = JntPosAction(robot) - model = ... """ def __init__(self, inputs, outputs, model=None, preprocessors=None, postprocessors=None): r"""Initialize the outer model. Args: - inputs (State, Action, array): inputs of the inner models (instance of State/Action) - outputs (State, Action, array): outputs of the inner models (instance of Action/State) + inputs (State, Action, np.array, torch.Tensor): inputs of the inner models (instance of State/Action) + outputs (State, Action, np.array, torch.Tensor): outputs of the inner models (instance of Action/State) model (Model, None): inner model which will be wrapped if not an instance of Model - preprocessors (None, Processor): the inputs are first given to the preprocessors then to the model. - postprocessors (None, Processor): the predicted outputs by the model are given to the processors before - being returned. + preprocessors (None, Processor, list of Processor): the inputs are first given to the preprocessors then + to the model. + postprocessors (None, Processor, list of Processor): the predicted outputs by the model are given to the + processors before being returned. """ # Check inputs and outputs, and convert to the correct format @@ -81,17 +74,8 @@ class Approximator(object): self.inputs = inputs self.outputs = outputs - # preprocessors and postprocessors - if preprocessors is None: - preprocessors = [] - if not isinstance(preprocessors, collections.Iterable): - preprocessors = [preprocessors] + # set pre- and post- processors self.preprocessors = preprocessors - - if postprocessors is None: - postprocessors = [] - if not isinstance(postprocessors, collections.Iterable): - postprocessors = [postprocessors] self.postprocessors = postprocessors # Check the given model: check if correct input/output sizes wrt the previous arguments, and check @@ -105,10 +89,12 @@ class Approximator(object): @property def inputs(self): + """Return the approximator's inputs.""" return self._inputs @inputs.setter def inputs(self, inputs): + """Set the approximator's inputs.""" if inputs is not None: if isinstance(inputs, (int, float)): inputs = np.array([inputs]) @@ -121,10 +107,12 @@ class Approximator(object): @property def outputs(self): + """Return the approximator's outputs.""" return self._outputs @outputs.setter def outputs(self, outputs): + """Set the approximator's outputs.""" if outputs is not None: if isinstance(outputs, (int, float)): outputs = np.array([outputs]) @@ -137,10 +125,12 @@ class Approximator(object): @property def model(self): + """Return the inner learning model.""" return self._model @model.setter def model(self, model): + """Set the inner learning model.""" if model is not None: # check model type # if not isinstance(model, Model): @@ -160,11 +150,88 @@ class Approximator(object): # set model self._model = model + @property + def preprocessors(self): + """Return the list of pre-processors.""" + return self._preprocessors + + @preprocessors.setter + def preprocessors(self, processors): + """Set the list of pre-processors.""" + if processors is None: + processors = [] + elif callable(processors): + processors = [processors] + elif isinstance(processors, collections.Iterable): + for idx, processor in enumerate(processors): + if not callable(processor): + raise ValueError("The {} processor {} is not callable.".format(idx, processor)) + else: + raise TypeError("Expecting the processors to be None, a callable class / function such as `Processor`, " + "or a list of them. Instead got: {}".format(type(processors))) + self._preprocessors = processors + + @property + def postprocessors(self): + """Return the list of post-processors.""" + return self._postprocessors + + @postprocessors.setter + def postprocessors(self, processors): + """Set the list of post-processors.""" + if processors is None: + processors = [] + elif callable(processors): + processors = [processors] + elif isinstance(processors, collections.Iterable): + for idx, processor in enumerate(processors): + if not callable(processor): + raise ValueError("The {} processor {} is not callable.".format(idx, processor)) + else: + raise TypeError("Expecting the processors to be None, a callable class / function such as `Processor`, " + "or a list of them. Instead got: {}".format(type(processors))) + self._postprocessors = processors + + @property + def input_size(self): + """Return the approximator input size.""" + return self.model.input_size + + @property + def output_size(self): + """Return the approximator output size.""" + return self.model.output_size + + @property + def input_shape(self): + """Return the approximator input shape.""" + return self.model.input_shape + + @property + def output_shape(self): + """Return the approximator output shape.""" + return self.model.output_shape + + @property + def input_dim(self): + """Return the input dimension.""" + return self.model.input_dim + + @property + def output_dim(self): + """Return the output dimension.""" + return self.model.output_dim + @property def num_parameters(self): - """Return the total number of parameters""" + """Return the total number of parameters of the inner learning model.""" return self.model.num_parameters + @property + def num_hyperparameters(self): + """Return the total number of hyper-parameters of the inner learning model.""" + return self.model.num_hyperparameters + ########### # Methods # ########### @@ -239,51 +306,113 @@ class Approximator(object): """ return self.model.is_generative() - def reset(self): - """Reset the approximator.""" - for processor in self.preprocessors: - processor.reset() - for processor in self.postprocessors: - processor.reset() - self.model.reset() - - def predict(self, x, to_numpy=True): - for processor in self.preprocessors: - x = processor(x) - x = self.model(x.data[0]) - for processor in self.postprocessors: - x = processor(x) - return x + def _size(self, x): + """Return the total size of a `State`, `Action`, numpy.array, or torch.Tensor.""" + size = 0 + if isinstance(x, (State, Action)): + if x.is_discrete(): + size = x.space[0].n + else: + size = x.total_size() + elif isinstance(x, np.ndarray): + size = x.size + elif isinstance(x, torch.Tensor): + size = x.numel() + elif isinstance(x, int): + size = x + return size def parameters(self): - """Return the approximator parameters.""" + """Return an iterator over the approximator parameters.""" return self.model.parameters() - def get_params(self): + def named_parameters(self): + """Return an iterator over the approximator parameters, yielding both the name and the parameter itself.""" + return self.model.named_parameters() + + def list_parameters(self): """Return the list of parameters.""" return list(self.parameters()) def hyperparameters(self): - """Return the approximator hyper-parameters.""" + """Return an iterator over the approximator hyper-parameters.""" return self.model.hyperparameters() - def get_hyperparams(self): + def named_hyperparameters(self): + """Return an iterator over the approximator hyper-parameters, yielding both the name and the hyper-parameter + itself.""" + return self.model.named_hyperparameters() + + def list_hyperparameters(self): """Return the list of hyper-parameters.""" return list(self.hyperparameters()) def get_vectorized_parameters(self, to_numpy=True): + """Return a vectorized form of the parameters""" return self.model.get_vectorized_parameters(to_numpy=to_numpy) def set_vectorized_parameters(self, vector): + """Set the vector parameters.""" self.model.set_vectorized_parameters(vector=vector) - def get_input_dims(self): - """Return the input dimensions.""" - return self.model.input_dims + def reset(self, reset_processors=False): + """Reset the approximators.""" + if reset_processors: + for processor in self.preprocessors: + if isinstance(processor, Processor): + processor.reset() + for processor in self.postprocessors: + if isinstance(processor, Processor): + processor.reset() + self.model.reset() - def get_output_dims(self): - """Return the output dimensions.""" - return self.model.output_dims + def predict(self, x=None, to_numpy=True, return_logits=False): + """Predict the output given the input.""" + # if no input is given, take the provided inputs at the beginning + if x is None: + x = self.inputs + + # if the input is an instance of State or Action, get the inner merged data. + if isinstance(x, (State, Action)): + x = x.merged_data + if len(x) == 1: + x = x[0] + + # go through each preprocessor + for processor in self.preprocessors: + x = processor(x) + + # go through the model + x = self.model.predict(x, to_numpy=False) + + # go through each postprocessor + for processor in self.postprocessors: + x = processor(x) + + # set the output data + if isinstance(self.outputs, (State, Action)): # TODO: think when multiple outputs and to set them + if self.outputs.is_discrete() and not return_logits: + if isinstance(x, np.ndarray): + x = np.array([np.argmax(x)]) + elif isinstance(x, torch.Tensor): + x = torch.argmax(x, dim=0, keepdim=True) + else: + raise TypeError("Expecting `x` to be a numpy array, torch.Tensor, or a list of them, instead got: " + "{}".format(type(x))) + + # set the data + if isinstance(x, np.ndarray): + self.outputs.data = x + else: # isinstance(x, torch.Tensor): + self.outputs.torch_data = x + + # return the data + # convert to numpy if specified + if to_numpy and isinstance(x, torch.Tensor): + if x.requires_grad: + return x.detach().numpy() + return x.numpy() + return x def save(self, filename): """save the inner model.""" @@ -298,283 +427,16 @@ class Approximator(object): ############# def __call__(self, x): + """Predict the output using the inner learning model given the input.""" return self.predict(x) - def __str__(self): + def __repr__(self): + """Return a representation of the model.""" return self.model.__str__() - -class RandomApproximator(Approximator): - r"""Random Approximator - """ - - class Random(object): - - def __init__(self, num_outputs, seed=None): - self.num_outputs = num_outputs - if seed is not None: - np.random.seed(seed) - - def __init__(self, outputs, preprocessors=None, postprocessors=None): - # call parent class - model = self.Random(num_outputs=self._size(outputs), seed=None) - super(RandomApproximator, self).__init__(inputs=None, outputs=outputs, model=model, - preprocessors=preprocessors, postprocessors=postprocessors) - - def _size(self, x): - size = 0 - if isinstance(x, (State, Action)): - if x.is_discrete(): - size = x.space[0].n - else: - size = x.total_size() - elif isinstance(x, np.ndarray): - size = x.size - elif isinstance(x, torch.Tensor): - size = x.numel() - elif isinstance(x, int): - size = x - return size - - # def predict(self, x): - # if isinstance(self.outputs, (State, Action)): - # # get the space of each output - # spaces = self.outputs.space - # - # # sample from each space - # output_data = [space.sample() for space in spaces] - # - # # set the data for each action - # self.outputs.data = output_data - # - # return self.outputs - - def predict(self, x): - # x = self.preprocessors(x) - x = self.model.predict(x.data[0]) - if isinstance(self.outputs, (State, Action)) and self.outputs.is_discrete(): - x = np.argmax(x) - # x = self.postprocessors(x) - return x - - -class LinearApproximator(Approximator): - r"""Linear Function Approximator - """ - - def __init__(self, inputs, outputs, preprocessors=None, postprocessors=None): - # call parent class - model = Linear(num_inputs=self._size(inputs), num_outputs=self._size(outputs), add_bias=True) - super(LinearApproximator, self).__init__(inputs, outputs, model=model, preprocessors=preprocessors, - postprocessors=postprocessors) - - def _size(self, x): - size = 0 - if isinstance(x, (State, Action)): - if x.is_discrete(): - size = x.space[0].n - else: - size = x.total_size() - elif isinstance(x, np.ndarray): - size = x.size - elif isinstance(x, torch.Tensor): - size = x.numel() - elif isinstance(x, int): - size = x - return size - - def predict(self, x, to_numpy=True, return_logits=False): - x = x.data[0] - for processor in self.preprocessors: - x = processor(x) - x = self.model.predict(x, to_numpy=to_numpy) - if isinstance(self.outputs, (State, Action)) and self.outputs.is_discrete() and not return_logits: - if to_numpy: - x = np.array([np.argmax(x)]) - else: - x = torch.argmax(x, dim=0, keepdim=True) - # x = self.postprocessors(x) - return x - - -class NNApproximator(Approximator): - r"""Neural Network Function Approximator - """ - - def __init__(self, inputs, outputs, model, preprocessors=None, postprocessors=None): - - # call parent class - super(NNApproximator, self).__init__(inputs, outputs, model, preprocessors=preprocessors, - postprocessors=postprocessors) - - # convert/wrap the model - if not isinstance(model, NN): - model = NN(model, input_dims=inputs.shape, output_dims=outputs.shape) # TODO - self.model = model - - -class MLPApproximator(NNApproximator): - r"""Multi-Layer Perceptron Function Approximator - - It creates a feed-forward and fully-connected neural network, where linear layers are followed by non-linear - activation functions. The input and output dimensions are inferred from the inputs and outputs. - """ - - def __init__(self, inputs, outputs, hidden_units=(), - activation_fct='Linear', last_activation_fct=None, dropout_prob=None, - preprocessors=None, postprocessors=None): - - # check that the inputs and ouputs are 1D - # if not self._check1D(inputs): - # raise ValueError("Length of input shape should be 1! Instead, got {}".format(inputs.shape)) - # print(outputs) - # print(outputs.shape) - # if not self._check1D(outputs): - # raise ValueError("Length of output shape should be 1! Instead, got {}".format(outputs.shape)) - - input_size = self._size(inputs) - output_size = self._size(outputs) - - # create model - num_units = [input_size] + list(hidden_units) + [output_size] - model = MLP(num_units=num_units, activation_fct=activation_fct, last_activation_fct=last_activation_fct, - dropout_prob=dropout_prob) - - # call superclass - super(MLPApproximator, self).__init__(inputs, outputs, model, preprocessors=preprocessors, - postprocessors=postprocessors) - - def _size(self, x): - if isinstance(x, (State, Action)): - size = x.total_size() - elif isinstance(x, np.ndarray): - size = x.size - elif isinstance(x, torch.Tensor): - size = x.numel() - elif isinstance(x, int): - size = x - return size - - def _check1D(self, arg): - """Check that the given argument is a 1D vector, or simple array""" - # if isinstance(arg, np.ndarray): - shapes = arg.shape - # else: - # shape = arg.shape() - for shape in shapes: - if not (len(shape) == 1 and isinstance(shape[0], int)): - return False - return True - - def predict(self, x): - # convert given input to torch tensor - if isinstance(x, (State, Action)): - x = np.concatenate((x. data)) - x = torch.from_numpy(x).float() - - # feed it to the model and get predicted output - x = self.model(x) - - # check output - if isinstance(self.outputs, (State, Action)): - # data - self.outputs.train_data = x - # convert back output from torch tensor to np array - x = x.detach().numpy() - self.outputs.data = x - - return self.outputs - - return x - - -class NEATApproximator(Approximator): - r"""NEAT Approximator - - See Also: `neat_model.py`, `neat_policy`, `neat_algo.py` - """ - - def __init__(self, inputs, outputs, num_hidden=0, activation_fct='relu', network_type='feedforward', - aggregation='sum', weights_limits=(-20, 20), bias_limits=(-20, 20), - preprocessors=None, postprocessors=None): - - # call parent class - model = NEATModel(num_inputs=self._size(inputs), num_outputs=self._size(outputs), num_hidden=num_hidden, - activation_fct=activation_fct, network_type=network_type, aggregation=aggregation, - weights_limits=weights_limits, bias_limits=bias_limits) - super(NEATApproximator, self).__init__(inputs, outputs, model=model, preprocessors=preprocessors, - postprocessors=postprocessors) - - ############## - # Properties # - ############## - - @property - def config(self): - """Return the config object""" - return self.model.config - - @config.setter - def config(self, config): - """Set the config file (str) or object.""" - self.model.config = config - - @property - def genome(self): - return self.model.genome - - @genome.setter - def genome(self, genome): - self.model.genome = genome - - @property - def network(self): - return self.model.network - - @property - def population(self): - return self.model.population - - ########### - # Methods # - ########### - - def _size(self, x): - size = 0 - if isinstance(x, (State, Action)): - if x.is_discrete(): - size = x.space[0].n - else: - size = x.total_size() - elif isinstance(x, np.ndarray): - size = x.size - elif isinstance(x, torch.Tensor): - size = x.numel() - elif isinstance(x, int): - size = x - return size - - def predict(self, x, to_numpy=True): - # x = self.preprocessors(x) - x = self.model.predict(x.merged_data[0]) - - if isinstance(self.outputs, (State, Action)): - if self.outputs.is_discrete(): - x = np.argmax(x) - elif self.outputs.is_continuous(): - x = 2 * np.array(x) - 1 - else: - raise NotImplementedError("The outputs are not discrete or continuous...") - self.outputs.data = x - # x = self.postprocessors(x) - # return x - return self.outputs - - def set_network(self, genome=None, config=None): - self.model.set_network(genome, config) - - def update_config(self, config): - self.model.update_config(config) + def __str__(self): + """Return a string describing the model.""" + return self.model.__str__() # Tests diff --git a/pyrobolearn/approximators/basic_approximator.py b/pyrobolearn/approximators/basic_approximator.py new file mode 100644 index 0000000..ae1d7bf --- /dev/null +++ b/pyrobolearn/approximators/basic_approximator.py @@ -0,0 +1,78 @@ +#!/usr/bin/env python +"""Define basic function approximators. + +Define the various basic approximators such as the random approximator, linear approximator, etc. + +Dependencies: +- `pyrobolearn.models` +- `pyrobolearn.states` +- `pyrobolearn.actions` +""" + +import collections +import numpy as np +import torch + +from pyrobolearn.states import State +from pyrobolearn.actions import Action + +from pyrobolearn.approximators.approximator import Approximator +from pyrobolearn.models.basics.linear import Linear + +__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 RandomApproximator(Approximator): + r"""Random Approximator + """ + + class Random(object): + + def __init__(self, num_outputs, seed=None): + self.num_outputs = num_outputs + if seed is not None: + np.random.seed(seed) + + def __init__(self, outputs, preprocessors=None, postprocessors=None): + """ + Initialize the random approximator. + + Args: + outputs (State, Action, np.array, torch.Tensor): outputs of the inner models (instance of Action/State) + preprocessors (None, Processor, list of Processor): the inputs are first given to the preprocessors then + to the model. + postprocessors (None, Processor, list of Processor): the predicted outputs by the model are given to the + processors before being returned. + """ + # call parent class + model = self.Random(num_outputs=self._size(outputs), seed=None) + super(RandomApproximator, self).__init__(inputs=None, outputs=outputs, model=model, + preprocessors=preprocessors, postprocessors=postprocessors) + + +class LinearApproximator(Approximator): + r"""Linear Function Approximator + """ + + def __init__(self, inputs, outputs, preprocessors=None, postprocessors=None): + """ + Initialize the linear approximator. + + Args: + outputs (State, Action, np.array, torch.Tensor): outputs of the inner models (instance of Action/State) + preprocessors (None, Processor, list of Processor): the inputs are first given to the preprocessors then + to the model. + postprocessors (None, Processor, list of Processor): the predicted outputs by the model are given to the + processors before being returned. + """ + # call parent class + model = Linear(num_inputs=self._size(inputs), num_outputs=self._size(outputs), add_bias=True) + super(LinearApproximator, self).__init__(inputs, outputs, model=model, preprocessors=preprocessors, + postprocessors=postprocessors) diff --git a/pyrobolearn/approximators/nn_approximator.py b/pyrobolearn/approximators/nn_approximator.py new file mode 100644 index 0000000..21254d7 --- /dev/null +++ b/pyrobolearn/approximators/nn_approximator.py @@ -0,0 +1,233 @@ +#!/usr/bin/env python +"""Define Neural Network approximators. + +Dependencies: +- `pyrobolearn.models.nn` +- `pyrobolearn.states` +- `pyrobolearn.actions` +""" + +import collections +import numpy as np +import torch + +from pyrobolearn.states import State +from pyrobolearn.actions import Action + +from pyrobolearn.approximators.approximator import Approximator +from pyrobolearn.models.nn import NN, MLP, NEATModel + +__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 NNApproximator(Approximator): + r"""Neural Network Function Approximator + """ + + def __init__(self, inputs, outputs, model, preprocessors=None, postprocessors=None): + """ + Initialize the Neural Network approximator. + + Args: + inputs (State, Action, np.array, torch.Tensor): inputs of the inner models (instance of State/Action) + outputs (State, Action, np.array, torch.Tensor): outputs of the inner models (instance of Action/State) + model (Model, torch.nn.Module): Learning model + preprocessors (None, Processor, list of Processor): the inputs are first given to the preprocessors then + to the model. + postprocessors (None, Processor, list of Processor): the predicted outputs by the model are given to the + processors before being returned. + """ + + # call parent class + super(NNApproximator, self).__init__(inputs, outputs, model, preprocessors=preprocessors, + postprocessors=postprocessors) + + # convert/wrap the model + if not isinstance(model, NN): + model = NN(model, input_shape=inputs.shape, output_shape=outputs.shape) # TODO + self.model = model + + +class MLPApproximator(NNApproximator): + r"""Multi-Layer Perceptron Function Approximator + + It creates a feed-forward and fully-connected neural network, where linear layers are followed by non-linear + activation functions. The input and output dimensions are inferred from the inputs and outputs. + """ + + def __init__(self, inputs, outputs, hidden_units=(), + activation_fct='Linear', last_activation_fct=None, dropout_prob=None, + preprocessors=None, postprocessors=None): + """ + Initialize the Multi-Layer Perceptron approximator. + + Args: + inputs (State, Action, np.array, torch.Tensor): inputs of the inner models (instance of State/Action) + outputs (State, Action, np.array, torch.Tensor): outputs of the inner models (instance of Action/State) + hidden_units (tuple, list of int): number of hidden units in each layer + activation_fct (str): activation function to apply on each layer + last_activation_fct (str, None): activation function to apply on the last layer + dropout_prob (None, float): dropout probability + preprocessors (None, Processor, list of Processor): the inputs are first given to the preprocessors then + to the model. + postprocessors (None, Processor, list of Processor): the predicted outputs by the model are given to the + processors before being returned. + """ + + # check that the inputs and ouputs are 1D + # if not self._check1D(inputs): + # raise ValueError("Length of input shape should be 1! Instead, got {}".format(inputs.shape)) + # print(outputs) + # print(outputs.shape) + # if not self._check1D(outputs): + # raise ValueError("Length of output shape should be 1! Instead, got {}".format(outputs.shape)) + + input_size = self._size(inputs) + output_size = self._size(outputs) + + # create model + num_units = [input_size] + list(hidden_units) + [output_size] + model = MLP(num_units=num_units, activation_fct=activation_fct, last_activation_fct=last_activation_fct, + dropout_prob=dropout_prob) + + # call superclass + super(MLPApproximator, self).__init__(inputs, outputs, model, preprocessors=preprocessors, + postprocessors=postprocessors) + + def _check1D(self, arg): + """Check that the given argument is a 1D vector, or simple array""" + # if isinstance(arg, np.ndarray): + shapes = arg.shape + # else: + # shape = arg.shape() + for shape in shapes: + if not (len(shape) == 1 and isinstance(shape[0], int)): + return False + return True + + def predict(self, x): + # convert given input to torch tensor + if isinstance(x, (State, Action)): + x = x.merged_data[0] + x = torch.from_numpy(x).float() + + # feed it to the model and get predicted output + x = self.model(x) + + # check output + if isinstance(self.outputs, (State, Action)): + # data + self.outputs.train_data = x + # convert back output from torch tensor to np array + x = x.detach().numpy() + self.outputs.data = x + + return self.outputs + + return x + + +class NEATApproximator(Approximator): + r"""NEAT Approximator + + See Also: `neat_model.py`, `neat_policy`, `neat_algo.py` + """ + + def __init__(self, inputs, outputs, num_hidden=0, activation_fct='relu', network_type='feedforward', + aggregation='sum', weights_limits=(-20, 20), bias_limits=(-20, 20), + preprocessors=None, postprocessors=None): + """ + Initialize the NEAT Approximator. This uses as the inner learning model a neural network that can evolve its + weights as well as its topology. + + Args: + inputs (int, np.array, torch.Tensor, State, Action): inputs. + outputs (int, np.array, torch.Tensor, State, Action): outputs. + num_hidden (int): number of hidden units. + activation_fct (str): activation function to use. + network_type (str): type of neural network. Select between 'feedforward' and 'recurrent'. + aggregation (str): how to aggregate the input signals of a node. Select between 'sum', 'product', 'max', + 'min', 'maxabs', 'median', and 'mean'. + weights_limits (tuple): weight limits / bounds. The tuple contains the lower and upper bounds. + bias_limits (tuple): bias limits / bounds. The tuple contains the lower and upper bounds. + preprocessors (None, Processor, list of Processor): the inputs are first given to the preprocessors then + to the model. + postprocessors (None, Processor, list of Processor): the predicted outputs by the model are given to the + processors before being returned. + """ + + # call parent class + model = NEATModel(num_inputs=self._size(inputs), num_outputs=self._size(outputs), num_hidden=num_hidden, + activation_fct=activation_fct, network_type=network_type, aggregation=aggregation, + weights_limits=weights_limits, bias_limits=bias_limits) + super(NEATApproximator, self).__init__(inputs, outputs, model=model, preprocessors=preprocessors, + postprocessors=postprocessors) + + ############## + # Properties # + ############## + + @property + def config(self): + """Return the config object""" + return self.model.config + + @config.setter + def config(self, config): + """Set the config file (str) or object.""" + self.model.config = config + + @property + def genome(self): + """Return the NEAT model's genome.""" + return self.model.genome + + @genome.setter + def genome(self, genome): + """Set the genome.""" + self.model.genome = genome + + @property + def network(self): + """Return the NEAT model's network.""" + return self.model.network + + @property + def population(self): + """Return the population used in NEAT.""" + return self.model.population + + ########### + # Methods # + ########### + + def predict(self, x, to_numpy=True): + # x = self.preprocessors(x) + x = self.model.predict(x.merged_data[0]) + + if isinstance(self.outputs, (State, Action)): + if self.outputs.is_discrete(): + x = np.argmax(x) + elif self.outputs.is_continuous(): + x = 2 * np.array(x) - 1 + else: + raise NotImplementedError("The outputs are not discrete or continuous...") + self.outputs.data = x + # x = self.postprocessors(x) + # return x + return self.outputs + + def set_network(self, genome=None, config=None): + """Set the genome network.""" + self.model.set_network(genome, config) + + def update_config(self, config): + """Update the configuration file.""" + self.model.update_config(config) diff --git a/pyrobolearn/approximators/processor.py b/pyrobolearn/approximators/processor.py deleted file mode 100644 index 5f0006a..0000000 --- a/pyrobolearn/approximators/processor.py +++ /dev/null @@ -1,33 +0,0 @@ - - -class Processor(object): - r"""Processor - - This class describes pre- and post- processors. Specifically, it describes how to process the data before and - after the policy. - """ - def __init__(self, inputs, outputs): - self.inputs = inputs - self.outputs = outputs - - -class PreProcessor(Processor): - r"""Preprocessor - - It processes the input data before giving it to the policy/controller. For instance, it can be a state estimator - such as a kalman filter. - """ - - def __init__(self, inputs, outputs): - super(PreProcessor, self).__init__(inputs, outputs) - - -class PostProcessor(Processor): - r"""Postprocessor - - It processes the data outputted by the policy. For instance, the policy could output cartesian positions, - and we could process this data using an inverse kinematic scheme to output joint data. - """ - - def __init__(self, inputs, outputs): - super(PostProcessor, self).__init__(inputs, outputs) \ No newline at end of file diff --git a/pyrobolearn/models/__init__.py b/pyrobolearn/models/__init__.py index 255ae1e..ace1619 100644 --- a/pyrobolearn/models/__init__.py +++ b/pyrobolearn/models/__init__.py @@ -4,14 +4,8 @@ from .model import Model # General Learning models # -# Linear -from .linear import Linear - -# PCA -from .pca import PCA - -# Polynomial -from .polynomial import Polynomial, PolynomialFunction +# basics: Linear, Polynomial, PCA +from .basics import * # Gaussian from .gaussian import Gaussian, MVN # MVN is an alias diff --git a/pyrobolearn/models/basic_models.py b/pyrobolearn/models/basic_models.py deleted file mode 100644 index e69de29..0000000 diff --git a/pyrobolearn/models/basics/__init__.py b/pyrobolearn/models/basics/__init__.py new file mode 100644 index 0000000..775235d --- /dev/null +++ b/pyrobolearn/models/basics/__init__.py @@ -0,0 +1,9 @@ + +# Linear +from .linear import Linear + +# PCA +from .pca import PCA + +# Polynomial +from .polynomial import Polynomial, PolynomialFunction diff --git a/pyrobolearn/models/linear.py b/pyrobolearn/models/basics/linear.py similarity index 90% rename from pyrobolearn/models/linear.py rename to pyrobolearn/models/basics/linear.py index 41e9053..ab45dfd 100644 --- a/pyrobolearn/models/linear.py +++ b/pyrobolearn/models/basics/linear.py @@ -13,7 +13,8 @@ except ImportError as e: import numpy as np import torch -# from model import Model + +# from pyrobolearn.models.model import Model __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -52,24 +53,34 @@ class Linear(object): ############## @property - def input_dims(self): - """Return the input dimension of the model""" + def input_size(self): + """Return the input size of the model""" return self.model.weight.shape[1] @property - def output_dims(self): - """Return the output dimension of the model""" + def output_size(self): + """Return the output size of the model""" return self.model.weight.shape[0] @property def input_shape(self): """Return the input shape of the model""" - return tuple([self.input_dims]) + return tuple([self.input_size]) @property def output_shape(self): """Return the output shape of the model""" - return tuple([self.output_dims]) + return tuple([self.output_size]) + + @property + def input_dim(self): + """Return the input dimension; i.e. len(input_shape).""" + return len(self.input_shape) + + @property + def output_dim(self): + """Return the output dimension; i.e. len(output_shape).""" + return len(self.output_shape) @property def num_parameters(self): @@ -217,9 +228,11 @@ class Linear(object): pass def reset(self): + """Reset the linear model.""" pass def __call__(self, x, to_numpy=True): + """Predict the output given the input :attr:`x`.""" return self.predict(x, to_numpy=to_numpy) # def concatenate(self, other): @@ -239,8 +252,8 @@ if __name__ == '__main__': # test with numpy x = np.array(range(3)) model = Linear(num_inputs=len(x), num_outputs=2, add_bias=True) - print("Linear model's input size: {}".format(model.input_dims)) - print("Linear model's output size: {}".format(model.output_dims)) + print("Linear model's input size: {}".format(model.input_size)) + print("Linear model's output size: {}".format(model.output_size)) y = model(x) print("Linear input: {}".format(x)) print("Linear output: {}".format(y)) diff --git a/pyrobolearn/models/pca.py b/pyrobolearn/models/basics/pca.py similarity index 90% rename from pyrobolearn/models/pca.py rename to pyrobolearn/models/basics/pca.py index 267c897..a5af796 100644 --- a/pyrobolearn/models/pca.py +++ b/pyrobolearn/models/basics/pca.py @@ -1,5 +1,7 @@ #!/usr/bin/env python -"""Define the PCA model. +"""Provide the Principal Component Analysis model. + +Dependencies: None """ import numpy as np @@ -90,37 +92,37 @@ class PCA(object): # TODO: think if PCA can be considered as a model @staticmethod - def isParametric(): + def is_parametric(): """PCA is a non-parametric approach""" return False @staticmethod - def isLinear(): + def is_linear(): """PCA does not have parameters, but it is a linear dimensionality reduction algo""" return True @staticmethod - def isRecurrent(): + def is_recurrent(): """PCA is not recurrent""" return False @staticmethod - def isLatent(): + def is_latent(): """PCA gives a latent model""" return True @staticmethod - def isProbabilistic(): + def is_probabilistic(): """PCA is not a probabilistic approach but a deterministic one""" return False @staticmethod - def isDiscriminative(): + def is_discriminative(): """PCA is a discriminative model, which projects the given data into a lower space""" return True @staticmethod - def isGenerative(): + def is_generative(): """PCA is not a generative model from which you can sample from it""" return False @@ -128,16 +130,30 @@ class PCA(object): # Methods # ########### - def parameters(self): + """Return an iterator over the parameters.""" raise RuntimeError("PCA doesn't have any parameters.") - def getParams(self): + def named_parameters(self): + """Return an iterator over the parameters, yielding both the name and the parameter itself.""" raise RuntimeError("PCA doesn't have any parameters.") - def getHyperparams(self): + def list_parameters(self): + """Return the list of parameters.""" + return list(self.parameters()) + + def hyperparameters(self): + """Return an iterator over the hyper-parameters.""" pass + def named_hyperparameters(self): + """Return an iterator over the hyper-parameters, yielding both the name and the hyper-parameter itself.""" + pass + + def list_hyperparameters(self): + """Return the list of hyper-parameters.""" + return list(self.hyperparameters()) + def train(self, X, normalize=False, copy=True): """ Compute PCA on the given data. This method will center the data. @@ -270,4 +286,4 @@ class HierarchicalPCA(PCA): \end{array} \right] """ - pass \ No newline at end of file + pass diff --git a/pyrobolearn/models/polynomial.py b/pyrobolearn/models/basics/polynomial.py similarity index 81% rename from pyrobolearn/models/polynomial.py rename to pyrobolearn/models/basics/polynomial.py index 7bcda69..14f49b4 100644 --- a/pyrobolearn/models/polynomial.py +++ b/pyrobolearn/models/basics/polynomial.py @@ -11,6 +11,8 @@ import collections import numpy as np import torch +# from pyrobolearn.models.model import Model + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] @@ -21,6 +23,71 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" +class PolynomialFunction(object): + r"""Polynomial function + + Polynomial function to be applied on the given inputs. + """ + + def __init__(self, degree=1): + """ + Initialize the polynomial function. + + Args: + degree (int, list of ints): degree(s) of the polynomial. Setting `degree=3`, will return `[1,x,x^2,x^3]` + as output, while setting `degree=[1,3]` will return `[x,x^3]` as output. + """ + self.degree = degree + + ############## + # Properties # + ############## + + @property + def degree(self): + """Return the degree of the polynomial""" + return self._degree + + @degree.setter + def degree(self, degree): + """Set the degree of the polynomial""" + # checks + if isinstance(degree, int): + degree = range(degree + 1) + elif isinstance(degree, collections.Iterable): + for d in degree: + if not isinstance(d, int): + raise TypeError("Expecting the given degrees to be positive integers, but got {}".format(type(d))) + if d < 0: + raise ValueError("Expecting the given degrees to be positive integers, but got {}".format(d)) + else: + raise TypeError("Expecting the degree to be a positive integer or a list of positive integers.") + self._degree = degree + + @property + def size(self): + """Return the number of exponents. The output vector is then of size = size * len(x)""" + return len(self.degree) + + ########### + # Methods # + ########### + + def reset(self): + """Reset the polynomial model.""" + pass + + def predict(self, x): + """Return output polynomial vector""" + if isinstance(x, np.ndarray): + return np.concatenate([x**d for d in self.degree]) + elif isinstance(x, torch.Tensor): + return torch.cat([x**d for d in self.degree]) + + def __call__(self, x): + return self.predict(x) + + class Polynomial(object): r"""Polynomial model @@ -38,9 +105,7 @@ class Polynomial(object): Args: num_inputs (int): dimension of the input vector x num_outputs (int): dimension of the output vector y - polynomial_fct (callable, PolynomialFunction): polynomial function to be applied on the input vector x. - It has to be callable, returns the output vector :math:`\phi(x)` when called, and has a `size` - attribute which returns the number of exponents in the polynomial. + polynomial_fct (PolynomialFunction): polynomial function :math:`\phi` to be applied on the input vector x. """ self.phi = polynomial_fct num_inputs = num_inputs * self.phi.size @@ -57,18 +122,12 @@ class Polynomial(object): return self._phi @phi.setter - def phi(self, fct): + def phi(self, function): """Set the polynomial function""" - if not callable(fct): - raise TypeError("Expecting the polynomial function to be callable.") - - if not hasattr(fct, 'size'): - raise ValueError("Expecting the polynomial function to have the 'size' attribute.") - - # if len(inspect.getargspec(fct).args) < 1: - # raise TypeError("Expecting the polynomial function to accept an argument (the state).") - - self._phi = fct + if not isinstance(function, PolynomialFunction): + raise TypeError("Expecting the given polynomial function to be an instance of `PolynomialFunction`, " + "instead got: {}".format(type(function))) + self._phi = function @property def polynomial_function(self): @@ -76,29 +135,39 @@ class Polynomial(object): return self._phi @polynomial_function.setter - def polynomial_function(self, fct): + def polynomial_function(self, function): """Set the polynomial function""" - self.phi = fct + self.phi = function @property - def input_dims(self): + def input_size(self): """Return the input dimension of the model""" return self.model.weight.shape[1] @property - def output_dims(self): + def output_size(self): """Return the output dimension of the model""" return self.model.weight.shape[0] @property def input_shape(self): """Return the input shape of the model""" - return tuple([self.input_dims]) + return tuple([self.input_size]) @property def output_shape(self): """Return the output shape of the model""" - return tuple([self.output_dims]) + return tuple([self.output_size]) + + @property + def input_dim(self): + """Return the input dimension of the model; i.e. len(input_shape).""" + return len(self.input_shape) + + @property + def output_dim(self): + """Return the output dimension of the model; i.e. len(output_shape).""" + return len(self.output_shape) @property def num_parameters(self): @@ -167,6 +236,20 @@ class Polynomial(object): """Return a list of parameters""" return list(self.parameters()) + def hyperparameters(self): + """Return an iterator over the hyperparameters.""" + for degree in self.phi.degree: + yield degree + + def named_hyperparameters(self): + """Return an iterator over the model hyperparameters, yielding both the name and the hyperparameter itself.""" + for idx, degree in enumerate(self.phi.degree): + yield "degree {}".format(idx), degree + + def list_hyperparameters(self): + """Return the hyperparameters in the form of a list.""" + return list(self.hyperparameters()) + def get_vectorized_parameters(self, to_numpy=True): """Return a vectorized form (1 dimensional array) of the parameters.""" parameters = self.parameters() @@ -214,71 +297,18 @@ class Polynomial(object): return y def __call__(self, x, to_numpy=True): + """Predict the output given the input :attr:`x`.""" return self.predict(x, to_numpy=to_numpy) -class PolynomialFunction(object): - r"""Polynomial function - - Polynomial function to be applied on the given inputs. - """ - - def __init__(self, degree=1): - """ - Initialize the polynomial function. - - Args: - degree (int, list of ints): degree(s) of the polynomial. Setting `degree=3`, will return `[1,x,x^2,x^3]` - as output, while setting `degree=[1,3]` will return `[x,x^3]` as output. - """ - self.degree = degree - - @property - def degree(self): - """Return the degree of the polynomial""" - return self._degree - - @degree.setter - def degree(self, degree): - """Set the degree of the polynomial""" - # checks - if isinstance(degree, int): - degree = range(degree + 1) - elif isinstance(degree, collections.Iterable): - for d in degree: - if not isinstance(d, int): - raise TypeError("Expecting the given degrees to be positive integers, but got {}".format(type(d))) - if d < 0: - raise ValueError("Expecting the given degrees to be positive integers, but got {}".format(d)) - else: - raise TypeError("Expecting the degree to be a positive integer or a list of positive integers.") - - self._degree = degree - - @property - def size(self): - """Return the number of exponents. The output vector is then of size = size * len(x)""" - return len(self.degree) - - def predict(self, x): - """Return output polynomial vector""" - if isinstance(x, np.ndarray): - return np.concatenate([x**d for d in self.degree]) - elif isinstance(x, torch.Tensor): - return torch.cat([x**d for d in self.degree]) - - def __call__(self, x): - return self.predict(x) - - # Tests if __name__ == '__main__': # test with numpy x = np.array(range(3)) fct = PolynomialFunction(degree=3) model = Polynomial(num_inputs=len(x), num_outputs=2, polynomial_fct=fct) - print("Polynomial input size: {}".format(model.input_dims)) - print("Polynomial output size: {}".format(model.output_dims)) + print("Polynomial input size: {}".format(model.input_size)) + print("Polynomial output size: {}".format(model.output_size)) y = model(x) print("Polynomial input: {}".format(x)) print("Polynomial output: {}".format(y)) diff --git a/pyrobolearn/models/cpg/__init__.py b/pyrobolearn/models/cpg/__init__.py new file mode 100644 index 0000000..b7b9479 --- /dev/null +++ b/pyrobolearn/models/cpg/__init__.py @@ -0,0 +1,3 @@ + +# import cpg +from .cpg import * diff --git a/pyrobolearn/models/cpg.py b/pyrobolearn/models/cpg/cpg.py similarity index 97% rename from pyrobolearn/models/cpg.py rename to pyrobolearn/models/cpg/cpg.py index 93d206e..212d805 100644 --- a/pyrobolearn/models/cpg.py +++ b/pyrobolearn/models/cpg/cpg.py @@ -11,6 +11,8 @@ import matplotlib.pyplot as plt from matplotlib.widgets import Slider, CheckButtons from matplotlib.animation import FuncAnimation +# from pyrobolearn.models.model import Model + __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -699,6 +701,36 @@ class CPGNetwork(object): # Properties # ############## + @property + def input_size(self): + """Return the input size of the model.""" + return len(self.nodes) + + @property + def output_size(self): + """Return the output size of the model.""" + return len(self.nodes) + + @property + def input_shape(self): + """Return the input shape of the model.""" + return tuple([self.input_size]) + + @property + def output_shape(self): + """Return the output shape of the model.""" + return tuple([self.output_size]) + + @property + def input_dim(self): + """Return the input dimension of the model; i.e. len(input_shape).""" + return len(self.input_shape) + + @property + def output_dim(self): + """Return the output dimension of the model; i.e. len(output_shape).""" + return len(self.output_shape) + @property def num_parameters(self): """Return the total number of parameters in this CPG network.""" @@ -708,6 +740,7 @@ class CPGNetwork(object): # Methods # ########### + # TODO: update parameters to hyperparameters def parameters(self): """Returns an iterator over the model parameters.""" for node in self.nodes.values(): diff --git a/pyrobolearn/models/dmp.py b/pyrobolearn/models/dmp.py deleted file mode 100644 index 1f9065b..0000000 --- a/pyrobolearn/models/dmp.py +++ /dev/null @@ -1,1725 +0,0 @@ -#!/usr/bin/env python -"""Define dynamic movement primitives (their canonical and transformation systems) - -This file implements dynamic movement primitives for discrete and rhythmic movements. -""" - - -from abc import ABCMeta, abstractmethod -import numpy as np -import scipy.interpolate - - -__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" - - -# Canonical Systems # - -class CS(object): - r"""Canonical System. - - A canonical system (CS) drives a dynamic movement primitive (DMP) by providing a phase variable [1]. - The phase variable was introduced to avoid an explicit dependency with time in the DMP equations. Canonical - systems can be categorized in two main categories: - * discrete CS: used for discrete movements (such as reaching, pushing/pulling, hitting, etc) - * rhythmic CS: used for rhythmic movements (such as walking, running, dribbling, sewing, flipping a pancake, etc) - - Each of these systems are described by differential equations which are solved using Euler's method. - See their corresponding classes `DiscreteCS` and `RhythmicCS` for more information. - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - """ - - __metaclass__ = ABCMeta - - def __init__(self, dt=0.01, T=1.): - """Initialize the canonical system. - - Args: - dt (float): the time step used in Euler's method when solving the differential equation - A very small step will lead to a better accuracy but will take more time. - """ - # set variables - self.dt = dt - self.T = T - self.timesteps = int(T / self.dt) - # rescale integration step (same as np.linspace(0.,T.,timesteps) instead of np.arange(0,T,dt)) - self.dt = self.T / (self.timesteps - 1.) - - self.init_phase = 1.0 - self.s = 1.0 - - # reset the phase variable - self.reset() - - @abstractmethod - def step(self, tau=1.0, error_coupling=1.0): - """Perform a step using Euler's method. This needs to be implemented in the child classes.""" - raise NotImplementedError - - def reset(self): - """Reset the phase variable""" - self.s = self.init_phase - return self.s - - def rollout(self, tau=1.0, error_coupling=1.0): - """Generate phase variable in an open loop fashion. - - Args: - tau (float): Increase tau to make the system slower, and decrease it to make it faster - error_coupling (float): slow down if the error is > 1 - """ - timesteps = int(self.timesteps * tau) - self.s_track = np.zeros(timesteps) - - # reset - self.reset() - - # roll - for t in range(timesteps): - self.s_track[t] = self.s - self.step(tau, error_coupling) - - return self.s_track - - -class DiscreteCS(CS): - r"""Discrete Canonical System. - - The discrete canonical system drives the various DMPs by providing the phase variable at each time step, and is - given by: - - .. math:: \tau \dot{s} = - \alpha_s s - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, :math:`s` is the phase - variable that drives the DMP, and :math:`\alpha_s` is a predefined constant. - This differential equation is solved using Euler's method. - - This version is used for discrete movements, where :math:`s` starts from 1 and converge to 0 as time progresses. - The phase variable was introduced to avoid an explicit dependency of time in the DMP equations. - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - """ - - def __init__(self, alpha_s=1, dt=0.01): - super(DiscreteCS, self).__init__(dt=dt, T=1.0) - self.alpha_s = alpha_s - - def reset(self): - """Reset the phase variable""" - self.s = self.init_phase - return self.s - - def step(self, tau=1.0, error_coupling=1.0): - """Generate phase value for discrete movements. - - The phase variable :math:`s` is generated by solving :math:`\tau \dot{s} = - \alpha_s s` using Euler's method. - This phase decays from 1 to 0. - - Args: - tau (float): Increase tau to make the system slower, and decrease it to make it faster - error_coupling (float): slow down if the error is > 1 - - Returns: - float: phase value - """ - s = self.s - self.s += (-self.alpha_s/tau * self.s * error_coupling) * self.dt - # return self.s - return s - - -class RhythmicCS(CS): - r"""Rhythmic Canonical System. - - The rhythmic canonical system drives the various DMPs by providing a phase variable that is periodic [1]. It is - used for rhythmic movements (such as walking, dribbling, sewing, etc.) and is given by: - - .. math:: \tau \dot{s} = 1 - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, :math:`s` is the phase - variable that drives the DMP. This differential equation is solved using Euler's method. - - Rhythmic canonical systems can also be coupled with each other as done in [2] to synchronize various DMPs. - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - [2] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 - """ - - def __init__(self, dt=0.01): - super(RhythmicCS, self).__init__(dt=dt, T=2*np.pi) - self.init_phase = 0.0 - - def reset(self): - """Reset the phase variable""" - self.s = self.init_phase - return self.s - - def step(self, tau=1.0, error_coupling=1.0): - r"""Generate phase value for rhythmic movements. - - The phase variable :math:`s` is generated by solving :math:`\tau \dot{s} = 1` using Euler's method. - - Args: - tau (float): Increase tau to make the system slower, and decrease it to make it faster - error_coupling (float): slow down if the error is > 1 - - Returns: - float: phase value - """ - s = self.s - self.s += (1./tau * error_coupling) * self.dt - # return self.s - return s - - -class RhythmicNetworkCS(CS): - r"""Rhythmic Network CS. - - In this version, instead of having one canonical system that drives all the various DMPs, we have several - canonical systems coupled with each other, and where each one of them is associated to a particular DMP. - - The evolution of the phase variable :math:`\phi` of the system :math:`i` is given by: - - .. math:: \dot{\phi}_i = \omega_i + \sum_j a_j w_{ij} \sin(\phi_j - \phi_i - \varphi_{ij}) - - where :math:`\omega` is the desired angular velocity (desired frequency), :math:`w_{ij}` are the coupling weights, - :math:`\varphi_{ij}` are the phase biases, and :math:`a_j` are the amplitudes of the other systems :math:`j`. - This formulation is similar to Central Pattern Generators (CPGs), see [3]. - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - [2] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 - [3] "Central pattern generators for locomotion control in animals and robots: a review", Ijspeert, 2008 - """ - def __init__(self, dt=0.01): - super(RhythmicNetworkCS, self).__init__(dt=dt) - - -# Basis Functions # - -class BF(object): - r"""Basis function used in the forcing terms - """ - __metaclass__ = ABCMeta - - def __init__(self): - pass - - @abstractmethod - def compute(self, s): - raise NotImplementedError - - # alias - def __call__(self, s): - return self.compute(s) - - -class EBF(BF): - r"""Exponential basis function - - This basis function is given by the formula: - - .. math:: \psi(s) = \exp \left( - \frac{1}{2 \sigma^2} (s - c)^2 \right) - - where :math:`c` is the center, and :math:`\sigma` is the width of a normal distribution. - - This is often used for discrete DMPs. - """ - def __init__(self, center=0, sigma=1., h=None): - """Initialize basis function - - Args: - center (float, np.ndarray): center of the distribution - sigma (float, np.ndarray): width of the distribution - h (float, np.ndarray): concentration/precision of the basis fct (h = 1/(2*\sigma^2)). - if h is not provided, it will check sigma. - """ - super(EBF, self).__init__() - - if isinstance(center, np.ndarray): pass - - self.c = center - if h is None: - self.h = 1. / (2*sigma**2) # measure the concentration - else: - self.h = h - - def compute(self, s): - if isinstance(s, np.ndarray): - s = s[:, None] - return np.exp(-self.h * (s - self.c)**2) - - -class CBF(BF): - r"""Circular basis function (aka von Mises basis function) - - This basis function is given by the formula: - - .. math:: \psi(s) = \exp \left( h (\cos(s - c) - 1) \right) - - where :math:`c` is the center, and :math:`h` is a measure of concentration. - - This is often used for rhythmic DMPs. - """ - def __init__(self, center=0, h=1.): - """Initialize basis function - - Args: - center (float, np.ndarray): center of the basis fct - h (float, np.ndarray): concentration/precision of the basis fct - """ - super(CBF, self).__init__() - self.c = center - self.h = h - - def compute(self, s): - if isinstance(s, np.ndarray): - s = s[:, None] - # return np.exp(self.h * np.cos(s - self.c) - 1) # this is bad as it is not bounded as we increase the - # number of basis functions. - return np.exp(self.h * np.cos(s - self.c) - self.h) - - -# FORCING TERMS # - -class ForcingTerm(object): - r"""Forcing term used in DMPs - - This basically computes the unscaled forcing term, i.e. a weighted sum of basis functions, which is given by: - - .. math:: f(s) = \frac{ sum_{i} \psi_i(s) w_i }{ \sum_i \psi_i(s) } - - where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the - given input phase variable :math:`s`. - """ - - def __init__(self, weights, basis_functions): - # check that the arguments have the same length - self.w = weights - self.psi = basis_functions - - @property - def weights(self): - return self.w - - @staticmethod - def is_linear(): - return True - - @staticmethod - def is_parametric(): - return True - - @staticmethod - def is_recurrent(): - return False - - def compute(self, s): - """Compute the forcing term - - Compute the value of the forcing term :math:`f(s)` at the given phase value :math:`s`. - - Args: - s (float): phase value - - Returns: - float: value of the forcing term at the given phase value - """ - psi_track = self.psi(s) - if len(psi_track.shape) == 1: - return np.dot(psi_track, self.w) / np.sum(psi_track) - return np.dot(psi_track, self.w) / np.sum(psi_track, axis=1) - - def weighted_basis(self, s): - """Generate weighted basis - - Returns: - np.array[T, M]: weighted basis - """ - return self.psi(s) * self.w - - def normalized_weighted_basis(self, s): - """Generate normalized weighted basis - - Args: - s (float): phase value - - Returns: - np.array[T,M]: normalized weighted basis - """ - psi_track = self.psi(s) - return ((psi_track * self.w).T / np.sum(psi_track, axis=1)).T - - # alias - def __call__(self, s): - return self.compute(s) - - def __str__(self): - return self.__class__.__name__ - - # To override in child classes - def train(self, f_target): - raise NotImplementedError - - # alias - generate_weights = train - - -class DiscreteForcingTerm(ForcingTerm): - r"""Discrete Forcing Term - - .. math:: f(s) = \frac{ sum_{i} \psi_i(s) w_i }{ \sum_i \psi_i(s) } s - - where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the - given input phase variable :math:`s`. - - This forcing term has the property that as the phase converges to 0, it also converges to 0, allowing the - linear part of the DMP equation to converge to the goal. - """ - - def __init__(self, cs, num_basis): - """Initialize the discrete forcing term. - - Args: - cs (CS): discrete canonical system - num_basis (int): number of basis functions - """ - # set canonical system - if not isinstance(cs, DiscreteCS): - raise TypeError("Expecting 'cs' to be an instance of DiscreteCS") - self.cs = cs - - # set num_basis - self.num_basis = num_basis - - # create weights - weights = np.zeros(num_basis) # default f=0 - - # desired activations throughout time - c = np.linspace(0, cs.T, num_basis) - c = np.exp(-cs.alpha_s * c) - - # set variance of basis functions (this was found by trial and error by DeWolf) - h = np.ones(num_basis) * num_basis**1.5 / c / cs.alpha_s - - basis = EBF(center=c, h=h) - super(DiscreteForcingTerm, self).__init__(weights, basis) - - def compute(self, s): - # call parent compute - f = super(DiscreteForcingTerm, self).compute(s) - # scale with phase s - return f * s - - def train(self, f_target, plot=False): - """Train the weights to match the given target forcing term - - Generate a set of weights over the basis functions such that the target forcing term trajectory is matched. - - Args: - f_target (np.array): the desired forcing term trajectory - """ - - # calculate phase and basis functions - s_track = self.cs.rollout() - psi_track = self.psi(s_track) # shape=TxM - - # efficiently calculate BF weights using LWR (Locally Weighted (Linear) Regression) - # spatial scaling term - for b in range(self.num_basis): - numerator = np.sum(s_track * psi_track[:, b] * f_target) - denominator = np.sum(s_track**2 * psi_track[:, b]) - self.w[b] = numerator / denominator - - self.w = np.nan_to_num(self.w) - - if plot: - import matplotlib.pyplot as plt - # plot the basis function activations - plt.figure() - plt.subplot(211) - plt.plot(psi_track) - plt.title('basis functions') - - # plot the desired forcing function vs approx for the first dmp - plt.subplot(212) - plt.title('discrete force') - plt.plot(f_target, label='f_target', linewidth=2.5) - plt.plot(self.compute(s_track), label='f_pred', linewidth=2.5) - - # weighted sum of basis functions - wps = self.weighted_basis(s_track) - plt.plot(wps, linewidth=0.5) - - plt.legend() - plt.tight_layout() - plt.show() - - -class RhythmicForcingTerm(ForcingTerm): - r"""Rhythmic Forcing Term - - .. math:: f(s) = \frac{ sum_{i} \psi_i(s) w_i }{ \sum_i \psi_i(s) } a - - where :math:`w` are the learnable weight parameters, :math:`\psi` are the basis functions evaluated at the - given input phase variable :math:`s`, and :math:`a` is the amplitude. - - When used with DMPs, it produces a limit cycle behavior. - """ - - def __init__(self, cs, num_basis, amplitude=1.): - """ - Initialize the rhythmic forcing term. - - Args: - cs (CS): rhythmic canonical system - num_basis (int): number of basis functions - amplitude (float): amplitude - """ - # set canonical system - if not isinstance(cs, RhythmicCS): - raise TypeError("Expecting 'cs' to be an instance of RhythmicCS") - self.cs = cs - - # set num_basis and amplitude - self.num_basis = num_basis - self.a = amplitude - - # create weights - weights = np.zeros(num_basis,) # default f=0 - - # set the centre of the Gaussian basis functions to be spaced evenly - c = np.linspace(0, cs.T, num_basis + 1) # the '+1' is because it is rhythmic, c(0) = c(2pi) - c = c[:-1] - - # set concentration of basis function (this was found by trial and error by DeWolf) - h = np.ones(num_basis) * num_basis - - # create basis functions - basis = CBF(center=c, h=h) - super(RhythmicForcingTerm, self).__init__(weights, basis) - - def compute(self, s): - # call parent compute - f = super(RhythmicForcingTerm, self).compute(s) - # scale with amplitude and return it - return f * self.a - - def train(self, f_target, plot=False): - """Train the weights to match the given target forcing term - - Generate a set of weights over the basis functions such that the target forcing term trajectory is matched. - - Args: - f_target (np.array): the desired forcing term trajectory - plot (bool): If True, it will plot. - """ - - # calculate phase and basis functions - s_track = self.cs.rollout() - psi_track = self.psi(s_track) # shape=TxM - - # efficiently calculate BF weights using LWR (Locally Weighted (Linear) Regression) - for b in range(self.num_basis): - self.w[b] = (np.dot(psi_track[:, b], f_target) / (np.sum(psi_track[:, b]))) # + 1e-10)) - - if plot: - import matplotlib.pyplot as plt - # plot the basis function activations - plt.figure() - plt.subplot(211) - plt.plot(psi_track) - plt.title('basis functions') - - # plot the desired forcing function vs approx for the first dmp - plt.subplot(212) - plt.title('rhythmic force') - plt.plot(f_target, label='f_target', linewidth=2.5) - plt.plot(self.compute(s_track), label='f_pred', linewidth=2.5) - wps = self.weighted_basis(s_track) - plt.plot(wps, linewidth=0.5) - plt.legend() - plt.tight_layout() - plt.show() - - -# DMP # - -class DMP(object): - r"""Dynamic Movement Primitive - - Dynamic movement primitives (DMPs) are a set of differential equations (for each degree of freedoms (DoFs), i.e. - general coordinates) that encodes a movement [1]. It is thought that movement primitives are the building blocks - of a movement, and several evidences show that such modules exist in animals [2]. - - DMPs are often formulated as a 2nd-order differential equation: - - .. math:: \tau^2 \ddot{y} = \alpha ( \beta (g - y) - \dot{y}) + f(s) - - or sometimes, as a first-order differential system: - - .. math:: - - \tau \dot{z} &= \alpha ( \beta (g - y) - z) + f(s) \\ - \tau \dot{y} &= z - - They can also be rewritten as: - - .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} + f(s) - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` - is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, - velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. These equations are also - known as the transformation systems and represent, with the canonical system, DMPs. - - All of the above formulations are equivalent to each other. However, in my humble opinion, the last equation - depicts better what the transformation system constitutes; it is a unit-mass spring-damper system or PD controller - with a forcing term. This last term is non-linear and can be learned from the demonstrations. - If the forcing is zero, then the differential equation is stable, and the position :math:`y` converges to the goal. - The stiffness and damping coefficients (:math:`K` and :math:`D`) are often selected such that the whole system - (without the forcing term) is critically damped (:math:`D = 2 \sqrt{K}`). Other behaviors can be obtained by - selecting the stiffness and damping coefficient such that we obtain: - * an undamped system: :math:`D = 0` or :math:`K \rightarrow \infty` - * an underdamped system: :math:`D < 2 \sqrt{K}` - * a critically damped system: :math:`D = 2 \sqrt{K}` - * an overdamped system: :math:`D > 2 \sqrt{K}` - - Because the last formulation is more intuitive (at least for me), it will be used in this class. - Imitation is performed by learning the forcing term. - - DMPs can be categorized in two main categories: - * discrete DMP: used to represent discrete movements such as such as reaching, pushing/pulling, etc. - * rhythmic DMP: used to represent rhythmic movements such as walking, running dribbling, sewing, etc. - - DMP have the following nice properties: - * translation invariant - * linear parameters but still allows to represent non-linear movements - - Here are few limitations/shortcomings: - * hard to couple sensory information with it - * have to come up with the number of basis functions - - For a more biologically-inspired DMP [5] which allows to adapt the goal in real-time and a better rescaling, see - the `BioDMP` class. - - Note that this code was inspired by the `pydmps` code [2,3], but differ in several ways, notably: - - we undertake a more object-oriented programming (OOP) approach - - the equations are a little bit differents (e.g. :math:`tau`) in which we use the ones presented in the refs - - we decouple the Euler's method time step with the time step for the number of data points - - timesteps: we go from 0 to T included, while DeWolf goes from 0 to T-1 - - we use array operation instead of iterating over each element to update them - - we enforce consistency between the various methods and data structures - - we implement `BioDMP` which allows to adapt and rescale the goal in real-time based on [4] - - we implemented DMP sequencing based on [5] - - we implemented DMP that can be used with orientations based on [7] - - phase nodes which allows to couple phases, such as done in [8] for locomotion - - it can be used with RL algorithms, notably PoWER [9] and PI^2 [10] - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - [2] "Motor primitives in vertebrates and invertebrates", Flash et al., 2005 - [3] Tutorials on DMP: https://studywolf.wordpress.com/category/robotics/dynamic-movement-primitive/ - [4] PyDMPs (from DeWolf, 2013): https://github.com/studywolf/pydmps - [5] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation - and Obstacle Avoidance", Hoffmann et al., 2009 - [6] "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011 - [7] "Orientation in Cartesian Space Dynamic Movement Primitives", Ude et al., 2014 - [8] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 - [9] "Policy Search for Motor Primitives in Robotics", Kober et al., 2010 - [10] "A Generalized Path Integral Control Approach to Reinforcement Learning", Theodorou et al., 2010 - """ - - def __init__(self, canonical_system, forcing_term, y0=0, goal=1, stiffness=None, damping=None): - """Initialize the DMP. - - Args: - canonical_system (CS): canonical system which drives the DMP transformation system - forcing_terms (list): list of forcing terms (one forcing term for each DMP). Each forcing term can have - different number of basis functions. - y0 (float, float[M]): initial state of DMPs - goal (float, float[M]): goal state of DMPs - stiffness (float): stiffness term in the transformation system for DMPs - damping (float): damping term in the transformation system for DMPs - """ - - self.cs = canonical_system - - if isinstance(forcing_term, ForcingTerm): - forcing_term = [forcing_term] - elif isinstance(forcing_term, (list, tuple)): - for f in forcing_term: - if not isinstance(f, ForcingTerm): - raise TypeError("An item in the iterable is not an instance of ForcingTerm.") - else: - raise TypeError("Expecting forcing term to be an instance of ForcingTerm or a list/tuple of ForcingTerm") - - self.f = forcing_term - self.num_dmps = len(forcing_term) - self.dt = self.cs.dt - self.timesteps = self.cs.timesteps - - # check initial and goal positions # TODO use property to set them - if isinstance(y0, (int, float)): - y0 = np.ones(self.num_dmps) * y0 - if isinstance(y0, (list, tuple)): - y0 = np.array(y0) - self.y0 = y0 - self.dy0 = np.zeros(self.num_dmps) - self.ddy0 = np.zeros(self.num_dmps) - if isinstance(goal, (int, float)): - goal = np.ones(self.num_dmps) * goal - elif isinstance(goal, (list, tuple)): - goal = np.array(goal) - self.goal = goal - self._check_offset() - - # set stiffness and damping coefficient (if not specified, make them critically damped, i.e. D=2\sqrt{K}) - self.D = np.ones(self.num_dmps) * 25. if damping is None else damping - self.K = self.D**2 / 4. if stiffness is None else stiffness - - # set up the DMP system - self.prev_s = self.cs.init_phase - self.reset() - - # target forcing term (keep a copy) - self.f_target = None - - def __repr__(self): - return self.__class__.__name__ - - def __call__(self, *args, **kwargs): - return self.step(*args, **kwargs) - - ############## - # Properties # - ############## - - @property - def num_parameters(self): - """Return the total number of parameters""" - return np.array([force.w for force in self.f]).size - - ################## - # Static Methods # - ################## - - @staticmethod - def copy(other): - if not isinstance(other, DMP): - raise TypeError("Trying to copy an object which is not a DMP") - - @staticmethod - def is_parametric(): - """ - Return True as a DMP has weights that need to be optimized. - """ - return True - - @staticmethod - def is_linear(): - """ - Return True as a DMP is linear in terms of its weights (i.e. learnable parameters) - """ - return True - - @staticmethod - def is_recurrent(): - """ - Return False. - """ - return False - - @staticmethod - def is_probabilistic(): - """The DMP is a deterministic model""" - return False - - @staticmethod - def is_discriminative(): - """The DMP is a discriminative model which predicts the output :math:`y` given the input :math:`x`""" - return True - - @staticmethod - def is_generative(): - """The DMP is not a generative model""" - return False - - ########### - # Methods # - ########### - - def parameters(self): - """Returns an iterator over the model parameters.""" - for force in self.f: - yield force.w - - def named_parameters(self): - """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" - for force in self.f: - yield str(force), force.w - - def list_parameters(self): - """Return a list of parameters""" - return list(self.parameters()) - - def get_vectorized_parameters(self, to_numpy=True): - """Return a vectorized form (1 dimensional array) of the parameters.""" - parameters = self.parameters() - vector = np.concatenate([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): - """Set the vector parameters.""" - # 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.parameters(): - # size = parameter.nelement() - # parameter.data = vector[idx:idx+size].reshape(parameter.shape) - # idx += size - - # set the parameters from the vectorized one - idx = 0 - for force in self.f: - size = force.w.size - force.w = vector[idx:idx+size].reshape(force.w.shape) - idx += size - - def get_damping_ratio(self): - """ - Return the damping ratio :math:`\zeta = D / D_c` where :math:`D_c = 2 \sqrt{K}`. - - * if :math:`\zeta` = 0, the system is undamped (i.e. no damping) - * if :math:`\zeta` < 1, the system is underdamped (i.e. there will be some oscillations) - * if :math:`\zeta` = 1, the system is critically damped (i.e. return to equilibrium as fast as possible - without oscillating). - * if :math:`\zeta` > 1, the system is overdamped (i.e. the system returns to equilibrium without oscillating - but might be slow depending on the damping value). - """ - return self.D / (2*np.sqrt(self.K)) - - def _check_offset(self): - """Check to see if the initial position and goal are the same. If that is the case, offset slightly so that - the forcing term is not 0. Otherwise, look at the `BioDMP` class. - """ - self.goal[self.y0 == self.goal] += 1e-4 - - def get_scaling_term(self, new_goal=None): - # this is overridden by the child classes - return np.ones(self.num_dmps) - - def _generate_goal(self, y_des): - raise NotImplementedError() - - def reset(self): - """Reset the transformation and canonical systems""" - self.y = self.y0.copy() - self.dy = self.dy0.copy() # np.zeros(self.num_dmps) - self.ddy = self.ddy0.copy() - self.prev_s = self.cs.reset() - - def step(self, s=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, external_force=None, rescale_force=True): - """Run the DMP transformation system for a single time step. - - Args: - s (None, float): the phase value. If None, it will use the canonical system. - tau (float): Increase tau to make the system slower, and decrease it to make it faster - error (float): optional system feedback - forcing_term (float[M]): if given, it will replace the forcing term (where `M` = number of DMPs) - new_goal (float[M]): new goal (where `M` = number of DMPs) - rescale_force (bool): if the given forcing term should be rescaled. - """ - - # system feedback - error_coupling = 1.0 / (1.0 + error) - - # get phase from canonical system - if s is None: - s = self.cs.step(tau=tau, error_coupling=error_coupling) - elif not isinstance(s, (float, int)): - raise TypeError("Expecting the phase 's' to be a float or integer. Instead, I got {}".format(type(s))) - - # check if same phase as before - if s == self.prev_s: - return self.y, self.dy, self.ddy - - if new_goal is None: - new_goal = self.goal - - # save previous position and velocity - prev_y, prev_dy = self.y.copy(), self.dy.copy() - - # compute scaling factor for the forcing term - scaling = self.get_scaling_term(new_goal) - - # for each DMP, solve transformation system equation using Euler's method - for d in range(self.num_dmps): - - # compute forcing term - if forcing_term is None: - # f = self.f[d](s) * scaling[d] - f = self.f_gen(s) * scaling[d] - else: - if rescale_force: - f = forcing_term[d] * scaling[d] - else: - f = forcing_term[d] - - # DMP acceleration - self.ddy[d] = self.K[d]/(tau**2) * (new_goal[d] - self.y[d]) - self.D[d]/tau * self.dy[d] + f/(tau**2) - if external_force is not None: - self.ddy[d] += external_force[d] - self.dy[d] += self.ddy[d] / tau * self.dt * error_coupling - self.y[d] += self.dy[d] * self.dt * error_coupling - - # return self.y, self.dy, self.ddy - return prev_y, prev_dy, self.ddy - - def rollout(self, timesteps=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, rescale_force=True, - **kwargs): - """Generate position, velocity, and acceleration trajectories, no feedback is incorporated. - - Args: - tau (float): Increase tau to make the system slower, and decrease it to make it faster - timesteps (None, int): the number of steps to perform - error (float): optional system feedback - forcing_term (np.ndarray): if given, it will replace the forcing term (shape [num_dmps, timesteps]) - new_goal (np.ndarray): new goal (of shape [num_dmps,]) - - Returns: - float[M,T]: y (position) trajectories - float[M,T]: dy (velocity) trajectories - float[M,T]: ddy (acceleration) trajectories - """ - - # reset the canonical and transformation systems - self.reset() - - if timesteps is None: - timesteps = int(self.timesteps * tau) - - # set up tracking vectors - y_track = np.zeros((self.num_dmps, timesteps)) - dy_track = np.zeros((self.num_dmps, timesteps)) - ddy_track = np.zeros((self.num_dmps, timesteps)) - - # for the other timesteps, solve DMP equation using Euler's method - for t in range(timesteps): - if forcing_term is None: - y, dy, ddy = self.step(tau=tau, error=error, new_goal=new_goal, external_force=None) - else: - y, dy, ddy = self.step(tau=tau, error=error, forcing_term=forcing_term[:, t], new_goal=new_goal, - rescale_force=rescale_force) - - # record timestep - y_track[:, t] = y - dy_track[:, t] = dy - ddy_track[:, t] = ddy - - return y_track, dy_track, ddy_track - - def train(self, f_target): - """Train the forcing terms.""" - # train each forcing term - if f_target.shape[0] != len(self.f): - raise ValueError("Mismatch between the number of forcing terms") - - # train each forcing term - for forcing_term, target in zip(self.f, f_target): - forcing_term.train(target) - - def imitate(self, y_des, dy_des=None, ddy_des=None, interpolation='cubic', plot=False): - """Imitate a desired trajectory, and learn the parameters that best realizes it. - - Args: - y_des (np.array): the desired position trajectories of each DMP with shape [num_dmps, timesteps] - dy_des (np.array): the desired velocities with shape [num_dmps, timesteps] - ddy_des (np.array): the desired accelerations with shape [num_dmps, timesteps] - interpolation (str): how to interpolate the data. Select between 'linear', 'cubic', and 'hermite'. - """ - - # set initial state and goal - if y_des.ndim == 1: - y_des = y_des.reshape(1, len(y_des)) - self.y0 = y_des[:, 0].copy() - self.goal = self._generate_goal(y_des) - self._check_offset() - - timesteps = y_des.shape[1] - - def interpolate(x, dt, period, timesteps, new_timesteps, interpolation=interpolation, return_gen=False): - # generate function to interpolate the desired trajectory - t = np.linspace(0, period, timesteps) - if interpolation == 'linear': # use linear interpolation - path_gen = scipy.interpolate.interp1d(t, x, axis=-1) - elif interpolation == 'cubic': # use cubic spline interpolation - path_gen = scipy.interpolate.CubicSpline(t, x, axis=-1) - else: # TODO: implement hermite (see utils.interpolator.hermite) - raise ValueError("The requested interpolation has not been implemented. Select between 'linear' or " - "'cubic'") - if return_gen: - return path_gen - return path_gen([t * self.dt for t in range(new_timesteps)]) - - y_des = interpolate(y_des, self.dt, self.cs.T, timesteps, self.timesteps, interpolation=interpolation) - - # compute desired velocity if necessary - if dy_des is None: - # calculate velocity of y_des - dy_des = np.diff(y_des) / self.dt - # add zero to the beginning of every row - print(dy_des.shape) - dy_des = np.hstack((np.zeros((self.num_dmps, 1)), dy_des)) - else: - if dy_des.ndim == 1: - dy_des = dy_des.reshape(1, len(dy_des)) - dy_des = interpolate(dy_des, self.dt, self.cs.T, dy_des.shape[1], self.timesteps, - interpolation=interpolation) - self.dy0 = dy_des[:, 0].copy() - - # compute desired acceleration if necessary - if ddy_des is None: - # calculate acceleration of y_des - ddy_des = np.diff(dy_des) / self.dt - # add zero to the beginning of every row - ddy_des = np.hstack((np.zeros((self.num_dmps, 1)), ddy_des)) - else: - if ddy_des.ndim == 1: - ddy_des = ddy_des.reshape(1, len(ddy_des)) - ddy_des = interpolate(ddy_des, self.dt, self.cs.T, ddy_des.shape[1], self.timesteps, - interpolation=interpolation) - self.ddy0 = ddy_des[:, 0].copy() - - # find the force required to move along this trajectory (with shape [num_dmps, timesteps]) - f_target = ddy_des - self.K.reshape(-1, 1) * (self.goal.reshape(-1,1) - y_des) + self.D.reshape(-1, 1) * dy_des - - # plot - if plot: - import matplotlib.pyplot as plt - plt.figure() - plt.plot(y_des[0], 'b', label='pos') - plt.plot(dy_des[0], 'g', label='vel') - plt.plot(ddy_des[0], 'r', label='acc') - plt.plot(f_target[0], 'k', label='force') - plt.legend() - plt.show() - - self.f_gen = interpolate(f_target, self.dt, self.cs.T, ddy_des.shape[1], 2000, - interpolation=interpolation, return_gen=True) - - # efficiently generate weights to realize f_target - self.f_target = f_target - self.train(f_target) - - # reset the canonical and transformation systems - self.reset() - - return y_des - - def get_forcing_term(self, s): - """ - Get the forcing terms based on the given phase value. - - Args: - s (float, float[T]): phase value(s) - - Returns: - float[M], float[M,T]: forcing terms - """ - return np.array([self.f[d](s) for d in range(self.num_dmps)]) - - def generate_goal(self, y0=None, dy0=None, ddy0=None, f0=None): - """ - Generate the goal from the initial positions, velocities, accelerations, and forces. - - Args: - y0 (float[M], None): initial positions. If None, it will take the default initial positions. - dy0 (float[M], None): initial velocities. If None, it will take the default initial velocities. - ddy0 (float[M], None): initial accelerations. If None, it will take the default initial accerelations. - f0 (float[M], None): initial forcing terms. If None, it will compute it based on the learned weights. - You can also give `dmp.f_target[:,0]` to get the correct goal. - - Returns: - float[M]: goal position for each DMP. - """ - if y0 is None: - y0 = self.y0 - if dy0 is None: - dy0 = self.dy0 - if ddy0 is None: - ddy0 = self.ddy0 - if f0 is None: - s0 = self.cs.init_phase - f0 = self.get_forcing_term(s0) - - return 1/self.K * (ddy0 + self.D * dy0 + self.K * y0 - f0) - - def sequence(self, model, mode=0): - """ - Define how to sequence with another DMP model. - - Args: - model (DMP): DMP model - mode (int): specifies how to sequence the two DMP models. - - Returns: - DMP: the sequenced model - - References: - [1] "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011 - """ - if not isinstance(model, DMP): - raise TypeError("The given model is not an instance of DMP.") - pass - - # def __rshift__(self, other): - # """ - # Sequence DMP model with another learning model. - # - # Ref: "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011 - # - # :param other: another DMP model - # :return: - # """ - # # If we sequence two DMP models - # if isinstance(other, DMP): - # - # else: - # # if it is another model, call the parent's method which knows how to sequence different models - # super(DMP, self).__rshift__(other) - - -class DiscreteDMP(DMP): - r"""Discrete Dynamic Movement Primitive - - Discrete DMPs have the same mathematical formulation as general DMPs, which is given by: - - .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} + f(s) (g - y0) - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` - is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, - velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. - - However, the forcing term in the case of discrete DMPs is given by: - - .. math:: f(s) = \frac{\sum_i \psi_i(s) w_i}{\sum_i \psi_i(s)} s - - where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the - given input phase variable :math:`s`, :math:`g` is the goal, and :math:`y_0` is the initial position. Note that - as the phase converges to 0, the forcing term also converges to that value. - - The basis functions (in the discrete case) are given by: - - .. math:: \psi_i(s) = \exp \left( - \frac{1}{2 \sigma_i^2} (x - c_i)^2 \right) - - where :math:`c_i` is the center of the basis function :math:`i`, and :math:`\sigma_i` is its width. - - Also, the canonical system associated with this transformation system is given by: - - .. math:: \tau \dot{s} = - \alpha_s s - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, :math:`s` is the phase - variable that drives the DMP, and :math:`\alpha_s` is a predefined constant. - - All these differential equations are solved using Euler's method. - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - """ - - def __init__(self, num_dmps, num_basis, dt=0.01, y0=0, goal=1, - forcing_terms=None, stiffness=None, damping=None): - """Initialize the discrete DMP - - Args: - num_dmps (int): number of DMPs - num_basis (int, int[M]): number of basis functions, or list of number of basis functions. - dt (float): step integration for Euler's method - y0 (float, float[M]): initial position(s) - goal (float, float[M]): goal(s) - forcing_terms (list, ForcingTerm): the forcing terms (which can have different basis functions) - stiffness (float): stiffness coefficient - damping (float): damping coefficient - """ - - # create discrete canonical system - cs = DiscreteCS(dt=dt) - - # create forcing terms (each one contains the basis functions and learnable weights) - if forcing_terms is None: - if isinstance(num_basis, int): - forcing_terms = [DiscreteForcingTerm(cs, num_basis) for _ in range(num_dmps)] - else: - if not isinstance(num_basis, (np.ndarray, list, tuple, set)): - raise TypeError("Expecting 'num_basis' to be an int, list, tuple, np.array or set.") - if len(num_basis) != num_dmps: - raise ValueError("The length of th list of number of basis doesn't match the number of DMPs") - forcing_terms = [DiscreteForcingTerm(cs, n_basis) for n_basis in num_basis] - - # call super class constructor - super(DiscreteDMP, self).__init__(canonical_system=cs, forcing_term=forcing_terms, y0=y0, goal=goal, - stiffness=stiffness, damping=damping) - - def get_scaling_term(self, new_goal=None): - """ - Return the scaling term for the forcing term. - - Args: - new_goal (float, float[M], None): the new goal position. If None, it will be the current goal. - - Returns: - float, float[M]: scaling term - """ - if new_goal is None: - new_goal = self.goal - return (new_goal - self.y0) / (self.goal - self.y0) - - def _generate_goal(self, y_des): - """Generate the goal for path imitation. - - Args: - y_des (np.array): the desired trajectory to follow with shape [num_dmps, timesteps] - - Returns: - float[M]: goal position - """ - return np.copy(y_des[:, -1]) - - -class RhythmicDMP(DMP): - r"""Rhythmic Dynamic Movement Primitive - - Rhythmic DMPs have the same mathematical formulation as general DMPs, which is given by: - - .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} + f(s) - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` - is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, - velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. - - However, the forcing term in the case of rhythmic DMPs is given by: - - .. math:: f(s) = \frac{\sum_i \psi_i(s) w_i}{\sum_i \psi_i(s)} a - - where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the - given input phase variable :math:`s`, and :math:`a` is the amplitude. - - The basis functions (in the rhythmic case) are given by: - - .. math:: \psi_i(s) = \exp \left( - h_i (\cos(s - c_i) - 1) \right) - - where :math:`c_i` is the center of the basis, and :math:`h_i` is a measure of concentration. - - Also, the canonical system associated with this transformation system is given by: - - .. math:: \tau \dot{s} = 1 - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, and :math:`s` is the - phase variable that drives the DMP. - - All these differential equations are solved using Euler's method. - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - """ - - def __init__(self, num_dmps, num_basis, dt=0.01, y0=0, goal=1, - forcing_terms=None, stiffness=None, damping=None): - """Initialize the rhythmic DMP - - Args: - num_dmps (int): number of DMPs - num_basis (int): number of basis functions - dt (float): step integration for Euler's method - y0 (float, np.array): initial position(s) - goal (float, np.array): goal(s) - forcing_terms (list, ForcingTerm): the forcing terms (which can have different basis functions) - stiffness (float): stiffness coefficient - damping (float): damping coefficient - """ - - # create rhythmic canonical system - cs = DiscreteCS(dt=dt) - - # create forcing terms (each one contains the basis functions and learnable weights) - if forcing_terms is None: - if isinstance(num_basis, int): - forcing_terms = [RhythmicForcingTerm(cs, num_basis) for _ in range(num_dmps)] - else: - if not isinstance(num_basis, (np.ndarray, list, tuple, set)): - raise TypeError("Expecting 'num_basis' to be an int, list, tuple, np.array or set.") - if len(num_basis) != num_dmps: - raise ValueError("The length of th list of number of basis doesn't match the number of DMPs") - forcing_terms = [RhythmicForcingTerm(cs, n_basis) for n_basis in num_basis] - - # call super class constructor - super(RhythmicDMP, self).__init__(canonical_system=cs, forcing_term=forcing_terms, y0=y0, goal=goal, - stiffness=stiffness, damping=damping) - - def get_scaling_term(self, new_goal=None): - """ - Return the scaling term for the forcing term. For rhythmic DMPs it's non-diminishing, so this function just - returns 1. - """ - return np.ones(self.num_dmps) - - def _generate_goal(self, y_des): - """Generate the goal for path imitation. - - For rhythmic DMPs, the goal is the average of the desired trajectory. - - Args: - y_des (float[M,T]): the desired trajectory to follow (with shape [num_dmps, timesteps]) - - Returns: - float[M]: goal positions (one for each DMP) - """ - goal = np.zeros(self.num_dmps) - for n in range(self.num_dmps): - num_idx = ~np.isnan(y_des[n]) # ignore nan's when calculating goal - goal[n] = .5 * (y_des[n, num_idx].min() + y_des[n, num_idx].max()) - return goal - - -class BioDiscreteDMP(DiscreteDMP): - r"""Biologically-inspired Discrete DMPs - - One of the main problems with the initial DMP formulation is when some goal coordinates coincide with their - corresponding initial position coordinates, it results in an inappropriate rescaling when displacing a little bit - the goal. - - To deal with this problem, a new formulation of the transformation system was proposed in [2] and is given by: - - .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} - K(g - y_0)s + K f(s) - - where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` - is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, - velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. - - The forcing term is expressed as: - - .. math:: f(s) = \frac{\sum_i \psi_i(s) w_i}{ \sum_j \psi_j(s)} s - - Properties (from [2]): - * Invariant under affine transformation - * Movement generalization to new targets - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - [2] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation - and Obstacle Avoidance", Hoffmann et al., 2009 - [3] "Learning and Generalization of Motor Skills by Learning from Demonstration", Pastor et al., 2009 - """ - - def __init__(self, num_dmps, num_basis, dt=0.01, y0=0, goal=1, - forcing_terms=None, stiffness=None, damping=None): - """Initialize the discrete DMP - - Args: - num_dmps (int): number of DMPs - num_basis (int): number of basis functions - dt (float): step integration for Euler's method - y0 (float, np.array): initial position(s) - goal (float, np.array): goal(s) - forcing_terms (list, ForcingTerm): the forcing terms (which can have different basis functions) - stiffness (float): stiffness coefficient - damping (float): damping coefficient - """ - # if stiffness is None and damping is None: - # # from paper [2] - # stiffness = 150 * np.ones(num_dmps) - # damping = 2 * np.sqrt(stiffness) - - self.cst = 0.75 # this depends on the K and D value - - super(BioDiscreteDMP, self).__init__(num_dmps, num_basis, dt=dt, y0=y0, goal=goal, - forcing_terms=forcing_terms, stiffness=stiffness, damping=damping) - - def step(self, s=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, external_force=None): - """Run the DMP transformation system for a single time step. - - Args: - s (None, float): the phase value. If None, it will use the canonical system. - tau (float): Increase tau to make the system slower, and decrease it to make it faster - error (float): optional system feedback - forcing_term (np.ndarray): if given, it will replace the forcing term (shape [dmp,]) - new_goal (np.ndarray): new goal (of shape [num_dmps,]) - """ - - # system feedback - error_coupling = 1.0 / (1.0 + error) - - # get phase from canonical system - if s is None: - s = self.cs.step(tau=tau, error_coupling=error_coupling) - elif not isinstance(s, (float, int)): - raise TypeError("Expecting the phase 's' to be a float or integer. Instead, I got {}".format(type(s))) - - # check if same phase as before - if s == self.prev_s: - return self.y, self.dy, self.ddy - - if new_goal is None: - new_goal = self.goal - else: - new_goal = new_goal + self.cst * (new_goal - self.goal) - - # save previous position and velocity - prev_y, prev_dy = self.y.copy(), self.dy.copy() - - # for each DMP, solve transformation system equation using Euler's method - for d in range(self.num_dmps): - - # compute forcing term - if forcing_term is None: - f = self.f[d](s) + self.K[d] * s * (self.goal[d] - new_goal[d]) - else: - f = forcing_term[d] - - # DMP acceleration - self.ddy[d] = self.K[d]/(tau**2) * (new_goal[d] - self.y[d]) - self.D[d]/tau * self.dy[d] + f/(tau**2) - if external_force is not None: - self.ddy[d] += external_force[d] - self.dy[d] += self.ddy[d] / tau * self.dt * error_coupling - self.y[d] += self.dy[d] * self.dt * error_coupling - - # return self.y, self.dy, self.ddy - return prev_y, prev_dy, self.ddy - - def _check_offset(self): - """No need to check for an offset with this class""" - pass - - def generate_goal(self, y0=None, dy0=None, ddy0=None, f0=None): - """ - Generate the goal from the initial positions, velocities, accelerations, and forces. - - Args: - y0 (float[M], None): initial positions. If None, it will take the default initial positions. - dy0 (float[M], None): initial velocities. If None, it will take the default initial velocities. - ddy0 (float[M], None): initial accelerations. If None, it will take the default initial accerelations. - f0 (float[M], None): initial forcing terms. If None, it will compute it based on the learned weights. - You can also give `dmp.f_target[:,0]` to get the correct goal. - - Returns: - float[M]: goal position for each DMP. - """ - if y0 is None: - y0 = self.y0 - if dy0 is None: - dy0 = self.dy0 - if ddy0 is None: - ddy0 = self.ddy0 - if f0 is None: - s0 = self.cs.init_phase - f0 = self.get_forcing_term(s0) - - return 1/self.K * (ddy0 + self.D * dy0 + self.K * y0 - self.K * f0) - - -# Methods # -# TODO: check other methods in my various files - -def obstacle_avoidance(y, dy, goal, obstacles): - """Obstacle avoidance using formulation in [1,2]. - - This is taken from [3] and generalized to 3D. This returns the coupling term. - - .. math:: p(y,dy) = \gamma R dy \phi \exp(-\beta \phi) - - Args: - y (np.ndarray): position at one time step - dy (np.ndarray): velocity at one time step - goal (np.ndarray): goal position - obstacles (list on np.ndarray): list of obstacle positions - - Returns: - np.array: coupling term p(y,dy) - - References: - [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 - [2] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation - and Obstacle Avoidance", Hoffmann et al., 2009 - [3] PyDMPs, DeWolf, 2013: https://github.com/studywolf/pydmps/blob/master/examples/avoid_obstacles.py - """ - # TODO: generalize to 3D - # TODO: improve it by considering moving obstacles (see paper [2]) - - # define few variables - beta = 20.0 / np.pi - gamma = 100. - - # init coupling term - p = np.zeros(len(y)) - - for obstacle in obstacles: - # if we're moving - if np.linalg.norm(dy) > 1e-5: - - # get the angle we're heading in - phi_dy = -np.arctan2(dy[1], dy[0]) - R_dy = np.array([[np.cos(phi_dy), -np.sin(phi_dy)], - [np.sin(phi_dy), np.cos(phi_dy)]]) - # calculate vector to object relative to body - obj_vec = obstacle - y - # rotate it by the direction we're going - obj_vec = np.dot(R_dy, obj_vec) - # calculate the angle of obj relative to the direction we're going - phi = np.arctan2(obj_vec[1], obj_vec[0]) - - dphi = gamma * phi * np.exp(-beta * abs(phi)) - R = np.dot(R_halfpi, np.outer(obstacle - y, dy)) - pval = -np.nan_to_num(np.dot(R, dy) * dphi) - - # check to see if the distance to the obstacle is further than - # the distance to the target, if it is, ignore the obstacle - if np.linalg.norm(obj_vec) > np.linalg.norm(goal - y): - pval = 0 - - p += pval - - return p - - -def automatic_num_basis(ydes, dmpClass, init_guess=10, tolerance=0.0001, debug=True, plot=False): - """ - Automatic discovery of the 'optimal' number of basis functions. - - Args: - ydes (float[M,T]): desired position trajectory. `M` is the number of basis functions, and `T` is the length - of the trajectory. - init_guess (int): initial guess for the number of basis function - tolerance (float): acceptable tolerance for the MSE between the desired and predicted position trajectory. - - Returns: - int: optimal number of basis functions to use. - """ - ydes = np.array(ydes) - prev_mse = np.sum(ydes ** 2) - prev_num_basis = int(init_guess / 2) - num_basis = init_guess - num_dmps = ydes.shape[0] - - mses, bfs = [], [] - - while True: - # Evaluate the DMP with the specified nb of basis fcts - dmp = dmpClass(num_dmps=num_dmps, num_basis=num_basis) - dmp.imitate(y_des=ydes) - y = dmp.rollout(tau=1.0) - mse = np.sum((ydes - y)**2) - mses.append(mse) - bfs.append(num_basis) - - if debug: - print("Num basis: {} - MSE: {}".format(num_basis, mse)) - - num_basis_tmp = num_basis - if prev_num_basis < num_basis: - if mse < prev_mse: - # can maybe still improve the MSE by increasing 2 times the number of basis functions - num_basis += 2 * (num_basis - prev_num_basis) - else: # mse >= prev_mse - # we jumped too much forward, backtrack from half - num_basis -= int((num_basis - prev_num_basis) / 2) - elif prev_num_basis > num_basis: - if mse <= prev_mse: - # can maybe still improve the MSE by decreasing by half the number of basis functions - num_basis -= int((prev_num_basis - num_basis) / 2) - else: # mse > prev_mse - if abs(prev_mse - mse) < tolerance: - break - # otherwise, we jumped too much backward, backtrack from half - num_basis += int((prev_num_basis - num_basis) / 2) - else: - break - - prev_num_basis = num_basis_tmp - prev_mse = mse - - if debug: - print("Best number of basis: {} - with MSE: {}".format(num_basis, mse)) - - if plot: - plt.subplot(1, 2, 1) - plt.title('Number of basis') - plt.plot(bfs) - plt.subplot(1, 2, 2) - plt.title('MSE') - plt.subplot(1, 2, 2) - plt.show() - - return num_basis - - -# Tests -if __name__ == '__main__': - import matplotlib.pyplot as plt - - # tests canonical systems - discrete_cs = DiscreteCS() - rhythmic_cs = RhythmicCS() - - # check tau - plt.subplot(1, 2, 1) - plt.title('Discrete CS') - for tau in [1., 0.5, 2.]: - rollout = discrete_cs.rollout(tau=tau) - plt.plot(np.linspace(0, 1., len(rollout)), rollout, label='tau='+str(tau)) - plt.legend() - plt.subplot(1, 2, 2) - plt.title('Rhythmic CS') - for tau in [1., 0.5, 2.]: - rollout = rhythmic_cs.rollout(tau=tau) - plt.plot(np.linspace(0, 1., len(rollout)), rollout, label='tau='+str(tau)) - plt.legend() - plt.show() - - # tests basis functions - num_basis = 20 - discrete_f = DiscreteForcingTerm(discrete_cs, num_basis) - rhythmic_f = RhythmicForcingTerm(rhythmic_cs, num_basis) - plt.subplot(1, 2, 1) - rollout = discrete_cs.rollout() - plt.title('discrete basis fcts') - plt.plot(rollout, discrete_f.psi(rollout)) - plt.subplot(1, 2, 2) - rollout = rhythmic_cs.rollout() - plt.title('rhythmic basis fcts') - plt.plot(rollout, rhythmic_f.psi(rollout)) - plt.show() - - # tests forcing terms - f = np.sin(np.linspace(0, 2*np.pi, 100)) - discrete_f.train(f, plot=True) - f = np.sin(np.linspace(0, 2*np.pi, int(2*np.pi*100))) - rhythmic_f.train(f, plot=True) - - # Test discrete DMP - discrete_dmp = DiscreteDMP(num_dmps=1, num_basis=num_basis) - t = np.linspace(-6, 6, 100) - y_target = 1 / (1 + np.exp(-t)) - discrete_dmp.imitate(y_target) - y, dy, ddy = discrete_dmp.rollout() - - plt.plot(y_target, label='y_target') - plt.plot(y[0], label='y_pred') - # plt.plot(dy[0]) - # plt.plot(ddy[0]) - y, dy, ddy = discrete_dmp.rollout(new_goal=np.array([2.])) - plt.plot(y[0], label='y_scaled') - plt.title('Discrete DMP') - plt.legend() - plt.show() - - # Test Biologically-inspired DMP - t = np.linspace(0., 1., 100) - y_d = np.sin(np.pi * t) - new_goal = np.array([[0.8, -0.25], - [0.8, 0.25], - [1.2, -0.25]]) - - discrete_dmp = DiscreteDMP(num_dmps=2, num_basis=100) - discrete_dmp.imitate(np.array([t, y_d])) - y, dy, ddy = discrete_dmp.rollout() - init_points = np.array([discrete_dmp.y0, discrete_dmp.goal]) - # print(discrete_dmp.generate_goal()) - # print(discrete_dmp.generate_goal(f0=discrete_dmp.f_target[:,0])) - - # check with standard discrete DMP when rescaling the goal - plt.subplot(1, 3, 1) - plt.title('Initial discrete DMP') - plt.scatter(init_points[:,0], init_points[:,1], color='b') - plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') - plt.plot(y[0], y[1], 'b', label='original') - - plt.subplot(1, 3, 2) - plt.title('Rescaled discrete DMP') - plt.scatter(init_points[:,0], init_points[:,1], color='b') - plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') - plt.plot(y[0], y[1], 'b', label='original') - for g in new_goal: - y, dy, ddy = discrete_dmp.rollout(new_goal=g) - plt.plot(y[0], y[1], 'g', label='scaled') - plt.legend(['original', 'scaled']) - - # change goal with biologically-inspired DMP - new_goal = np.array([[0.8, -0.25], - [0.8, 0.25], - [0.4, 0.1], - [5., 0.15], - [1.2, -0.25], - [-0.8, 0.1], - [-0.8, -0.25], - [5., -0.25]]) - bio_dmp = BioDiscreteDMP(num_dmps=2, num_basis=100) - bio_dmp.imitate(np.array([t, y_d])) - y, dy, ddy = bio_dmp.rollout() - init_points = np.array([bio_dmp.y0, bio_dmp.goal]) - - plt.subplot(1, 3, 3) - plt.title('Biologically-inspired DMP') - plt.scatter(init_points[:, 0], init_points[:, 1], color='b') - plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') - plt.plot(y[0], y[1], 'b', label='original') - for g in new_goal: - y, dy, ddy = bio_dmp.rollout(new_goal=g) - plt.plot(y[0], y[1], 'g', label='scaled') - plt.legend(['original', 'scaled']) - plt.show() - - # changing goal at the middle - y_list = [] - for g in new_goal: - bio_dmp.reset() - y_traj = np.zeros((2, 100)) - for t in range(100): - if t < 30: - y, dy, ddy = bio_dmp.step() - else: - y, dy, ddy = bio_dmp.step(new_goal=g) - y_traj[:, t] = y - y_list.append(y_traj) - for y in y_list: - plt.plot(y[0], y[1]) - plt.scatter(bio_dmp.y0[0], bio_dmp.y0[1], color='b') - plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') - plt.title('change goal at the middle') - plt.show() - - # changing goal at the middle but with a moving goal - g = np.hstack((np.arange(1.0, 2.0, 0.1).reshape(10, -1), - np.arange(0.0, 1.0, 0.1).reshape(10, -1))) - - bio_dmp.reset() - y_traj = np.zeros((2, 100)) - y_list = [] - for t in range(100): - y, dy, ddy = bio_dmp.step(new_goal=g[int(t/10)]) - y_traj[:, t] = y - if (t % 10) == 0: - y_list.append(y) - y_list = np.array(y_list) - - plt.plot(y_traj[0], y_traj[1]) - plt.scatter(bio_dmp.y0[0], bio_dmp.y0[1], color='b') - plt.scatter(g[:, 0], g[:, 1], color='r') - plt.scatter(y_list[:, 0], y_list[:, 1], color='g') - plt.title('moving goal') - plt.show() diff --git a/pyrobolearn/models/dmp/__init__.py b/pyrobolearn/models/dmp/__init__.py new file mode 100644 index 0000000..e2c2235 --- /dev/null +++ b/pyrobolearn/models/dmp/__init__.py @@ -0,0 +1,15 @@ + +# import canonical systems +from .canonical_systems import * + +# import basis functions +from .basis_functions import * + +# import forcing terms +from .forcing_terms import * + +# import dynamic movement primitives +from .dmp import * +from .discrete_dmp import * +from .rhythmic_dmp import * +from .biodiscrete_dmp import * diff --git a/pyrobolearn/models/dmp/basis_functions.py b/pyrobolearn/models/dmp/basis_functions.py new file mode 100644 index 0000000..9cf9735 --- /dev/null +++ b/pyrobolearn/models/dmp/basis_functions.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python +"""Define basis functions used in the forcing terms in dynamic movement primitives + +This file implements basis functions used for discrete and rhythmic dynamic movement primitives. +""" + +from abc import ABCMeta, abstractmethod +import numpy as np +import scipy.interpolate + + +__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 BF(object): + r"""Basis function used in the forcing terms + """ + __metaclass__ = ABCMeta + + def __init__(self): + pass + + @abstractmethod + def compute(self, s): + raise NotImplementedError + + # alias + def __call__(self, s): + return self.compute(s) + + +class EBF(BF): + r"""Exponential basis function + + This basis function is given by the formula: + + .. math:: \psi(s) = \exp \left( - \frac{1}{2 \sigma^2} (s - c)^2 \right) + + where :math:`c` is the center, and :math:`\sigma` is the width of a normal distribution. + + This is often used for discrete DMPs. + """ + def __init__(self, center=0, sigma=1., h=None): + """Initialize basis function + + Args: + center (float, np.ndarray): center of the distribution + sigma (float, np.ndarray): width of the distribution + h (float, np.ndarray): concentration/precision of the basis fct (h = 1/(2*\sigma^2)). + if h is not provided, it will check sigma. + """ + super(EBF, self).__init__() + + if isinstance(center, np.ndarray): pass + + self.c = center + if h is None: + self.h = 1. / (2*sigma**2) # measure the concentration + else: + self.h = h + + def compute(self, s): + if isinstance(s, np.ndarray): + s = s[:, None] + return np.exp(-self.h * (s - self.c)**2) + + +class CBF(BF): + r"""Circular basis function (aka von Mises basis function) + + This basis function is given by the formula: + + .. math:: \psi(s) = \exp \left( h (\cos(s - c) - 1) \right) + + where :math:`c` is the center, and :math:`h` is a measure of concentration. + + This is often used for rhythmic DMPs. + """ + def __init__(self, center=0, h=1.): + """Initialize basis function + + Args: + center (float, np.ndarray): center of the basis fct + h (float, np.ndarray): concentration/precision of the basis fct + """ + super(CBF, self).__init__() + self.c = center + self.h = h + + def compute(self, s): + if isinstance(s, np.ndarray): + s = s[:, None] + # return np.exp(self.h * np.cos(s - self.c) - 1) # this is bad as it is not bounded as we increase the + # number of basis functions. + return np.exp(self.h * np.cos(s - self.c) - self.h) diff --git a/pyrobolearn/models/dmp/biodiscrete_dmp.py b/pyrobolearn/models/dmp/biodiscrete_dmp.py new file mode 100644 index 0000000..9996607 --- /dev/null +++ b/pyrobolearn/models/dmp/biodiscrete_dmp.py @@ -0,0 +1,262 @@ +#!/usr/bin/env python +"""Define the biologically-inspired discrete dynamic movement primitive (as described in [1,2]) + +References: + [1] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation + and Obstacle Avoidance", Hoffmann et al., 2009 + [2] "Learning and Generalization of Motor Skills by Learning from Demonstration", Pastor et al., 2009 +""" + +import numpy as np + +from pyrobolearn.models.dmp.discrete_dmp import DiscreteDMP + +__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 BioDiscreteDMP(DiscreteDMP): + r"""Biologically-inspired Discrete DMPs + + One of the main problems with the initial DMP formulation is when some goal coordinates coincide with their + corresponding initial position coordinates, it results in an inappropriate rescaling when displacing a little bit + the goal. + + To deal with this problem, a new formulation of the transformation system was proposed in [2] and is given by: + + .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} - K(g - y_0)s + K f(s) + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` + is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, + velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. + + The forcing term is expressed as: + + .. math:: f(s) = \frac{\sum_i \psi_i(s) w_i}{ \sum_j \psi_j(s)} s + + Properties (from [2]): + * Invariant under affine transformation + * Movement generalization to new targets + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + [2] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation + and Obstacle Avoidance", Hoffmann et al., 2009 + [3] "Learning and Generalization of Motor Skills by Learning from Demonstration", Pastor et al., 2009 + """ + + def __init__(self, num_dmps, num_basis, dt=0.01, y0=0, goal=1, + forcing_terms=None, stiffness=None, damping=None): + """Initialize the discrete DMP + + Args: + num_dmps (int): number of DMPs + num_basis (int): number of basis functions + dt (float): step integration for Euler's method + y0 (float, np.array): initial position(s) + goal (float, np.array): goal(s) + forcing_terms (list, ForcingTerm): the forcing terms (which can have different basis functions) + stiffness (float): stiffness coefficient + damping (float): damping coefficient + """ + # if stiffness is None and damping is None: + # # from paper [2] + # stiffness = 150 * np.ones(num_dmps) + # damping = 2 * np.sqrt(stiffness) + + self.cst = 0.75 # this depends on the K and D value + + super(BioDiscreteDMP, self).__init__(num_dmps, num_basis, dt=dt, y0=y0, goal=goal, + forcing_terms=forcing_terms, stiffness=stiffness, damping=damping) + + def step(self, s=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, external_force=None, + rescale_force=True): + """Run the DMP transformation system for a single time step. + + Args: + s (None, float): the phase value. If None, it will use the canonical system. + tau (float): Increase tau to make the system slower, and decrease it to make it faster + error (float): optional system feedback + forcing_term (np.ndarray): if given, it will replace the forcing term (shape [dmp,]) + new_goal (np.ndarray): new goal (of shape [num_dmps,]) + """ + + # system feedback + error_coupling = 1.0 / (1.0 + error) + + # get phase from canonical system + if s is None: + s = self.cs.step(tau=tau, error_coupling=error_coupling) + elif not isinstance(s, (float, int)): + raise TypeError("Expecting the phase 's' to be a float or integer. Instead, I got {}".format(type(s))) + + # check if same phase as before + if s == self.prev_s: + return self.y, self.dy, self.ddy + + if new_goal is None: + new_goal = self.goal + else: + new_goal = new_goal + self.cst * (new_goal - self.goal) + + # save previous position and velocity + prev_y, prev_dy = self.y.copy(), self.dy.copy() + + # for each DMP, solve transformation system equation using Euler's method + for d in range(self.num_dmps): + + # compute forcing term + if forcing_term is None: + f = self.f[d](s) + self.K[d] * s * (self.goal[d] - new_goal[d]) + else: + f = forcing_term[d] + + # DMP acceleration + self.ddy[d] = self.K[d]/(tau**2) * (new_goal[d] - self.y[d]) - self.D[d]/tau * self.dy[d] + f/(tau**2) + if external_force is not None: + self.ddy[d] += external_force[d] + self.dy[d] += self.ddy[d] / tau * self.dt * error_coupling + self.y[d] += self.dy[d] * self.dt * error_coupling + + # return self.y, self.dy, self.ddy + return prev_y, prev_dy, self.ddy + + def _check_offset(self): + """No need to check for an offset with this class""" + pass + + def generate_goal(self, y0=None, dy0=None, ddy0=None, f0=None): + """ + Generate the goal from the initial positions, velocities, accelerations, and forces. + + Args: + y0 (float[M], None): initial positions. If None, it will take the default initial positions. + dy0 (float[M], None): initial velocities. If None, it will take the default initial velocities. + ddy0 (float[M], None): initial accelerations. If None, it will take the default initial accerelations. + f0 (float[M], None): initial forcing terms. If None, it will compute it based on the learned weights. + You can also give `dmp.f_target[:,0]` to get the correct goal. + + Returns: + float[M]: goal position for each DMP. + """ + if y0 is None: + y0 = self.y0 + if dy0 is None: + dy0 = self.dy0 + if ddy0 is None: + ddy0 = self.ddy0 + if f0 is None: + s0 = self.cs.init_phase + f0 = self.get_forcing_term(s0) + + return 1/self.K * (ddy0 + self.D * dy0 + self.K * y0 - self.K * f0) + + +# Tests +if __name__ == '__main__': + import matplotlib.pyplot as plt + + # tests basis functions + num_basis = 100 + + # Test Biologically-inspired DMP + t = np.linspace(0., 1., 100) + y_d = np.sin(np.pi * t) + new_goal = np.array([[0.8, -0.25], + [0.8, 0.25], + [1.2, -0.25]]) + + discrete_dmp = DiscreteDMP(num_dmps=2, num_basis=num_basis) + discrete_dmp.imitate(np.array([t, y_d])) + y, dy, ddy = discrete_dmp.rollout() + init_points = np.array([discrete_dmp.y0, discrete_dmp.goal]) + # print(discrete_dmp.generate_goal()) + # print(discrete_dmp.generate_goal(f0=discrete_dmp.f_target[:,0])) + + # check with standard discrete DMP when rescaling the goal + plt.subplot(1, 3, 1) + plt.title('Initial discrete DMP') + plt.scatter(init_points[:,0], init_points[:, 1], color='b') + plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') + plt.plot(y[0], y[1], 'b', label='original') + + plt.subplot(1, 3, 2) + plt.title('Rescaled discrete DMP') + plt.scatter(init_points[:, 0], init_points[:, 1], color='b') + plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') + plt.plot(y[0], y[1], 'b', label='original') + for g in new_goal: + y, dy, ddy = discrete_dmp.rollout(new_goal=g) + plt.plot(y[0], y[1], 'g', label='scaled') + plt.legend(['original', 'scaled']) + + # change goal with biologically-inspired DMP + new_goal = np.array([[0.8, -0.25], + [0.8, 0.25], + [0.4, 0.1], + [5., 0.15], + [1.2, -0.25], + [-0.8, 0.1], + [-0.8, -0.25], + [5., -0.25]]) + bio_dmp = BioDiscreteDMP(num_dmps=2, num_basis=num_basis) + bio_dmp.imitate(np.array([t, y_d])) + y, dy, ddy = bio_dmp.rollout() + init_points = np.array([bio_dmp.y0, bio_dmp.goal]) + + plt.subplot(1, 3, 3) + plt.title('Biologically-inspired DMP') + plt.scatter(init_points[:, 0], init_points[:, 1], color='b') + plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') + plt.plot(y[0], y[1], 'b', label='original') + for g in new_goal: + y, dy, ddy = bio_dmp.rollout(new_goal=g) + plt.plot(y[0], y[1], 'g', label='scaled') + plt.legend(['original', 'scaled']) + plt.show() + + # changing goal at the middle + y_list = [] + for g in new_goal: + bio_dmp.reset() + y_traj = np.zeros((2, 100)) + for t in range(100): + if t < 30: + y, dy, ddy = bio_dmp.step() + else: + y, dy, ddy = bio_dmp.step(new_goal=g) + y_traj[:, t] = y + y_list.append(y_traj) + for y in y_list: + plt.plot(y[0], y[1]) + plt.scatter(bio_dmp.y0[0], bio_dmp.y0[1], color='b') + plt.scatter(new_goal[:, 0], new_goal[:, 1], color='r') + plt.title('change goal at the middle') + plt.show() + + # changing goal at the middle but with a moving goal + g = np.hstack((np.arange(1.0, 2.0, 0.1).reshape(10, -1), + np.arange(0.0, 1.0, 0.1).reshape(10, -1))) + + bio_dmp.reset() + y_traj = np.zeros((2, 100)) + y_list = [] + for t in range(100): + y, dy, ddy = bio_dmp.step(new_goal=g[int(t/10)]) + y_traj[:, t] = y + if (t % 10) == 0: + y_list.append(y) + y_list = np.array(y_list) + + plt.plot(y_traj[0], y_traj[1]) + plt.scatter(bio_dmp.y0[0], bio_dmp.y0[1], color='b') + plt.scatter(g[:, 0], g[:, 1], color='r') + plt.scatter(y_list[:, 0], y_list[:, 1], color='g') + plt.title('moving goal') + plt.show() diff --git a/pyrobolearn/models/dmp/canonical_systems.py b/pyrobolearn/models/dmp/canonical_systems.py new file mode 100644 index 0000000..21c365b --- /dev/null +++ b/pyrobolearn/models/dmp/canonical_systems.py @@ -0,0 +1,227 @@ +#!/usr/bin/env python +"""Define canonical systems for dynamic movement primitives + +This file implements canonical systems for discrete and rhythmic dynamic movement primitives. +""" + +from abc import ABCMeta, abstractmethod +import numpy as np + + +__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 CS(object): + r"""Canonical System. + + A canonical system (CS) drives a dynamic movement primitive (DMP) by providing a phase variable [1]. + The phase variable was introduced to avoid an explicit dependency with time in the DMP equations. Canonical + systems can be categorized in two main categories: + * discrete CS: used for discrete movements (such as reaching, pushing/pulling, hitting, etc) + * rhythmic CS: used for rhythmic movements (such as walking, running, dribbling, sewing, flipping a pancake, etc) + + Each of these systems are described by differential equations which are solved using Euler's method. + See their corresponding classes `DiscreteCS` and `RhythmicCS` for more information. + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + """ + + __metaclass__ = ABCMeta + + def __init__(self, dt=0.01, T=1.): + """Initialize the canonical system. + + Args: + dt (float): the time step used in Euler's method when solving the differential equation + A very small step will lead to a better accuracy but will take more time. + """ + # set variables + self.dt = dt + self.T = T + self.timesteps = int(T / self.dt) + # rescale integration step (same as np.linspace(0.,T.,timesteps) instead of np.arange(0,T,dt)) + self.dt = self.T / (self.timesteps - 1.) + + self.init_phase = 1.0 + self.s = 1.0 + + # reset the phase variable + self.reset() + + @abstractmethod + def step(self, tau=1.0, error_coupling=1.0): + """Perform a step using Euler's method. This needs to be implemented in the child classes.""" + raise NotImplementedError + + def reset(self): + """Reset the phase variable""" + self.s = self.init_phase + return self.s + + def rollout(self, tau=1.0, error_coupling=1.0): + """Generate phase variable in an open loop fashion. + + Args: + tau (float): Increase tau to make the system slower, and decrease it to make it faster + error_coupling (float): slow down if the error is > 1 + """ + timesteps = int(self.timesteps * tau) + self.s_track = np.zeros(timesteps) + + # reset + self.reset() + + # roll + for t in range(timesteps): + self.s_track[t] = self.s + self.step(tau, error_coupling) + + return self.s_track + + +class DiscreteCS(CS): + r"""Discrete Canonical System. + + The discrete canonical system drives the various DMPs by providing the phase variable at each time step, and is + given by: + + .. math:: \tau \dot{s} = - \alpha_s s + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, :math:`s` is the phase + variable that drives the DMP, and :math:`\alpha_s` is a predefined constant. + This differential equation is solved using Euler's method. + + This version is used for discrete movements, where :math:`s` starts from 1 and converge to 0 as time progresses. + The phase variable was introduced to avoid an explicit dependency of time in the DMP equations. + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + """ + + def __init__(self, alpha_s=1, dt=0.01): + super(DiscreteCS, self).__init__(dt=dt, T=1.0) + self.alpha_s = alpha_s + + def reset(self): + """Reset the phase variable""" + self.s = self.init_phase + return self.s + + def step(self, tau=1.0, error_coupling=1.0): + """Generate phase value for discrete movements. + + The phase variable :math:`s` is generated by solving :math:`\tau \dot{s} = - \alpha_s s` using Euler's method. + This phase decays from 1 to 0. + + Args: + tau (float): Increase tau to make the system slower, and decrease it to make it faster + error_coupling (float): slow down if the error is > 1 + + Returns: + float: phase value + """ + s = self.s + self.s += (-self.alpha_s/tau * self.s * error_coupling) * self.dt + # return self.s + return s + + +class RhythmicCS(CS): + r"""Rhythmic Canonical System. + + The rhythmic canonical system drives the various DMPs by providing a phase variable that is periodic [1]. It is + used for rhythmic movements (such as walking, dribbling, sewing, etc.) and is given by: + + .. math:: \tau \dot{s} = 1 + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, :math:`s` is the phase + variable that drives the DMP. This differential equation is solved using Euler's method. + + Rhythmic canonical systems can also be coupled with each other as done in [2] to synchronize various DMPs. + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + [2] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 + """ + + def __init__(self, dt=0.01): + super(RhythmicCS, self).__init__(dt=dt, T=2*np.pi) + self.init_phase = 0.0 + + def reset(self): + """Reset the phase variable""" + self.s = self.init_phase + return self.s + + def step(self, tau=1.0, error_coupling=1.0): + r"""Generate phase value for rhythmic movements. + + The phase variable :math:`s` is generated by solving :math:`\tau \dot{s} = 1` using Euler's method. + + Args: + tau (float): Increase tau to make the system slower, and decrease it to make it faster + error_coupling (float): slow down if the error is > 1 + + Returns: + float: phase value + """ + s = self.s + self.s += (1./tau * error_coupling) * self.dt + # return self.s + return s + + +class RhythmicNetworkCS(CS): + r"""Rhythmic Network CS. + + In this version, instead of having one canonical system that drives all the various DMPs, we have several + canonical systems coupled with each other, and where each one of them is associated to a particular DMP. + + The evolution of the phase variable :math:`\phi` of the system :math:`i` is given by: + + .. math:: \dot{\phi}_i = \omega_i + \sum_j a_j w_{ij} \sin(\phi_j - \phi_i - \varphi_{ij}) + + where :math:`\omega` is the desired angular velocity (desired frequency), :math:`w_{ij}` are the coupling weights, + :math:`\varphi_{ij}` are the phase biases, and :math:`a_j` are the amplitudes of the other systems :math:`j`. + This formulation is similar to Central Pattern Generators (CPGs), see [3]. + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + [2] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 + [3] "Central pattern generators for locomotion control in animals and robots: a review", Ijspeert, 2008 + """ + def __init__(self, dt=0.01): + super(RhythmicNetworkCS, self).__init__(dt=dt) + + +# Tests +if __name__ == '__main__': + import matplotlib.pyplot as plt + + # tests canonical systems + discrete_cs = DiscreteCS() + rhythmic_cs = RhythmicCS() + + # check tau + plt.subplot(1, 2, 1) + plt.title('Discrete CS') + for tau in [1., 0.5, 2.]: + rollout = discrete_cs.rollout(tau=tau) + plt.plot(np.linspace(0, 1., len(rollout)), rollout, label='tau='+str(tau)) + plt.legend() + + plt.subplot(1, 2, 2) + plt.title('Rhythmic CS') + for tau in [1., 0.5, 2.]: + rollout = rhythmic_cs.rollout(tau=tau) + plt.plot(np.linspace(0, 1., len(rollout)), rollout, label='tau='+str(tau)) + plt.legend() + plt.show() diff --git a/pyrobolearn/models/dmp/discrete_dmp.py b/pyrobolearn/models/dmp/discrete_dmp.py new file mode 100644 index 0000000..cc8b00f --- /dev/null +++ b/pyrobolearn/models/dmp/discrete_dmp.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python +"""Define the discrete dynamic movement primitive. +""" + +import numpy as np + +from pyrobolearn.models.dmp.canonical_systems import DiscreteCS +from pyrobolearn.models.dmp.forcing_terms import DiscreteForcingTerm +from pyrobolearn.models.dmp.dmp import DMP + +__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 DiscreteDMP(DMP): + r"""Discrete Dynamic Movement Primitive + + Discrete DMPs have the same mathematical formulation as general DMPs, which is given by: + + .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} + f(s) (g - y0) + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` + is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, + velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. + + However, the forcing term in the case of discrete DMPs is given by: + + .. math:: f(s) = \frac{\sum_i \psi_i(s) w_i}{\sum_i \psi_i(s)} s + + where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the + given input phase variable :math:`s`, :math:`g` is the goal, and :math:`y_0` is the initial position. Note that + as the phase converges to 0, the forcing term also converges to that value. + + The basis functions (in the discrete case) are given by: + + .. math:: \psi_i(s) = \exp \left( - \frac{1}{2 \sigma_i^2} (x - c_i)^2 \right) + + where :math:`c_i` is the center of the basis function :math:`i`, and :math:`\sigma_i` is its width. + + Also, the canonical system associated with this transformation system is given by: + + .. math:: \tau \dot{s} = - \alpha_s s + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, :math:`s` is the phase + variable that drives the DMP, and :math:`\alpha_s` is a predefined constant. + + All these differential equations are solved using Euler's method. + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + """ + + def __init__(self, num_dmps, num_basis, dt=0.01, y0=0, goal=1, + forcing_terms=None, stiffness=None, damping=None): + """Initialize the discrete DMP + + Args: + num_dmps (int): number of DMPs + num_basis (int, int[M]): number of basis functions, or list of number of basis functions. + dt (float): step integration for Euler's method + y0 (float, float[M]): initial position(s) + goal (float, float[M]): goal(s) + forcing_terms (list, ForcingTerm): the forcing terms (which can have different basis functions) + stiffness (float): stiffness coefficient + damping (float): damping coefficient + """ + + # create discrete canonical system + cs = DiscreteCS(dt=dt) + + # create forcing terms (each one contains the basis functions and learnable weights) + if forcing_terms is None: + if isinstance(num_basis, int): + forcing_terms = [DiscreteForcingTerm(cs, num_basis) for _ in range(num_dmps)] + else: + if not isinstance(num_basis, (np.ndarray, list, tuple, set)): + raise TypeError("Expecting 'num_basis' to be an int, list, tuple, np.array or set.") + if len(num_basis) != num_dmps: + raise ValueError("The length of th list of number of basis doesn't match the number of DMPs") + forcing_terms = [DiscreteForcingTerm(cs, n_basis) for n_basis in num_basis] + + # call super class constructor + super(DiscreteDMP, self).__init__(canonical_system=cs, forcing_term=forcing_terms, y0=y0, goal=goal, + stiffness=stiffness, damping=damping) + + def get_scaling_term(self, new_goal=None): + """ + Return the scaling term for the forcing term. + + Args: + new_goal (float, float[M], None): the new goal position. If None, it will be the current goal. + + Returns: + float, float[M]: scaling term + """ + if new_goal is None: + new_goal = self.goal + return (new_goal - self.y0) / (self.goal - self.y0) + + def _generate_goal(self, y_des): + """Generate the goal for path imitation. + + Args: + y_des (np.array): the desired trajectory to follow with shape [num_dmps, timesteps] + + Returns: + float[M]: goal position + """ + return np.copy(y_des[:, -1]) + + +# Tests +if __name__ == '__main__': + import matplotlib.pyplot as plt + + # tests canonical systems + discrete_cs = DiscreteCS() + + # tests basis functions + num_basis = 20 + discrete_f = DiscreteForcingTerm(discrete_cs, num_basis) + + # tests forcing terms + f = np.sin(np.linspace(0, 2*np.pi, 100)) + discrete_f.train(f, plot=True) + + # Test discrete DMP + discrete_dmp = DiscreteDMP(num_dmps=1, num_basis=num_basis) + t = np.linspace(-6, 6, 100) + y_target = 1 / (1 + np.exp(-t)) + discrete_dmp.imitate(y_target) + y, dy, ddy = discrete_dmp.rollout() + + plt.plot(y_target, label='y_target') + plt.plot(y[0], label='y_pred') + # plt.plot(dy[0]) + # plt.plot(ddy[0]) + y, dy, ddy = discrete_dmp.rollout(new_goal=np.array([2.])) + plt.plot(y[0], label='y_scaled') + plt.title('Discrete DMP') + plt.legend() + plt.show() diff --git a/pyrobolearn/models/dmp/dmp.py b/pyrobolearn/models/dmp/dmp.py new file mode 100644 index 0000000..7062cfe --- /dev/null +++ b/pyrobolearn/models/dmp/dmp.py @@ -0,0 +1,617 @@ +#!/usr/bin/env python +"""Define the general dynamic movement primitive abstract class. + +This file implements the DMP abstract class from which all dynamic movement primitive classes inherit from. +""" + +import numpy as np +import copy +import scipy.interpolate + +from pyrobolearn.models.dmp.canonical_systems import CS +from pyrobolearn.models.dmp.forcing_terms import ForcingTerm + + +__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 DMP(object): + r"""Dynamic Movement Primitive + + Dynamic movement primitives (DMPs) are a set of differential equations (for each degree of freedoms (DoFs), i.e. + general coordinates) that encodes a movement [1]. It is thought that movement primitives are the building blocks + of a movement, and several evidences show that such modules exist in animals [2]. + + DMPs are often formulated as a 2nd-order differential equation: + + .. math:: \tau^2 \ddot{y} = \alpha ( \beta (g - y) - \dot{y}) + f(s) + + or sometimes, as a first-order differential system: + + .. math:: + + \tau \dot{z} &= \alpha ( \beta (g - y) - z) + f(s) \\ + \tau \dot{y} &= z + + They can also be rewritten as: + + .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} + f(s) + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` + is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, + velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. These equations are also + known as the transformation systems and represent, with the canonical system, DMPs. + + All of the above formulations are equivalent to each other. However, in my humble opinion, the last equation + depicts better what the transformation system constitutes; it is a unit-mass spring-damper system or PD controller + with a forcing term. This last term is non-linear and can be learned from the demonstrations. + If the forcing is zero, then the differential equation is stable, and the position :math:`y` converges to the goal. + The stiffness and damping coefficients (:math:`K` and :math:`D`) are often selected such that the whole system + (without the forcing term) is critically damped (:math:`D = 2 \sqrt{K}`). Other behaviors can be obtained by + selecting the stiffness and damping coefficient such that we obtain: + * an undamped system: :math:`D = 0` or :math:`K \rightarrow \infty` + * an underdamped system: :math:`D < 2 \sqrt{K}` + * a critically damped system: :math:`D = 2 \sqrt{K}` + * an overdamped system: :math:`D > 2 \sqrt{K}` + + Because the last formulation is more intuitive (at least for me), it will be used in this class. + Imitation is performed by learning the forcing term. + + DMPs can be categorized in two main categories: + * discrete DMP: used to represent discrete movements such as such as reaching, pushing/pulling, etc. + * rhythmic DMP: used to represent rhythmic movements such as walking, running dribbling, sewing, etc. + + DMP have the following nice properties: + * translation invariant + * linear parameters but still allows to represent non-linear movements + + Here are few limitations/shortcomings: + * hard to couple sensory information with it + * have to come up with the number of basis functions + + For a more biologically-inspired DMP [5] which allows to adapt the goal in real-time and a better rescaling, see + the `BioDMP` class. + + Note that this code was inspired by the `pydmps` code [2,3], but differ in several ways, notably: + - we undertake a more object-oriented programming (OOP) approach + - the equations are a little bit differents (e.g. :math:`tau`) in which we use the ones presented in the refs + - we decouple the Euler's method time step with the time step for the number of data points + - timesteps: we go from 0 to T included, while DeWolf goes from 0 to T-1 + - we use array operation instead of iterating over each element to update them + - we enforce consistency between the various methods and data structures + - we implement `BioDMP` which allows to adapt and rescale the goal in real-time based on [4] + - we implemented DMP sequencing based on [5] + - we implemented DMP that can be used with orientations based on [7] + - phase nodes which allows to couple phases, such as done in [8] for locomotion + - it can be used with RL algorithms, notably PoWER [9] and PI^2 [10] + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + [2] "Motor primitives in vertebrates and invertebrates", Flash et al., 2005 + [3] Tutorials on DMP: https://studywolf.wordpress.com/category/robotics/dynamic-movement-primitive/ + [4] PyDMPs (from DeWolf, 2013): https://github.com/studywolf/pydmps + [5] "Biologically-inspired Dynamical Systems for Movement Generation: Automatic Real-time Goal Adaptation + and Obstacle Avoidance", Hoffmann et al., 2009 + [6] "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011 + [7] "Orientation in Cartesian Space Dynamic Movement Primitives", Ude et al., 2014 + [8] "A Framework for Learning Biped Locomotion with Dynamical Movement Primitives", Nakanishi et al., 2004 + [9] "Policy Search for Motor Primitives in Robotics", Kober et al., 2010 + [10] "A Generalized Path Integral Control Approach to Reinforcement Learning", Theodorou et al., 2010 + """ + + def __init__(self, canonical_system, forcing_term, y0=0, goal=1, stiffness=None, damping=None): + """Initialize the DMP. + + Args: + canonical_system (CS): canonical system which drives the DMP transformation system + forcing_terms (list): list of forcing terms (one forcing term for each DMP). Each forcing term can have + different number of basis functions. + y0 (float, float[M]): initial state of DMPs + goal (float, float[M]): goal state of DMPs + stiffness (float): stiffness term in the transformation system for DMPs + damping (float): damping term in the transformation system for DMPs + """ + + self.cs = canonical_system + + if isinstance(forcing_term, ForcingTerm): + forcing_term = [forcing_term] + elif isinstance(forcing_term, (list, tuple)): + for f in forcing_term: + if not isinstance(f, ForcingTerm): + raise TypeError("An item in the iterable is not an instance of ForcingTerm.") + else: + raise TypeError("Expecting forcing term to be an instance of ForcingTerm or a list/tuple of ForcingTerm") + + self.f = forcing_term + self.num_dmps = len(forcing_term) + self.dt = self.cs.dt + self.timesteps = self.cs.timesteps + + # check initial and goal positions # TODO use property to set them + if isinstance(y0, (int, float)): + y0 = np.ones(self.num_dmps) * y0 + if isinstance(y0, (list, tuple)): + y0 = np.array(y0) + self.y0 = y0 + self.dy0 = np.zeros(self.num_dmps) + self.ddy0 = np.zeros(self.num_dmps) + if isinstance(goal, (int, float)): + goal = np.ones(self.num_dmps) * goal + elif isinstance(goal, (list, tuple)): + goal = np.array(goal) + self.goal = goal + self._check_offset() + + self.y, self.dy, self.ddy = self.y0, self.dy0, self.ddy0 + + # set stiffness and damping coefficient (if not specified, make them critically damped, i.e. D=2\sqrt{K}) + self.D = np.ones(self.num_dmps) * 25. if damping is None else damping + self.K = self.D**2 / 4. if stiffness is None else stiffness + + # set up the DMP system + self.prev_s = self.cs.init_phase + self.reset() + + # target forcing term (keep a copy) + self.f_target = None + + def __repr__(self): + return self.__class__.__name__ + + def __call__(self, *args, **kwargs): + return self.step(*args, **kwargs) + + ############## + # Properties # + ############## + + @property + def input_size(self): + """Return the input size of the model.""" + return 1 # 1 canonical system + + @property + def output_size(self): + """Return the output size of the model.""" + return len(self.f) + + @property + def input_shape(self): + """Return the input shape of the model.""" + return tuple([self.input_size]) + + @property + def output_shape(self): + """Return the output shape of the model.""" + return tuple([self.output_size]) + + @property + def input_dim(self): + """Return the input dimension of the model; i.e. len(input_shape).""" + return len(self.input_shape) + + @property + def output_dim(self): + """Return the output dimension of the model; i.e. len(output_shape).""" + return len(self.output_shape) + + @property + def num_parameters(self): + """Return the total number of parameters""" + return np.array([force.w for force in self.f]).size + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other): + if not isinstance(other, DMP): + raise TypeError("Trying to copy an object which is not a DMP") + if deep: + return copy.deepcopy(other) + return copy.copy(other) + + @staticmethod + def is_parametric(): + """Return True as a DMP has weights that need to be optimized.""" + return True + + @staticmethod + def is_linear(): + """Return True as a DMP is linear in terms of its weights (i.e. learnable parameters)""" + return True + + @staticmethod + def is_recurrent(): + """Return False as a DMP is not a recurrent model.""" + return False + + @staticmethod + def is_probabilistic(): + """The DMP is a deterministic model.""" + return False + + @staticmethod + def is_discriminative(): + """The DMP is a discriminative model which predicts the output :math:`y` given the input :math:`x`""" + return True + + @staticmethod + def is_generative(): + """The DMP is not a generative model.""" + return False + + ########### + # Methods # + ########### + + def parameters(self): + """Returns an iterator over the model parameters.""" + for force in self.f: + yield force.w + + def named_parameters(self): + """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" + for force in self.f: + yield str(force), force.w + + def list_parameters(self): + """Return a list of parameters""" + return list(self.parameters()) + + def hyperparameters(self): + """Return an iterator over the hyper-parameters.""" + yield self.K + yield self.D + # yield basis_functions + + def named_hyperparameters(self): + """Return an iterator over the hyper-parameters, yielding both the name and the hyper-parameter itself.""" + yield "stiffness", self.K + yield "damping", self.D + + def list_hyperparameters(self): + """Return a list of hyper-parameters.""" + return list(self.hyperparameters()) + + def get_vectorized_parameters(self, to_numpy=True): + """Return a vectorized form (1 dimensional array) of the parameters.""" + parameters = self.parameters() + vector = np.concatenate([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): + """Set the vector parameters.""" + # 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.parameters(): + # size = parameter.nelement() + # parameter.data = vector[idx:idx+size].reshape(parameter.shape) + # idx += size + + # set the parameters from the vectorized one + idx = 0 + for force in self.f: + size = force.w.size + force.w = vector[idx:idx+size].reshape(force.w.shape) + idx += size + + def get_damping_ratio(self): + """ + Return the damping ratio :math:`\zeta = D / D_c` where :math:`D_c = 2 \sqrt{K}`. + + * if :math:`\zeta` = 0, the system is undamped (i.e. no damping) + * if :math:`\zeta` < 1, the system is underdamped (i.e. there will be some oscillations) + * if :math:`\zeta` = 1, the system is critically damped (i.e. return to equilibrium as fast as possible + without oscillating). + * if :math:`\zeta` > 1, the system is overdamped (i.e. the system returns to equilibrium without oscillating + but might be slow depending on the damping value). + """ + return self.D / (2*np.sqrt(self.K)) + + def _check_offset(self): + """Check to see if the initial position and goal are the same. If that is the case, offset slightly so that + the forcing term is not 0. Otherwise, look at the `BioDMP` class. + """ + self.goal[self.y0 == self.goal] += 1e-4 + + def get_scaling_term(self, new_goal=None): + # this is overridden by the child classes + return np.ones(self.num_dmps) + + def _generate_goal(self, y_des): + raise NotImplementedError() + + def reset(self): + """Reset the transformation and canonical systems""" + self.y = self.y0.copy() + self.dy = self.dy0.copy() # np.zeros(self.num_dmps) + self.ddy = self.ddy0.copy() + self.prev_s = self.cs.reset() + + def step(self, s=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, external_force=None, + rescale_force=True): + """Run the DMP transformation system for a single time step. + + Args: + s (None, float): the phase value. If None, it will use the canonical system. + tau (float): Increase tau to make the system slower, and decrease it to make it faster + error (float): optional system feedback + forcing_term (float[M]): if given, it will replace the forcing term (where `M` = number of DMPs) + new_goal (float[M]): new goal (where `M` = number of DMPs) + rescale_force (bool): if the given forcing term should be rescaled. + """ + + # system feedback + error_coupling = 1.0 / (1.0 + error) + + # get phase from canonical system + if s is None: + s = self.cs.step(tau=tau, error_coupling=error_coupling) + elif not isinstance(s, (float, int)): + raise TypeError("Expecting the phase 's' to be a float or integer. Instead, I got {}".format(type(s))) + + # check if same phase as before + if s == self.prev_s: + return self.y, self.dy, self.ddy + + if new_goal is None: + new_goal = self.goal + + # save previous position and velocity + prev_y, prev_dy = self.y.copy(), self.dy.copy() + + # compute scaling factor for the forcing term + scaling = self.get_scaling_term(new_goal) + + # for each DMP, solve transformation system equation using Euler's method + for d in range(self.num_dmps): + + # compute forcing term + if forcing_term is None: + f = self.f[d](s) * scaling[d] + # f = self.f_gen(s) * scaling[d] + else: + if rescale_force: + f = forcing_term[d] * scaling[d] + else: + f = forcing_term[d] + + # DMP acceleration + self.ddy[d] = self.K[d]/(tau**2) * (new_goal[d] - self.y[d]) - self.D[d]/tau * self.dy[d] + f/(tau**2) + if external_force is not None: + self.ddy[d] += external_force[d] + self.dy[d] += self.ddy[d] / tau * self.dt * error_coupling + self.y[d] += self.dy[d] * self.dt * error_coupling + + # return self.y, self.dy, self.ddy + return prev_y, prev_dy, self.ddy + + def rollout(self, timesteps=None, tau=1.0, error=0.0, forcing_term=None, new_goal=None, rescale_force=True, + **kwargs): + """Generate position, velocity, and acceleration trajectories, no feedback is incorporated. + + Args: + tau (float): Increase tau to make the system slower, and decrease it to make it faster + timesteps (None, int): the number of steps to perform + error (float): optional system feedback + forcing_term (np.ndarray): if given, it will replace the forcing term (shape [num_dmps, timesteps]) + new_goal (np.ndarray): new goal (of shape [num_dmps,]) + + Returns: + float[M,T]: y (position) trajectories + float[M,T]: dy (velocity) trajectories + float[M,T]: ddy (acceleration) trajectories + """ + + # reset the canonical and transformation systems + self.reset() + + if timesteps is None: + timesteps = int(self.timesteps * tau) + + # set up tracking vectors + y_track = np.zeros((self.num_dmps, timesteps)) + dy_track = np.zeros((self.num_dmps, timesteps)) + ddy_track = np.zeros((self.num_dmps, timesteps)) + + # for the other timesteps, solve DMP equation using Euler's method + for t in range(timesteps): + if forcing_term is None: + y, dy, ddy = self.step(tau=tau, error=error, new_goal=new_goal, external_force=None) + else: + y, dy, ddy = self.step(tau=tau, error=error, forcing_term=forcing_term[:, t], new_goal=new_goal, + rescale_force=rescale_force) + + # record timestep + y_track[:, t] = y + dy_track[:, t] = dy + ddy_track[:, t] = ddy + + return y_track, dy_track, ddy_track + + def train(self, f_target): + """Train the forcing terms.""" + # train each forcing term + if f_target.shape[0] != len(self.f): + raise ValueError("Mismatch between the number of forcing terms") + + # train each forcing term + for forcing_term, target in zip(self.f, f_target): + forcing_term.train(target) + + def imitate(self, y_des, dy_des=None, ddy_des=None, interpolation='cubic', plot=False): + """Imitate a desired trajectory, and learn the parameters that best realizes it. + + Args: + y_des (np.array): the desired position trajectories of each DMP with shape [num_dmps, timesteps] + dy_des (np.array): the desired velocities with shape [num_dmps, timesteps] + ddy_des (np.array): the desired accelerations with shape [num_dmps, timesteps] + interpolation (str): how to interpolate the data. Select between 'linear', 'cubic', and 'hermite'. + """ + + # set initial state and goal + if y_des.ndim == 1: + y_des = y_des.reshape(1, len(y_des)) + self.y0 = y_des[:, 0].copy() + self.goal = self._generate_goal(y_des) + self._check_offset() + + timesteps = y_des.shape[1] + + def interpolate(x, dt, period, timesteps, new_timesteps, interpolation=interpolation, return_gen=False): + # generate function to interpolate the desired trajectory + t = np.linspace(0, period, timesteps) + if interpolation == 'linear': # use linear interpolation + path_gen = scipy.interpolate.interp1d(t, x, axis=-1) + elif interpolation == 'cubic': # use cubic spline interpolation + path_gen = scipy.interpolate.CubicSpline(t, x, axis=-1) + else: # TODO: implement hermite (see utils.interpolator.hermite) + raise ValueError("The requested interpolation has not been implemented. Select between 'linear' or " + "'cubic'") + if return_gen: + return path_gen + return path_gen([t * self.dt for t in range(new_timesteps)]) + + y_des = interpolate(y_des, dt=self.dt, period=self.cs.T, timesteps=timesteps, + new_timesteps=self.timesteps, interpolation=interpolation) + + # compute desired velocity if necessary + if dy_des is None: + # calculate velocity of y_des + dy_des = np.diff(y_des) / self.dt + # add zero to the beginning of every row + dy_des = np.hstack((np.zeros((self.num_dmps, 1)), dy_des)) + else: + if dy_des.ndim == 1: + dy_des = dy_des.reshape(1, len(dy_des)) + dy_des = interpolate(dy_des, self.dt, self.cs.T, dy_des.shape[1], self.timesteps, + interpolation=interpolation) + self.dy0 = dy_des[:, 0].copy() + + # compute desired acceleration if necessary + if ddy_des is None: + # calculate acceleration of y_des + ddy_des = np.diff(dy_des) / self.dt + # add zero to the beginning of every row + ddy_des = np.hstack((np.zeros((self.num_dmps, 1)), ddy_des)) + else: + if ddy_des.ndim == 1: + ddy_des = ddy_des.reshape(1, len(ddy_des)) + ddy_des = interpolate(ddy_des, self.dt, self.cs.T, ddy_des.shape[1], self.timesteps, + interpolation=interpolation) + self.ddy0 = ddy_des[:, 0].copy() + + # find the force required to move along this trajectory (with shape [num_dmps, timesteps]) + f_target = ddy_des - self.K.reshape(-1, 1) * (self.goal.reshape(-1, 1) - y_des) + self.D.reshape(-1, 1) * dy_des + + # plot + if plot: + import matplotlib.pyplot as plt + plt.figure() + plt.plot(y_des[0], 'b', label='pos') + plt.plot(dy_des[0], 'g', label='vel') + plt.plot(ddy_des[0], 'r', label='acc') + plt.plot(f_target[0], 'k', label='force') + plt.legend() + plt.show() + + # self.f_gen = interpolate(f_target, dt=self.dt, period=self.cs.T, timesteps=timesteps, + # new_timesteps=timesteps, interpolation=interpolation, return_gen=True) + + # efficiently generate weights to realize f_target + self.f_target = f_target + self.train(f_target) + + # reset the canonical and transformation systems + self.reset() + + return y_des + + def get_forcing_term(self, s): + """ + Get the forcing terms based on the given phase value. + + Args: + s (float, float[T]): phase value(s) + + Returns: + float[M], float[M,T]: forcing terms + """ + return np.array([self.f[d](s) for d in range(self.num_dmps)]) + + def generate_goal(self, y0=None, dy0=None, ddy0=None, f0=None): + """ + Generate the goal from the initial positions, velocities, accelerations, and forces. + + Args: + y0 (float[M], None): initial positions. If None, it will take the default initial positions. + dy0 (float[M], None): initial velocities. If None, it will take the default initial velocities. + ddy0 (float[M], None): initial accelerations. If None, it will take the default initial accerelations. + f0 (float[M], None): initial forcing terms. If None, it will compute it based on the learned weights. + You can also give `dmp.f_target[:,0]` to get the correct goal. + + Returns: + float[M]: goal position for each DMP. + """ + if y0 is None: + y0 = self.y0 + if dy0 is None: + dy0 = self.dy0 + if ddy0 is None: + ddy0 = self.ddy0 + if f0 is None: + s0 = self.cs.init_phase + f0 = self.get_forcing_term(s0) + + return 1/self.K * (ddy0 + self.D * dy0 + self.K * y0 - f0) + + def sequence(self, model, mode=0): + """ + Define how to sequence with another DMP model. + + Args: + model (DMP): DMP model + mode (int): specifies how to sequence the two DMP models. + + Returns: + DMP: the sequenced model + + References: + [1] "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011 + """ + if not isinstance(model, DMP): + raise TypeError("The given model is not an instance of DMP.") + pass + + # def __rshift__(self, other): + # """ + # Sequence DMP model with another learning model. + # + # Ref: "Action Sequencing using Dynamic Movement Primitives", Nemec et al., 2011 + # + # :param other: another DMP model + # :return: + # """ + # # If we sequence two DMP models + # if isinstance(other, DMP): + # + # else: + # # if it is another model, call the parent's method which knows how to sequence different models + # super(DMP, self).__rshift__(other) + diff --git a/pyrobolearn/models/dmp/forcing_terms.py b/pyrobolearn/models/dmp/forcing_terms.py new file mode 100644 index 0000000..759f537 --- /dev/null +++ b/pyrobolearn/models/dmp/forcing_terms.py @@ -0,0 +1,328 @@ +#!/usr/bin/env python +"""Define the forcing terms used in dynamic movement primitives + +This file implements the forcing terms used for discrete and rhythmic dynamic movement primitives. +""" + +import numpy as np +import matplotlib.pyplot as plt + +from pyrobolearn.models.dmp.canonical_systems import * +from pyrobolearn.models.dmp.basis_functions import * + + +__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 ForcingTerm(object): + r"""Forcing term used in DMPs + + This basically computes the unscaled forcing term, i.e. a weighted sum of basis functions, which is given by: + + .. math:: f(s) = \frac{ sum_{i} \psi_i(s) w_i }{ \sum_i \psi_i(s) } + + where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the + given input phase variable :math:`s`. + """ + + def __init__(self, weights, basis_functions): + # check that the arguments have the same length + self.w = weights + self.psi = basis_functions + + @property + def weights(self): + return self.w + + @staticmethod + def is_linear(): + return True + + @staticmethod + def is_parametric(): + return True + + @staticmethod + def is_recurrent(): + return False + + def compute(self, s): + """Compute the forcing term + + Compute the value of the forcing term :math:`f(s)` at the given phase value :math:`s`. + + Args: + s (float): phase value + + Returns: + float: value of the forcing term at the given phase value + """ + psi_track = self.psi(s) + if len(psi_track.shape) == 1: + return np.dot(psi_track, self.w) / np.sum(psi_track) + return np.dot(psi_track, self.w) / np.sum(psi_track, axis=1) + + def weighted_basis(self, s): + """Generate weighted basis + + Returns: + np.array[T, M]: weighted basis + """ + return self.psi(s) * self.w + + def normalized_weighted_basis(self, s): + """Generate normalized weighted basis + + Args: + s (float): phase value + + Returns: + np.array[T,M]: normalized weighted basis + """ + psi_track = self.psi(s) + return ((psi_track * self.w).T / np.sum(psi_track, axis=1)).T + + # alias + def __call__(self, s): + return self.compute(s) + + def __str__(self): + return self.__class__.__name__ + + # To override in child classes + def train(self, f_target): + raise NotImplementedError + + # alias + generate_weights = train + + +class DiscreteForcingTerm(ForcingTerm): + r"""Discrete Forcing Term + + .. math:: f(s) = \frac{ sum_{i} \psi_i(s) w_i }{ \sum_i \psi_i(s) } s + + where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the + given input phase variable :math:`s`. + + This forcing term has the property that as the phase converges to 0, it also converges to 0, allowing the + linear part of the DMP equation to converge to the goal. + """ + + def __init__(self, cs, num_basis): + """Initialize the discrete forcing term. + + Args: + cs (CS): discrete canonical system + num_basis (int): number of basis functions + """ + # set canonical system + if not isinstance(cs, DiscreteCS): + raise TypeError("Expecting 'cs' to be an instance of DiscreteCS") + self.cs = cs + + # set num_basis + self.num_basis = num_basis + + # create weights + weights = np.zeros(num_basis) # default f=0 + + # desired activations throughout time + c = np.linspace(0, cs.T, num_basis) + c = np.exp(-cs.alpha_s * c) + + # set variance of basis functions (this was found by trial and error by DeWolf) + h = np.ones(num_basis) * num_basis**1.5 / c / cs.alpha_s + + basis = EBF(center=c, h=h) + super(DiscreteForcingTerm, self).__init__(weights, basis) + + def compute(self, s): + # call parent compute + f = super(DiscreteForcingTerm, self).compute(s) + # scale with phase s + return f * s + + def train(self, f_target, plot=False): + """Train the weights to match the given target forcing term + + Generate a set of weights over the basis functions such that the target forcing term trajectory is matched. + + Args: + f_target (np.array): the desired forcing term trajectory + """ + + # calculate phase and basis functions + s_track = self.cs.rollout() + psi_track = self.psi(s_track) # shape=TxM + + # efficiently calculate BF weights using LWR (Locally Weighted (Linear) Regression) + # spatial scaling term + for b in range(self.num_basis): + numerator = np.sum(s_track * psi_track[:, b] * f_target) + denominator = np.sum(s_track**2 * psi_track[:, b]) + self.w[b] = numerator / denominator + + self.w = np.nan_to_num(self.w) + + if plot: + # plot the basis function activations + plt.figure() + plt.subplot(211) + plt.plot(psi_track) + plt.title('basis functions') + + # plot the desired forcing function vs approx for the first dmp + plt.subplot(212) + plt.title('discrete force') + plt.plot(f_target, label='f_target', linewidth=2.5) + plt.plot(self.compute(s_track), label='f_pred', linewidth=2.5) + + # weighted sum of basis functions + wps = self.weighted_basis(s_track) + plt.plot(wps, linewidth=0.5) + + plt.legend() + plt.tight_layout() + plt.show() + + +class RhythmicForcingTerm(ForcingTerm): + r"""Rhythmic Forcing Term + + .. math:: f(s) = \frac{ sum_{i} \psi_i(s) w_i }{ \sum_i \psi_i(s) } a + + where :math:`w` are the learnable weight parameters, :math:`\psi` are the basis functions evaluated at the + given input phase variable :math:`s`, and :math:`a` is the amplitude. + + When used with DMPs, it produces a limit cycle behavior. + """ + + def __init__(self, cs, num_basis, amplitude=1.): + """ + Initialize the rhythmic forcing term. + + Args: + cs (CS): rhythmic canonical system + num_basis (int): number of basis functions + amplitude (float): amplitude + """ + # set canonical system + if not isinstance(cs, RhythmicCS): + raise TypeError("Expecting 'cs' to be an instance of RhythmicCS") + self.cs = cs + + # set num_basis and amplitude + self.num_basis = num_basis + self.a = amplitude + + # create weights + weights = np.zeros(num_basis,) # default f=0 + + # set the centre of the Gaussian basis functions to be spaced evenly + c = np.linspace(0, cs.T, num_basis + 1) # the '+1' is because it is rhythmic, c(0) = c(2pi) + c = c[:-1] + + # set concentration of basis function (this was found by trial and error by DeWolf) + h = np.ones(num_basis) * num_basis + + # create basis functions + basis = CBF(center=c, h=h) + super(RhythmicForcingTerm, self).__init__(weights, basis) + + def compute(self, s): + # call parent compute + f = super(RhythmicForcingTerm, self).compute(s) + # scale with amplitude and return it + return f * self.a + + def train(self, f_target, plot=False): + """Train the weights to match the given target forcing term + + Generate a set of weights over the basis functions such that the target forcing term trajectory is matched. + + Args: + f_target (np.array): the desired forcing term trajectory + plot (bool): If True, it will plot. + """ + + # calculate phase and basis functions + s_track = self.cs.rollout() + psi_track = self.psi(s_track) # shape=TxM + + # efficiently calculate BF weights using LWR (Locally Weighted (Linear) Regression) + for b in range(self.num_basis): + self.w[b] = (np.dot(psi_track[:, b], f_target) / (np.sum(psi_track[:, b]))) # + 1e-10)) + + if plot: + # plot the basis function activations + plt.figure() + plt.subplot(211) + plt.plot(psi_track) + plt.title('basis functions') + + # plot the desired forcing function vs approx for the first dmp + plt.subplot(212) + plt.title('rhythmic force') + plt.plot(f_target, label='f_target', linewidth=2.5) + plt.plot(self.compute(s_track), label='f_pred', linewidth=2.5) + wps = self.weighted_basis(s_track) + plt.plot(wps, linewidth=0.5) + plt.legend() + plt.tight_layout() + plt.show() + + +# Tests +if __name__ == '__main__': + import matplotlib.pyplot as plt + + # tests canonical systems + discrete_cs = DiscreteCS() + rhythmic_cs = RhythmicCS() + + # plot canonical systems + plt.subplot(1, 2, 1) + plt.title('Discrete CS') + for tau in [1., 0.5, 2.]: + rollout = discrete_cs.rollout(tau=tau) + plt.plot(np.linspace(0, 1., len(rollout)), rollout, label='tau='+str(tau)) + plt.legend() + + plt.subplot(1, 2, 2) + plt.title('Rhythmic CS') + for tau in [1., 0.5, 2.]: + rollout = rhythmic_cs.rollout(tau=tau) + plt.plot(np.linspace(0, 1., len(rollout)), rollout, label='tau='+str(tau)) + plt.legend() + plt.show() + + # tests basis functions + num_basis = 20 + discrete_f = DiscreteForcingTerm(discrete_cs, num_basis) + rhythmic_f = RhythmicForcingTerm(rhythmic_cs, num_basis) + + plt.subplot(1, 2, 1) + rollout = discrete_cs.rollout() + plt.title('discrete basis fcts') + plt.plot(rollout, discrete_f.psi(rollout)) + + plt.subplot(1, 2, 2) + rollout = rhythmic_cs.rollout() + plt.title('rhythmic basis fcts') + plt.plot(rollout, rhythmic_f.psi(rollout)) + plt.show() + + # tests forcing terms + f = np.sin(np.linspace(0, 2*np.pi, 100)) + discrete_f.train(f, plot=True) + + f = np.sin(np.linspace(0, 2*np.pi, int(2*np.pi*100))) + rhythmic_f.train(f, plot=True) diff --git a/pyrobolearn/models/dmp/rhythmic_dmp.py b/pyrobolearn/models/dmp/rhythmic_dmp.py new file mode 100644 index 0000000..0dbd960 --- /dev/null +++ b/pyrobolearn/models/dmp/rhythmic_dmp.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +"""Define the rhythmic dynamic movement primitive. +""" + +import numpy as np + +from pyrobolearn.models.dmp.canonical_systems import RhythmicCS +from pyrobolearn.models.dmp.forcing_terms import RhythmicForcingTerm +from pyrobolearn.models.dmp.dmp import DMP + +__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 RhythmicDMP(DMP): + r"""Rhythmic Dynamic Movement Primitive + + Rhythmic DMPs have the same mathematical formulation as general DMPs, which is given by: + + .. math:: \tau^2 \ddot{y} = K (g - y) - D \tau \dot{y} + f(s) + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the reproduced movement, :math:`K` + is the stiffness coefficient, :math:`D` is the damping coefficient, :math:`y, \dot{y}, \ddot{y}` are the position, + velocity, and acceleration of a DoF, and :math:`f(s)` is the non-linear forcing term. + + However, the forcing term in the case of rhythmic DMPs is given by: + + .. math:: f(s) = \frac{\sum_i \psi_i(s) w_i}{\sum_i \psi_i(s)} a + + where :math:`w` are the learnable weight parameters, and :math:`\psi` are the basis functions evaluated at the + given input phase variable :math:`s`, and :math:`a` is the amplitude. + + The basis functions (in the rhythmic case) are given by: + + .. math:: \psi_i(s) = \exp \left( - h_i (\cos(s - c_i) - 1) \right) + + where :math:`c_i` is the center of the basis, and :math:`h_i` is a measure of concentration. + + Also, the canonical system associated with this transformation system is given by: + + .. math:: \tau \dot{s} = 1 + + where :math:`\tau` is a scaling factor that allows to slow down or speed up the movement, and :math:`s` is the + phase variable that drives the DMP. + + All these differential equations are solved using Euler's method. + + References: + [1] "Dynamical movement primitives: Learning attractor models for motor behaviors", Ijspeert et al., 2013 + """ + + def __init__(self, num_dmps, num_basis, dt=0.01, y0=0, goal=1, + forcing_terms=None, stiffness=None, damping=None): + """Initialize the rhythmic DMP + + Args: + num_dmps (int): number of DMPs + num_basis (int): number of basis functions + dt (float): step integration for Euler's method + y0 (float, np.array): initial position(s) + goal (float, np.array): goal(s) + forcing_terms (list, ForcingTerm): the forcing terms (which can have different basis functions) + stiffness (float): stiffness coefficient + damping (float): damping coefficient + """ + + # create rhythmic canonical system + cs = RhythmicCS(dt=dt) + + # create forcing terms (each one contains the basis functions and learnable weights) + if forcing_terms is None: + if isinstance(num_basis, int): + forcing_terms = [RhythmicForcingTerm(cs, num_basis) for _ in range(num_dmps)] + else: + if not isinstance(num_basis, (np.ndarray, list, tuple, set)): + raise TypeError("Expecting 'num_basis' to be an int, list, tuple, np.array or set.") + if len(num_basis) != num_dmps: + raise ValueError("The length of th list of number of basis doesn't match the number of DMPs") + forcing_terms = [RhythmicForcingTerm(cs, n_basis) for n_basis in num_basis] + + # call super class constructor + super(RhythmicDMP, self).__init__(canonical_system=cs, forcing_term=forcing_terms, y0=y0, goal=goal, + stiffness=stiffness, damping=damping) + + def get_scaling_term(self, new_goal=None): + """ + Return the scaling term for the forcing term. For rhythmic DMPs it's non-diminishing, so this function just + returns 1. + """ + return np.ones(self.num_dmps) + + def _generate_goal(self, y_des): + """Generate the goal for path imitation. + + For rhythmic DMPs, the goal is the average of the desired trajectory. + + Args: + y_des (float[M,T]): the desired trajectory to follow (with shape [num_dmps, timesteps]) + + Returns: + float[M]: goal positions (one for each DMP) + """ + goal = np.zeros(self.num_dmps) + for n in range(self.num_dmps): + num_idx = ~np.isnan(y_des[n]) # ignore nan's when calculating goal + goal[n] = .5 * (y_des[n, num_idx].min() + y_des[n, num_idx].max()) + return goal diff --git a/pyrobolearn/models/gaussian.py b/pyrobolearn/models/gaussian.py index 111f10a..d82fb67 100755 --- a/pyrobolearn/models/gaussian.py +++ b/pyrobolearn/models/gaussian.py @@ -6,7 +6,6 @@ the possible operations (that I could think of) that can be performed on it. It Mixture Models, Probabilistic Movement Primitives, Kernelized Movement Primitives, etc. """ - import numpy as np import scipy from scipy.stats import multivariate_normal as mvn @@ -19,6 +18,7 @@ from matplotlib.patches import Ellipse # import torch # import geomstats +# from pyrobolearn.models.model import Model __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/models/gmm/__init__.py b/pyrobolearn/models/gmm/__init__.py new file mode 100644 index 0000000..e0c0173 --- /dev/null +++ b/pyrobolearn/models/gmm/__init__.py @@ -0,0 +1,3 @@ + +# import gmm +from .gmm import * diff --git a/pyrobolearn/models/gmm.py b/pyrobolearn/models/gmm/gmm.py similarity index 99% rename from pyrobolearn/models/gmm.py rename to pyrobolearn/models/gmm/gmm.py index 6dd0c7a..425bd08 100755 --- a/pyrobolearn/models/gmm.py +++ b/pyrobolearn/models/gmm/gmm.py @@ -5,7 +5,6 @@ This file provides the Gaussian Mixture Model, and uses the Gaussian model defin Gaussian Mixture Regression is achieved by conditioning the GMM to some input. """ - import numpy as np try: import cPickle as pickle @@ -14,7 +13,7 @@ except ImportError as e: from sklearn.cluster import KMeans from sklearn.mixture import GaussianMixture, BayesianGaussianMixture -# from model import Model +# from pyrobolearn.models.model import Model from pyrobolearn.models.gaussian import Gaussian diff --git a/pyrobolearn/models/gp/__init__.py b/pyrobolearn/models/gp/__init__.py new file mode 100644 index 0000000..dc9a0ba --- /dev/null +++ b/pyrobolearn/models/gp/__init__.py @@ -0,0 +1,3 @@ + +# import gp +from .gp import * diff --git a/pyrobolearn/models/gp.py b/pyrobolearn/models/gp/gp.py similarity index 99% rename from pyrobolearn/models/gp.py rename to pyrobolearn/models/gp/gp.py index 0c2b58e..6a43695 100644 --- a/pyrobolearn/models/gp.py +++ b/pyrobolearn/models/gp/gp.py @@ -10,7 +10,6 @@ differentiation: autograd), GPU capabilities, and more Pythonic approach. """ import copy - try: import cPickle as pickle except ImportError as e: @@ -20,7 +19,8 @@ import numpy as np import torch import gpytorch # import GPy -# from model import Model + +# from pyrobolearn.models.model import Model __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -84,6 +84,10 @@ class ExactGPModel(gpytorch.models.ExactGP): self.mean = mean self.kernel = kernel + ############## + # Properties # + ############## + @property def mean(self): r"""Return the GP prior mean; that is :math:`\mu(x)` from :math:`p(f|x) = N(\mu(x), K(x,x))`.""" @@ -115,6 +119,10 @@ class ExactGPModel(gpytorch.models.ExactGP): "{}".format(type(kernel))) self._kernel = kernel + ########### + # Methods # + ########### + def forward(self, x): r"""Return the prior probability density function :math:`p(f|x) = \mathcal{N}(. | \mu(x), K(x,x))`.""" mean_x = self.mean(x) @@ -593,7 +601,7 @@ class GPR(GP): # TESTS if __name__ == '__main__': import matplotlib.pyplot as plt - from utils.converter import torch_to_numpy + from pyrobolearn.utils.converter import torch_to_numpy # create input and output data x = torch.linspace(0, 1, 100) diff --git a/pyrobolearn/models/hmm/__init__.py b/pyrobolearn/models/hmm/__init__.py new file mode 100644 index 0000000..2c1ce9a --- /dev/null +++ b/pyrobolearn/models/hmm/__init__.py @@ -0,0 +1,3 @@ + +# import hmm +from .hmm import * diff --git a/pyrobolearn/models/hmm.py b/pyrobolearn/models/hmm/hmm.py similarity index 100% rename from pyrobolearn/models/hmm.py rename to pyrobolearn/models/hmm/hmm.py diff --git a/pyrobolearn/models/kmp/__init__.py b/pyrobolearn/models/kmp/__init__.py new file mode 100644 index 0000000..be02baf --- /dev/null +++ b/pyrobolearn/models/kmp/__init__.py @@ -0,0 +1,3 @@ + +# import kmp +from .kmp import * diff --git a/pyrobolearn/models/kmp.py b/pyrobolearn/models/kmp/kmp.py similarity index 100% rename from pyrobolearn/models/kmp.py rename to pyrobolearn/models/kmp/kmp.py diff --git a/pyrobolearn/models/model.py b/pyrobolearn/models/model.py index 7f5fb96..53fc90a 100644 --- a/pyrobolearn/models/model.py +++ b/pyrobolearn/models/model.py @@ -4,8 +4,9 @@ Dependencies: None """ - from abc import ABCMeta, abstractmethod +import copy +import numpy as np __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -18,26 +19,29 @@ __status__ = "Development" class Model(object): - r"""(Learning Base) Model (aka parametrized model) + r"""Learning Model (aka parametrized model) This abstract class must be inherited by any learning models, and provides a common API for all these models. It is often a simple wrapper over a specific learning model implemented in a certain library. - The learning model has no direct knowledge about the inputs and outputs (such as states / actions), and can be - used out of the box. In our framework, we refer these learning models as the 'inner models', and the class that - connects the inputs/outputs (such as states and actions) with the inner model as the 'outer model'. - Outer models are thus learning models that maps states / actions to states / actions, while inner models often - maps arrays to arrays (where an array can represent a scalar, a vector, a matrix, a tensor,...). In other words, - the outer model is a wrapper around the inner model but knows how to deal with state / action inputs and outputs. + The learning model has no direct knowledge about the inputs and outputs (such as `State` / `Action`), and can be + used out of the box. In the PyRoboLearn (PRL) framework, we refer these learning models as the 'inner models', and + the class that connects the inputs / outputs (such as states and actions) with the inner model as the 'outer model'. + Outer models are thus learning models that maps states / actions / arrays to states / actions / arrays, while inner + models only maps arrays to arrays (where an array can represent a scalar, a vector, a matrix, a tensor,...). In + other words, the outer model is a wrapper around the inner model but knows how to deal with state / action inputs + and outputs as well. For instance, neural networks are a popular kind of inner models which map arrays to arrays. These inner models - can be used to represent outer models such as rl (which map states to actions), dynamic models (which map + can be used to represent outer models such as policies (which map states to actions), dynamic models (which map states and actions to the next states), value estimators (which, for example, map states to a real number), transformation mappings (which map states to states, or actions to actions). Transformation mappings can for instance be used to map a human kinematic state to a robot kinematic state. - .. seealso: - * https://www.codecademy.com/en/forum_questions/512cd091ffeb9e603b005713 + The methods are partly inspired by `torch.nn.Module` [1]. + + References: + [1] `torch.nn`: https://pytorch.org/docs/stable/nn.html """ __metaclass__ = ABCMeta @@ -47,29 +51,74 @@ class Model(object): """ self._models = [] # TODO: should be a directed graph - self._input_shape = None - self._output_shape = None - ############## # Properties # ############## @property def models(self): + """Return the inner models.""" return self._models + @property + def input_size(self): + """Return the input size of the model.""" + shape = self.input_shape + if len(shape) > 0: + raise np.prod(shape) + return 0 + + @property + def output_size(self): + """Return the output size of the model.""" + shape = self.output_shape + if len(shape) > 0: + raise np.prod(shape) + return 0 + @property def input_shape(self): - return self._input_shape + """Return the input shape of the model.""" + raise NotImplementedError @property def output_shape(self): - return self._output_shape + """Return the output shape of the model.""" + raise NotImplementedError + + @property + def input_dim(self): + """Return the input dimension of the model; i.e. len(input_shape).""" + return len(self.input_shape) + + @property + def output_dim(self): + """Return the output dimension of the model; i.e. len(output_shape).""" + return len(self.output_shape) + + @property + def num_parameters(self): + """Return the number of parameters.""" + raise NotImplementedError + + @property + def num_hyperparameters(self): + """Return the number of hyperparameters.""" + raise NotImplementedError ################## # Static Methods # ################## + @staticmethod + def copy(other, deep=True): + """Return another copy of the learning model""" + if not isinstance(other, Model): + raise TypeError("Trying to copy an object which is not a Linear model") + if deep: + return copy.deepcopy(other) + return copy.copy(other) + @staticmethod def is_parametric(): """ @@ -144,26 +193,86 @@ class Model(object): """ raise NotImplementedError - # TODO: isClassifier, isRegressive, isSequential + # TODO: is_classifier, is_regressive, is_sequential ########### # Methods # ########### def has_models(self): + """ + Return True if the learning model has multiple learning models. + """ return len(self._models) > 0 def add_model(self, model): + """ + Add a model inside the list of inner models. + """ if not isinstance(model, Model): raise TypeError("Expecting the model to be an instance of Model.") if self.has_models(): # check that the output size of the last model is equal to the input size of the new model last_model = self._models[-1] - if last_model.output_dims() != model.input_dims(): + if last_model.output_size() != model.input_size(): # TODO pass self._models.append(model) + @abstractmethod + def parameters(self): + """Return an iterator over the parameters of the model.""" + raise NotImplementedError + + @abstractmethod + def named_parameters(self): + """Return an iterator over the model parameters, yielding both the name and the parameter itself.""" + raise NotImplementedError + + @abstractmethod + def list_parameters(self): + """Return the parameters in the form of a list.""" + raise NotImplementedError + + @abstractmethod + def hyperparameters(self): + """Return an iterator over the hyperparameters.""" + raise NotImplementedError + + @abstractmethod + def named_hyperparameters(self): + """Return an iterator over the model hyperparameters, yielding both the name and the hyperparameter itself.""" + raise NotImplementedError + + @abstractmethod + def list_hyperparameters(self): + """Return the hyperparameters in the form of a list.""" + raise NotImplementedError + + def get_vectorized_parameters(self, to_numpy=True): + """Return a vectorized form of the parameters""" + raise NotImplementedError + + def set_vectorized_parameters(self, vector): + """Set the vector parameters.""" + raise NotImplementedError + + def reset(self): + """Reset the learning model.""" + pass + + def train(self, *args, **kwargs): + """Set the model in training mode.""" + pass + + def eval(self): + """Set the model in evaluation mode.""" + pass + + def learn(self, *args, **kwargs): + """Learn the model (hyper-)parameters on the given data.""" + pass + @abstractmethod def _predict(self, x=None): """ @@ -172,60 +281,12 @@ class Model(object): """ raise NotImplementedError - def predict(self, x): + def predict(self, x=None): + """Predict the output using the learning model.""" if self.has_models(): return [model.predict(x) for model in self.models] return self._predict(x) - @abstractmethod - def parameters(self): - """ - Return an iterator over the parameters of the model. - """ - raise NotImplementedError - - @abstractmethod - def named_parameters(self): - """ - Return an iterator over the model parameters, yielding both the name and the parameter itself. - """ - raise NotImplementedError - - @abstractmethod - def get_params(self): - """ - Return the parameters in the form of a list or dictionary. - """ - raise NotImplementedError - - @abstractmethod - def hyperparameters(self): - """ - Return an iterator over the hyperparameters. - """ - raise NotImplementedError - - @abstractmethod - def get_hyperparams(self): - """ - Return the hyperparameters in the form of a list or dictionary. - """ - raise NotImplementedError - - @abstractmethod - def get_input_dims(self): - raise NotImplementedError - - # alias - # input_dims = getInputDims - - @abstractmethod - def get_output_dims(self): - raise NotImplementedError - - # alias - # output_dims = getOutputDims - @abstractmethod def save(self, filename): """ @@ -370,4 +431,3 @@ class Model(object): def __getitem__(self, key): pass - diff --git a/pyrobolearn/models/nn/__init__.py b/pyrobolearn/models/nn/__init__.py index d6a2716..7e113f9 100644 --- a/pyrobolearn/models/nn/__init__.py +++ b/pyrobolearn/models/nn/__init__.py @@ -9,16 +9,16 @@ from .mlp import * from .neat_model import NEATModel # import convolutional neural network -# from .cnn import * +# from cnn import * # import recurrent neural network -# from .rnn import * +# from rnn import * # import auto-encoder -# from .ae import * +# from ae import * # import variational auto-encoder -# from .vae import * +# from vae import * # import generative adversarial networks -# from .gan import * +# from gan import * diff --git a/pyrobolearn/models/nn/ae.py b/pyrobolearn/models/nn/ae.py index 55cbfc5..08ddd74 100644 --- a/pyrobolearn/models/nn/ae.py +++ b/pyrobolearn/models/nn/ae.py @@ -23,7 +23,7 @@ import inspect import numpy as np import torch -from dnn import NN +from pyrobolearn.models.nn.dnn import NN __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/models/nn/cnn.py b/pyrobolearn/models/nn/cnn.py index 12d264e..d12f0f3 100644 --- a/pyrobolearn/models/nn/cnn.py +++ b/pyrobolearn/models/nn/cnn.py @@ -23,7 +23,7 @@ import inspect import numpy as np import torch -from dnn import NN +from pyrobolearn.models.nn.dnn import NN __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/models/nn/dnn.py b/pyrobolearn/models/nn/dnn.py index 7f25b67..ff0ba06 100644 --- a/pyrobolearn/models/nn/dnn.py +++ b/pyrobolearn/models/nn/dnn.py @@ -54,7 +54,7 @@ class NN(object): # Model import torch.nn as nn model = nn.Sequential(nn.Conv2d(in_channels=3, out_channels=10, kernel_size=3), Flatten(), nn.Linear(320, 10)) - model = NN(model, input_dims=..., output_dims=...) + model = NN(model, input_size=..., output_size=...) References: [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 @@ -63,13 +63,13 @@ class NN(object): # Model # - nn.Sequential: https://pytorch.org/docs/master/_modules/torch/nn/modules/container.html#Sequential """ - def __init__(self, model, input_dims, output_dims, framework=None): + def __init__(self, model, input_shape, output_shape, framework=None): r"""Initialize the NN model. Args: model (torch.nn.Module or keras.base_layer.Layer): pytorch/keras model - input_dims (int, tuple/list of int): dimensions of the input - output_dims (int, tuple/list of int): dimensions of the output + input_shape (int, tuple/list of int): dimensions of the input + output_shape (int, tuple/list of int): dimensions of the output """ super(NN, self).__init__() @@ -84,8 +84,8 @@ class NN(object): # Model # set model (written in the specified framework) self.model = model - self.input_dims = input_dims - self.output_dims = output_dims + self._input_shape = input_shape + self._output_shape = output_shape # TODO: infer the framework based on the model self.framework = framework @@ -96,22 +96,56 @@ class NN(object): # Model @property def model(self): + """Return the inner learning model.""" return self._model @model.setter def model(self, model): + """Set the inner learning model.""" if model is not None: - if not (isinstance(model, torch.nn.Module) or isinstance(model, keras.models.Model)): + if not (isinstance(model, torch.nn.Module)): # or isinstance(model, keras.models.Model)): raise TypeError("The model should be an instance of torch.nn.Module or keras.models.Model") self._model = model + @property + def input_size(self): + """Return the input size of the model.""" + return np.prod(self.input_shape) + + @property + def output_size(self): + """Return the output size of the model.""" + return np.prod(self.output_shape) + @property def input_shape(self): # TODO - return self.input_dims + """Return the input shape.""" + return self._input_shape @property def output_shape(self): # TODO - return self.output_dims + """Return the output shape.""" + return self._output_shape + + @property + def input_dim(self): + """Return the input dimension.""" + return len(self.input_shape) + + @property + def output_dim(self): + """Return the output dimension.""" + return len(self.output_shape) + + @property + def num_parameters(self): + """Return the total number of trainable parameters.""" + return sum(p.numel() for p in self.parameters() if p.requires_grad) + + @property + def num_hyperparameters(self): + """Return the number of hyperparameters.""" + return len(list(self.hyperparameters())) ################## # Static Methods # @@ -155,30 +189,48 @@ class NN(object): # Model # Methods # ########### - def predict(self, x=None): - return self.model(x) - - def get_input_dims(self): - return self.input_dims - - def get_output_dims(self): - return self.output_dims - def parameters(self): + """Return an iterator over the model parameters.""" return self.model.parameters() - def get_params(self): + def named_parameters(self): + """Return an iterator over the model parameters, yielding both the name and the parameter itself.""" + return self.model.named_parameters() + + def list_parameters(self): + """Return a list of parameters.""" return list(self.parameters()) - def get_hyperparams(self): - """ - Return the number of units per layer, the number of layers, and the type of layers. - """ + def hyperparameters(self): + """Return an iterator over the model hyper-parameters; this includes the number of units per layer, the number + of layers, the activation functions, etc.""" raise NotImplementedError - def hyperparameters(self): + def named_hyperparameters(self): + """Return an iterator over the model hyper-parameters, yielding both the name and the parameter itself.""" raise NotImplementedError + def list_hyperparameters(self): + """Return a list of the hyper-parameters; this includes the number of units per layer, the number of layers, + the activation functions, etc.""" + raise NotImplementedError + + def predict(self, x=None, to_numpy=False): + """Predict the output given the input.""" + # convert to torch tensor if necessary + if isinstance(x, np.ndarray): + x = torch.from_numpy(x).float() + + # predict output given input + x = self.model(x) + + # return the output (and convert it to numpy if specified) + if to_numpy: + if x.requires_grad: + return x.detach().numpy() + return x.numpy() + return x + ############# # Operators # ############# @@ -228,8 +280,8 @@ class NNTorch(NN): r"""Neural Network written in PyTorch """ - def __init__(self, model, input_dims, output_dims): - super(NNTorch, self).__init__(model, input_dims, output_dims, framework='pytorch') + def __init__(self, model, input_shape, output_shape): + super(NNTorch, self).__init__(model, input_shape, output_shape, framework='pytorch') def save(self, filename): """ diff --git a/pyrobolearn/models/nn/gan.py b/pyrobolearn/models/nn/gan.py index b33f64a..8cec17d 100644 --- a/pyrobolearn/models/nn/gan.py +++ b/pyrobolearn/models/nn/gan.py @@ -25,7 +25,7 @@ import inspect import numpy as np import torch -from dnn import NN +from pyrobolearn.models.nn.dnn import NN __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/models/nn/mlp.py b/pyrobolearn/models/nn/mlp.py index 8221619..d0b40d8 100644 --- a/pyrobolearn/models/nn/mlp.py +++ b/pyrobolearn/models/nn/mlp.py @@ -59,7 +59,8 @@ class MLP(NN): Args: num_units (list/tuple of int): number of units in each layer (this includes the input and output layer) activation_fct (None, str, or list/tuple of str/None): activation function to be applied after each layer. - If list/tuple, then it has to match the + If list/tuple, then it has to match the number of + hidden layers. last_activation_fct (None or str): last activation function to be applied. If not specified, it will check if it is in the list/tuple of activation functions provided for the previous argument. @@ -77,7 +78,8 @@ class MLP(NN): raise ValueError("The current frameworks allowed are pytorch and keras") # instantiate the super class - super(MLP, self).__init__(model.model, input_dims=num_units[0], output_dims=num_units[-1], framework=framework) + super(MLP, self).__init__(model.model, input_shape=tuple([num_units[0]]), output_shape=tuple([num_units[-1]]), + framework=framework) # rewrite methods self.save = model.save @@ -168,12 +170,12 @@ class MLPTorch(NNTorch): # create nn model model = torch.nn.Sequential(*layers) - super(MLPTorch, self).__init__(model, input_dims=num_units[0], output_dims=num_units[-1]) + super(MLPTorch, self).__init__(model, input_shape=num_units[0], output_shape=num_units[-1]) # Tests if __name__ == '__main__': # create MLP network - mlp = MLPTorch(num_units=(2,10,3), activation_fct='relu') + mlp = MLPTorch(num_units=(2, 10, 3), activation_fct='relu') print(mlp) diff --git a/pyrobolearn/models/nn/neat_model.py b/pyrobolearn/models/nn/neat_model.py index 1fd998d..c50666b 100644 --- a/pyrobolearn/models/nn/neat_model.py +++ b/pyrobolearn/models/nn/neat_model.py @@ -181,10 +181,12 @@ class NEATModel(object): # Model): @property def genome(self): + """Return the genome.""" return self._genome @genome.setter def genome(self, genome): + """Set the genome.""" if not isinstance(genome, neat.genome.DefaultGenome): raise TypeError("Expecting genome to be an instance of neat.genome.DefaultGenome type") self._genome = genome @@ -194,6 +196,7 @@ class NEATModel(object): # Model): @property def network(self): + """Return the network model.""" return self.model ################## @@ -202,26 +205,32 @@ class NEATModel(object): # Model): @staticmethod def is_parametric(): + """The NEAT model is a parametric model.""" return True @staticmethod def is_linear(): + """The NEAT model is non-linear in general.""" return False @staticmethod def is_recurrent(): + """The NEAT model can be recurrent.""" return True @staticmethod def is_probabilistic(): + """The NEAT model is a non probabilistic model.""" return False @staticmethod def is_discriminative(): + """The NEAT model is a discriminative model.""" return True @staticmethod def is_generative(): + """The NEAT model is not a generative model.""" return False @staticmethod @@ -238,6 +247,35 @@ class NEATModel(object): # Model): # Methods # ########### + def parameters(self): + """Returns an iterator over the model parameters.""" + return [] + + def named_parameters(self): + """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" + return [], [] + + def hyperparameters(self): + """Returns an iterator over the model hyper-parameters.""" + return [] + + def named_hyperparameters(self): + """Returns an iterator over the model hyper-parameters, yielding both the name and the hyper-parameter + itself.""" + return [], [] + + def list_parameters(self): + """Return the list of parameters.""" + return list(self.parameters()) + + def list_hyperparameters(self): + """Return the list of hyper-parameters.""" + return list(self.hyperparameters()) + + def reset(self): + """Reset the learning model.""" + pass + def _create_config_str(self, config_dict=None): """Create string describing the config file from a config dictionary""" if config_dict is None: @@ -267,6 +305,7 @@ class NEATModel(object): # Model): return filename def set_network(self, genome=None, config=None): + """Set the network.""" # check arguments if genome is None: genome = self.genome @@ -284,6 +323,7 @@ class NEATModel(object): # Model): return self.model def update_config(self, config): + """Update the configuration.""" # update config (dict) if isinstance(config, dict): self.config_dict.update(config) @@ -302,24 +342,8 @@ class NEATModel(object): # Model): # set new network self.model = self.set_network(self.genome, self.config) - def parameters(self): - """Returns an iterator over the model parameters.""" - return [] - - def named_parameters(self): - """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" - return [] - - def hyperparameters(self): - pass - - def get_params(self): - pass - - def get_hyperparams(self): - pass - def predict(self, x=None): + """Predict the output of the model given the input.""" return self.model.activate(x) def save(self, filename): diff --git a/pyrobolearn/models/nn/rcnn.py b/pyrobolearn/models/nn/rcnn.py index ab2914d..4f0393b 100644 --- a/pyrobolearn/models/nn/rcnn.py +++ b/pyrobolearn/models/nn/rcnn.py @@ -24,7 +24,7 @@ import inspect import numpy as np import torch -from dnn import NN +from pyrobolearn.models.nn.dnn import NN __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/models/nn/rnn.py b/pyrobolearn/models/nn/rnn.py index f65da33..1fa3972 100644 --- a/pyrobolearn/models/nn/rnn.py +++ b/pyrobolearn/models/nn/rnn.py @@ -23,7 +23,7 @@ import inspect import numpy as np import torch -from dnn import NN +from pyrobolearn.models.nn.dnn import NN __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" diff --git a/pyrobolearn/models/nn/vae.py b/pyrobolearn/models/nn/vae.py index 0f14261..da7ede0 100644 --- a/pyrobolearn/models/nn/vae.py +++ b/pyrobolearn/models/nn/vae.py @@ -23,7 +23,7 @@ import inspect import numpy as np import torch -from dnn import NN +from pyrobolearn.models.nn.dnn import NN __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" @@ -50,13 +50,17 @@ class VAE(NN): self.encoder = None self.decoder = None + ################## + # Static methods # + ################## + @staticmethod - def isDiscriminative(): + def is_discriminative(): """A neural network is a discriminative model which given inputs predicts some outputs""" return True @staticmethod - def isGenerative(): # unless VAE, GAN,... + def is_generative(): # unless VAE, GAN,... """Standard neural networks are not generative, and thus we can not sample from it. This is different, for instance, for generative adversarial networks (GANs) and variational auto-encoders (VAEs).""" return True diff --git a/pyrobolearn/models/promp/__init__.py b/pyrobolearn/models/promp/__init__.py new file mode 100644 index 0000000..bdf5b33 --- /dev/null +++ b/pyrobolearn/models/promp/__init__.py @@ -0,0 +1,3 @@ + +# import promp +from .promp import * diff --git a/pyrobolearn/models/promp.py b/pyrobolearn/models/promp/promp.py similarity index 99% rename from pyrobolearn/models/promp.py rename to pyrobolearn/models/promp/promp.py index b129d49..a76e78b 100755 --- a/pyrobolearn/models/promp.py +++ b/pyrobolearn/models/promp/promp.py @@ -5,13 +5,12 @@ This file defines the Probabilistic Movement Primitive (ProMP) model, and use th in `gaussian.py` """ - from abc import ABCMeta, abstractmethod import numpy as np from scipy.linalg import block_diag import scipy.interpolate -# from pyrobolearn.models.model import Model +from pyrobolearn.models.model import Model from pyrobolearn.models.gaussian import Gaussian @@ -25,7 +24,6 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" - ################### Canonical Systems ####################### class CS(object): diff --git a/pyrobolearn/processors/__init__.py b/pyrobolearn/processors/__init__.py index 2c3b9f0..c2eb1e4 100644 --- a/pyrobolearn/processors/__init__.py +++ b/pyrobolearn/processors/__init__.py @@ -1,9 +1,9 @@ # import processors -from processor import Processor +from .processor import Processor # import basic processors (center, normalize, standardize, etc.) -from basic_processors import * +from .basic_processors import * # import linear processors -from linear_processor import LinearProcessor +from .linear_processor import LinearProcessor diff --git a/pyrobolearn/processors/linear_processor.py b/pyrobolearn/processors/linear_processor.py index 2dac3d9..36dc9fa 100644 --- a/pyrobolearn/processors/linear_processor.py +++ b/pyrobolearn/processors/linear_processor.py @@ -24,13 +24,22 @@ class LinearProcessor(Processor): """ def __init__(self, a, b): + """ + Initialize the linear processor. + + Args: + a (torch.Tensor, np.array): weight + b (torch.Tensor, np.array): bias + """ super(LinearProcessor, self).__init__() self.a = torch.tensor(a, dtype=torch.float) self.b = torch.tensor(b, dtype=torch.float) def reset(self): + """Reset the linear processor.""" pass @convert_numpy def compute(self, x): + """Compute the linear output given the input :attr:`x`.""" return self.a * x + self.b diff --git a/pyrobolearn/processors/processor.py b/pyrobolearn/processors/processor.py index d65963d..2a1fd93 100644 --- a/pyrobolearn/processors/processor.py +++ b/pyrobolearn/processors/processor.py @@ -38,7 +38,9 @@ def convert_numpy(f): x = f(self, x) # reconvert to numpy array if specified, and return it - if to_numpy: + if to_numpy and isinstance(x, torch.Tensor): + if x.requires_grad: + return x.detach().numpy() return x.numpy() # return torch Tensor @@ -56,14 +58,18 @@ class Processor(object): """ def __init__(self): + """Initialize the processor.""" pass def reset(self): + """Reset the processor.""" pass @convert_numpy def compute(self, x): + """Compute the output given the input :attr:`x`.""" pass def __call__(self, x, to_numpy=False): + """Alias: call :func:`compute` to compute the output given the input :attr:`x`.""" return self.compute(x, to_numpy=to_numpy) diff --git a/pyrobolearn/simulators/bullet.py b/pyrobolearn/simulators/bullet.py index 8890f55..f21545a 100644 --- a/pyrobolearn/simulators/bullet.py +++ b/pyrobolearn/simulators/bullet.py @@ -1323,16 +1323,22 @@ class Bullet(Simulator): if max_velocity is not None: kwargs['maxVelocity'] = max_velocity self.sim.setJointMotorControl2(body_id, joint_ids, controlMode=control_mode, **kwargs) - else: + else: # joint_ids is a list if positions is not None: kwargs['targetPositions'] = positions if velocities is not None: kwargs['targetVelocities'] = velocities if forces is not None: + if isinstance(forces, (int, float)): + forces = [forces] * len(joint_ids) kwargs['forces'] = forces if kp is not None: + if isinstance(kp, (int, float)): + kp = [kp] * len(joint_ids) kwargs['positionGains'] = kp if kd is not None: + if isinstance(kd, (int, float)): + kd = [kd] * len(joint_ids) kwargs['velocityGains'] = kd self.sim.setJointMotorControlArray(body_id, joint_ids, controlMode=control_mode, **kwargs) @@ -2663,16 +2669,16 @@ class Bullet(Simulator): aabb_min, aabb_max = self.sim.getAABB(body_id, link_id) return np.array(aabb_min), np.array(aabb_max) - def get_contact_points(self, body1, body2, link1_id=None, link2_id=None): + def get_contact_points(self, body1, body2=None, link1_id=None, link2_id=None): """ Returns the contact points computed during the most recent call to `step`. Args: body1 (int): only report contact points that involve body A - body2 (int): only report contact points that involve body B. Important: you need to have a valid body A - if you provide body B - link1_id (int): only report contact points that involve link index of body A - link2_id (int): only report contact points that involve link index of body B + body2 (int, None): only report contact points that involve body B. Important: you need to have a valid body + A if you provide body B + link1_id (int, None): only report contact points that involve link index of body A + link2_id (int, None): only report contact points that involve link index of body B Returns: list: diff --git a/pyrobolearn/simulators/simulator.py b/pyrobolearn/simulators/simulator.py index 1297e8f..0c887c9 100644 --- a/pyrobolearn/simulators/simulator.py +++ b/pyrobolearn/simulators/simulator.py @@ -1794,16 +1794,16 @@ class Simulator(object): """ pass - def get_contact_points(self, body1, body2, link1_id=None, link2_id=None): + def get_contact_points(self, body1, body2=None, link1_id=None, link2_id=None): """ Returns the contact points computed during the most recent call to `step`. Args: body1 (int): only report contact points that involve body A - body2 (int): only report contact points that involve body B. Important: you need to have a valid body A - if you provide body B - link1_id (int): only report contact points that involve link index of body A - link2_id (int): only report contact points that involve link index of body B + body2 (int, None): only report contact points that involve body B. Important: you need to have a valid + body A if you provide body B + link1_id (int, None): only report contact points that involve link index of body A + link2_id (int, None): only report contact points that involve link index of body B Returns: list: