diff --git a/README.md b/README.md index 6d57772..a3c42e8 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,8 @@ # PyRoboLearn This repository contains the code for the *PyRoboLearn* (PRL) framework: a Python framework for Robot Learning. -This framework revolves mainly around 7 axes: simulators, worlds, robots, interfaces, learning tasks (= environment -+ policy), learning models, and learning algorithms. +This framework revolves mainly around 7 axes: simulators, worlds, robots, interfaces, learning tasks (= environment and policy), learning models, and learning algorithms. ## Requirements -The framework has been tested with Python 2.7 and Ubuntu 16.04. \ No newline at end of file +The framework has been tested with Python 2.7 and Ubuntu 16.04. diff --git a/pyrobolearn/__init__.py b/pyrobolearn/__init__.py index c9a0d47..b1f650a 100644 --- a/pyrobolearn/__init__.py +++ b/pyrobolearn/__init__.py @@ -52,6 +52,9 @@ import tasks # import metrics +# import optimizers +# import optim + # import algos import algos @@ -62,7 +65,7 @@ import algos __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/actions/action.py b/pyrobolearn/actions/action.py index 8a6d3ad..346c67a 100644 --- a/pyrobolearn/actions/action.py +++ b/pyrobolearn/actions/action.py @@ -15,7 +15,7 @@ from pyrobolearn.utils.data_structures.orderedset import OrderedSet __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/actions/gym_actions.py b/pyrobolearn/actions/gym_actions.py index bfdffb3..b018823 100644 --- a/pyrobolearn/actions/gym_actions.py +++ b/pyrobolearn/actions/gym_actions.py @@ -13,7 +13,7 @@ from action import Action __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/actions/robot_actions/joint_actions.py b/pyrobolearn/actions/robot_actions/joint_actions.py index 3276a0d..b3da585 100644 --- a/pyrobolearn/actions/robot_actions/joint_actions.py +++ b/pyrobolearn/actions/robot_actions/joint_actions.py @@ -12,7 +12,7 @@ from robot_actions import RobotAction __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/actions/robot_actions/link_actions.py b/pyrobolearn/actions/robot_actions/link_actions.py index 5d5a6f1..ae96560 100644 --- a/pyrobolearn/actions/robot_actions/link_actions.py +++ b/pyrobolearn/actions/robot_actions/link_actions.py @@ -11,7 +11,7 @@ from robot_actions import RobotAction __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/actions/robot_actions/robot_actions.py b/pyrobolearn/actions/robot_actions/robot_actions.py index 71b6fda..9024dc3 100644 --- a/pyrobolearn/actions/robot_actions/robot_actions.py +++ b/pyrobolearn/actions/robot_actions/robot_actions.py @@ -16,7 +16,7 @@ from pyrobolearn.robots import Robot __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/backends/__init__.py b/pyrobolearn/backends/__init__.py new file mode 100644 index 0000000..dfcc0c5 --- /dev/null +++ b/pyrobolearn/backends/__init__.py @@ -0,0 +1,27 @@ +# The various backends allows to specify which framework we want to use + +import os + +# check if the 'PYROBOLEARN_BACKEND' environment variable has been defined +if 'PYROBOLEARN_BACKEND' not in os.environ: + os.environ['PYROBOLEARN_BACKEND'] = 'torch' + +# provide the various backends +if os.environ['PYROBOLEARN_BACKEND'] == 'torch': + import torch_backend as backend +elif os.environ['PYROBOLEARN_BACKEND'] == 'numpy': + import numpy_backend as backend +else: + pass + +# alias +b = backend + + +# Tests # + +# create array +a = b.array([1, 2, 3]) +print(type(a)) +print(a) +print(a.reshape((3, 1))) diff --git a/pyrobolearn/backends/numpy_backend.py b/pyrobolearn/backends/numpy_backend.py new file mode 100644 index 0000000..ae98620 --- /dev/null +++ b/pyrobolearn/backends/numpy_backend.py @@ -0,0 +1,20 @@ + +from autograd import * +from autograd.numpy import * +import autograd.numpy as np +import numpy + + +def array(data, dtype=None, copy=True, device=None, requires_grad=False, ndmin=0): + return np.array(data, dtype=dtype, copy=copy, ndmin=ndmin) + + +def inv(data, out=None): + # TODO: inverse not in autograd.numpy + if out is None: + return numpy.linalg.inv(data) + + # inplace + output = numpy.linalg.inv(data) + for i in range(len(out)): + out[i] = output[i] diff --git a/pyrobolearn/backends/tensorflow_backend.py b/pyrobolearn/backends/tensorflow_backend.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/backends/torch_backend.py b/pyrobolearn/backends/torch_backend.py new file mode 100644 index 0000000..3322b58 --- /dev/null +++ b/pyrobolearn/backends/torch_backend.py @@ -0,0 +1,18 @@ + +import torch +from torch import * + + +def array(data, dtype=None, copy=True, device=None, requires_grad=False, ndmin=0): + return torch.tensor(data, dtype=dtype, device=device, requires_grad=requires_grad) + + +def inv(data, out=None): + return torch.inverse(data, out=out) + + +def concatenate(data, axis=0, out=None): + return torch.cat(data, axis, out) + +# np.size vs torch.nelement() vs torch.size() + diff --git a/pyrobolearn/filters/__init__.py b/pyrobolearn/filters/__init__.py new file mode 100644 index 0000000..072a04f --- /dev/null +++ b/pyrobolearn/filters/__init__.py @@ -0,0 +1,18 @@ + +# import basic filter +from filter import * + +# import histogram filter +from histogram_filter import HistogramFilter + +# import kalman filter +from kalman_filter import KalmanFilter + +# import extended kalman filter +from extended_kalman_filter import EKF + +# import unscented kalman filter +from unscented_kalman_filter import UKF + +# import particle filter +from particle_filter import ParticleFilter diff --git a/pyrobolearn/filters/description.txt b/pyrobolearn/filters/description.txt new file mode 100644 index 0000000..540ec8f --- /dev/null +++ b/pyrobolearn/filters/description.txt @@ -0,0 +1 @@ +filters --> state estimators diff --git a/pyrobolearn/filters/extended_kalman_filter.py b/pyrobolearn/filters/extended_kalman_filter.py new file mode 100644 index 0000000..0966319 --- /dev/null +++ b/pyrobolearn/filters/extended_kalman_filter.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +"""Define the Extented Kalman Filter. +""" + +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 EKF(object): + r"""Extended Kalman Filter (EKF) + + Type: Gaussian filter (=Bayes filter for continuous spaces) + + This is an extension and generalization to the standard Kalman filter which also works with nonlinear systems. + This is more practical as state transitions and measurements are seldomly linear. The EKF calculates a Gaussian + distribution to the true belief, and represents thus an approximate of the belief (i.e. posterior). + + The dynamic model :math:`p(x_t | x_{t-1}, u_t)` and the measurement model :math:`p(z_t | x_t)` are given by: + + .. math:: + + x_t = f(x_{t-1}, u_t) + \epsilon_t \qquad \mbox{where} \qquad \epsilon_t \sim \mathcal{N}(0, R_t) + z_t = h(x_t) + \delta_t \qquad \mbox{where} \qquad \delta_t \sim \mathcal{N}(0, Q_t) + + where :math:`f` and :math:`h` are nonlinear functions. + + Because EKF cannot compute the true statistics in closed form, it uses linearization (via Taylor expansion). + That is, it approximates :math:`f` and :math:`h` by a linear function. Note that another way would be to compute + a Monte-carlo estimate of the Gaussian but would require a higher time-complexity (dependent on the number of + samples). + + In a mathematical language, the linearization via Taylor expansion is given by: + + .. math:: + + f(x_{t-1}, u_t) = f(\mu_{t-1}, u_t) + f'(\mu_{t-1}, u_t) (x_{t-1} - \mu_{t-1}) + h(x_t) = h(\mu_t) + h'(\mu_t) (x_t - \mu_t) + + where :math:`f'(\mu_{t-1}, u_t)` and :math:` h'(\mu_t)` represent the Jacobians evaluated at the means. + + Notes: EKF requires to compute the Jacobians for the dynamical and measurement models. + + Complexity: :math:`O(d^3 + n^2)` because of the matrix inversion when computing the kalman gain. :math:`d` is the + dimension of the measurement vector, and :math:`n` is the dimension of the state space. + + References: + [1] "Probabilistic Robotics", Thrun et al., 2006 (sec 3.3) + """ + + def __init__(self, mean, covariance, dynamic_noise_cov, measurement_noise_cov): + """ + Initialize the Extended Kalman Filter. + + Args: + mean (float[N]): initial mean of the state belief + covariance (float[N,N]): initial covariance matrix of the state belief + dynamic_noise_cov (float[N,N]): dynamic noise covariance matrix + measurement_noise_cov (float[K,K]): measurement noise covariance matrix + """ + self.mu = mean + self.Sigma = covariance + self.R = dynamic_noise_cov + self.Q = measurement_noise_cov + self.I = np.identity(self.Sigma.shape[0]) + + def predict(self, f, u): + """ + Predict (a priori) the next state (without taking into account measurements) using the nonlinear dynamic model + with added Gaussian noise. + + Args: + f (callable class): (nonlinear) dynamical function. This should have a method `jacobian`. + u (np.array): control array + + Returns: + float[N]: a priori mean of the Gaussian belief + float[N,N]: a priori covariance of the Gaussian belief + """ + F = f.jacobian(self.mu, u) + + # predict a priori the next state (by only using the nonlinear dynamic model) + self.mu = f(self.mu, u) + self.Sigma = F.dot(self.Sigma).dot(F.T) + self.R + + # return the a priori Gaussian belief on the next state + return self.mu, self.Sigma + + def measurement_update(self, h, z): + """ + Predict (a posteriori) the next state by incorporating the measurement :math:`z_t`. + + Args: + h (callable class): (nonlinear) measurement function. This should have a method `jacobian`. + z (np.array): measurement array + + Returns: + float[N]: a posteriori mean of the Gaussian belief + float[N,N]: a posteriori covariance matrix of the Gaussian belief + """ + H = h.jacobian(self.mu) + + # deviation error between the measurement and the a priori predicted state (aka innovation) + y = z - h(self.mu) + + # Innovation covariance matrix + S = H.dot(self.Sigma).dot(H.T) + self.Q + + # Kalman gain (which specifies the degree to which the measurement is incorporated into the new state) + K = self.Sigma.dot(H.T).dot(np.linalg.inv(S)) + + # predict a posteriori the next state + self.mu = self.mu + K.dot(y) + self.Sigma = (self.I - K.dot(H)).dot(self.Sigma) + + # return the a posteriori Gaussian belief on the next state + return self.mu, self.Sigma + + def compute(self, f, u, h, z): + """ + Perform a prediction and measurement update step. + + Args: + f (callable class): (nonlinear) dynamical function. This should have a method `jacobian`. + u (np.array): control array + h (callable class): (nonlinear) measurement function. This should have a method `jacobian`. + z (np.array): measurement array + + Returns: + float[N]: a posteriori mean vector of the Gaussian belief + float[N,N]: a posteriori covariance matrix of the Gaussian belief + """ + self.predict(f, u) + self.measurement_update(h, z) + return self.mu, self.Sigma diff --git a/pyrobolearn/filters/filter.py b/pyrobolearn/filters/filter.py new file mode 100644 index 0000000..cd64e74 --- /dev/null +++ b/pyrobolearn/filters/filter.py @@ -0,0 +1,8 @@ +# This file provides some common filters used in signal processing + +import scipy + + +class Filter(object): + r"""Filter abstract class""" + pass diff --git a/pyrobolearn/filters/histogram_filter.py b/pyrobolearn/filters/histogram_filter.py new file mode 100644 index 0000000..1412e1f --- /dev/null +++ b/pyrobolearn/filters/histogram_filter.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python +"""Define the Histogram Filter. +""" + +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 HistogramFilter(object): + r"""Histogram Filter (HF) + + Type: Non-parametric filter + + The histogram filter decomposes the continuous state space into finite regions, and represents the posterior + by a histogram. [1] + + Notes: + - this filter is well-suited to represent complex multimodal belief [1] + - the complexity depends on the number of parameters + + Complexity: :math:`O(M^N)` where :math:`M` is the number of regions/bins, and :math:`N` is the dimensionality + of the state. + + References: + [1] "Probabilistic Robotics", Thrun et al., 2006 (sec 4.1) + """ + + def __init__(self, state_dim=1, num_bins_per_dim=10): + self.num_bins = num_bins_per_dim + self.state_dim = state_dim + + # total number of parameters (=regions) + self.num_bins = num_bins_per_dim**state_dim + + # initial belief: uniform distribution + p = 1./self.num_bins + self.p = np.full([num_bins_per_dim]*state_dim, p) + + def predict(self, f, u): + """ + Predict (a priori) the next state (without taking into account measurements) using the probabilistic + nonlinear dynamic model. + + Args: + f (callable class/function): probabilistic (nonlinear) dynamical function that stacks on the axis 0 the + probability distribution for each region. That is the size of axis 0 should be + :math:`num(bins)^{dim(state)}`. + u (np.array): control array + """ + # probability to arrive in region k given control input u from any region i (convolution operation) + s = ''.join([chr(i) for i in range(97, 97 + self.state_dim + 1)]) + self.p = np.einsum(s+','+s[1:]+'->'+s[1:], f(self.p.shape, u), self.p) + + # return belief + return self.p + + def measurement_update(self, h, z): + """ + Predict (a posteriori) the next state by incorporating the measurement :math:`z_t`. + + Args: + h (callable class/function): probabilistic (nonlinear) measurement function to see measurement from + region k. This function should return an array of the same shape as the one provided in argument + where each cell contains the probability to see the measurement `z` from that region (i.e. index). + z (np.array): measurement array + """ + # probability to see measurement z from region k (multiplication operation) + self.p = h(z, self.p.shape) * self.p + + # normalize to get a proper probability distribution + self.p /= np.sum(self.p) + + # return belief + return self.p + + def compute(self, f, u, h, z): + """ + Perform a prediction and measurement update step. + + Args: + f (callable class/function): probabilistic (nonlinear) dynamical function that stacks on the axis 0 the + probability distribution for each region. That is the size of axis 0 should be + :math:`num(bins)^{dim(state)}`. + u (np.array): control array + h (callable class/function): probabilistic (nonlinear) measurement function to see measurement from + region k. This function should return an array of the same shape as the one provided in argument + where each cell contains the probability to see the measurement `z` from that region (i.e. index). + z (np.array): measurement array + """ + self.predict(f, u) + self.measurement_update(h, z) + return self.p diff --git a/pyrobolearn/filters/kalman_filter.py b/pyrobolearn/filters/kalman_filter.py new file mode 100644 index 0000000..3fb3514 --- /dev/null +++ b/pyrobolearn/filters/kalman_filter.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python +"""Define the Kalman Filter state estimator. +""" + +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 KalmanFilter(object): + r"""Kalman Filter (KF) + + Type: Gaussian filter (=Bayes filter for continuous spaces) + + The Kalman filter (also known as the linear quadratic estimator (LQE)) is a state estimator which "uses a series + of measurements observed over time, containing statistical noise and other inaccuracies, and produces estimates + of unknown variables that tend to be more accurate than those based on a single measurement alone, by estimating + a joint probability distribution over the variables for each timeframe." (Wikipedia) + + Assumptions: + * Markov assumption + * linear dynamic model with added Gaussian noise + * linear measurement model with added Gaussian noise + * initial belief on the state is normally distributed + + The Kalman filter consists of two steps: + * Prediction step: produces the a priori estimate of the current state variables, along with their + corresponding uncertainties, based on the dynamical model. + * Measurement update step: produces the a posteriori estimate of the state along with its uncertainty, + by taking into account the received measurements, comparing and incorporating them with the above + prediction. + + The dynamic model :math:`p(x_t | x_{t-1}, u_t)` and the measurement model :math:`p(z_t | x_t)` are given by: + + .. math:: + + x_t = A_t x_{t-1} + B_t u_t + \epsilon_t \qquad \mbox{where} \qquad \epsilon_t \sim \mathcal{N}(0, R_t) + z_t = C_t x_t + \delta_t \qquad \mbox{where} \qquad \delta_t \sim \mathcal{N}(0, Q_t) + + Notes: The use of the KF " does not assume that the errors are Gaussian. However, the filter yields the exact + conditional probability estimate in the special case that all errors are Gaussian." (Wikipedia) + + Complexity: :math:`O(d^3 + n^2)` because of the matrix inversion when computing the kalman gain. :math:`d` is the + dimension of the measurement vector, and :math:`n` is the dimension of the state space. + + References: + [1] "A New Approach to Linear Filtering and Prediction Problems", Kalman, 1960 + [2] "Probabilistic Robotics", Thrun et al., 2006 (sec 3.2) + """ + + def __init__(self, mean, covariance, dynamic_noise_cov, measurement_noise_cov): + """ + Initialize the Kalman filter. + + Args: + mean (float[N]): initial mean of the state belief + covariance (float[N,N]): initial covariance matrix of the state belief + dynamic_noise_cov (float[N,N]): dynamic noise covariance matrix + measurement_noise_cov (float[K,K]): measurement noise covariance matrix + """ + # self.belief = Gaussian(...) + self.mu = mean + self.Sigma = covariance + self.R = dynamic_noise_cov + self.Q = measurement_noise_cov + self.I = np.identity(self.Sigma.shape[0]) + + def predict(self, A, B, u): + """ + Predict (a priori) the next state (without taking into account measurements) using the linear dynamic model + with added Gaussian noise. + + Args: + A (float[N,N]): linear state transformation matrix + B (float[N,M]): linear control transformation matrix + u (float[M]): control vector + + Returns: + float[N]: a priori mean of the Gaussian belief + float[N,N]: a priori covariance of the Gaussian belief + """ + # predict a priori the next state (by only using the linear dynamic model) + self.mu = A.dot(self.mu) + B.dot(u) + self.Sigma = A.dot(self.Sigma).dot(A.T) + self.R + + # return the a priori Gaussian belief on the next state + return self.mu, self.Sigma + + def measurement_update(self, C, z): + """ + Predict (a posteriori) the next state by incorporating the measurement :math:`z_t`. + + Args: + C (float[K,N]): linear state-measurement transformation matrix + z (float[K]): measurement vector + + Returns: + float[N]: a posteriori mean vector of the Gaussian belief + float[N,N]: a posteriori covariance matrix of the Gaussian belief + """ + # deviation error between the measurement and the a priori predicted state (aka innovation) + y = z - C.dot(self.mu) + + # Innovation covariance matrix + S = C.dot(self.Sigma).dot(C.T) + self.Q + + # Kalman gain (which specifies the degree to which the measurement is incorporated into the new state) + K = self.Sigma.dot(C.T).dot(np.linalg.inv(S)) + + # predict a posteriori the next state + self.mu = self.mu + K.dot(y) + self.Sigma = (self.I - K.dot(C)).dot(self.Sigma) + + # return the a posteriori Gaussian belief on the next state + return self.mu, self.Sigma + + def compute(self, A, B, u, C, z): + """ + Perform a prediction and measurement update step. + + Args: + A (float[N,N]): linear state transformation matrix + B (float[N,M]): linear control transformation matrix + u (float[M]): control vector + C (float[K,N]): linear state-measurement transformation matrix + z (float[K]): measurement vector + + Returns: + float[N]: a posteriori mean vector of the Gaussian belief + float[N,N]: a posteriori covariance matrix of the Gaussian belief + """ + self.predict(A, B, u) + self.measurement_update(C, z) + return self.mu, self.Sigma diff --git a/pyrobolearn/filters/particle_filter.py b/pyrobolearn/filters/particle_filter.py new file mode 100644 index 0000000..af3df6d --- /dev/null +++ b/pyrobolearn/filters/particle_filter.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python +"""Define the Particle Filter. +""" + +import numpy as np +import bisect + +__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 Particle(object): + r"""Particle + """ + def __init__(self, state): + self.state = state + + +class ParticleFilter(object): + r"""Particle Filter + + Type: Non-parametric filter + + "Particle filtering uses a genetic mutation-selection sampling approach, with a set of particles + (also called samples) to represent the posterior distribution of some stochastic process given noisy + and/or partial observations." (Wikipedia) + In other words, "a particle is a hypothesis as to what the true world state may be at time t." [1] + + Complexity: O(exp(n)) + + References: + [1] "Probabilistic Robotics", Thrun et al., 2006 (sec 4.3) + """ + + def __init__(self, state_min, state_max, num_particles=100): + # initialize particles randomly between the 2 given bounds (i.e. state_min and state_max) + self.particles = np.random.uniform(state_min, state_max, size=(num_particles, state_min.size)) + self.weights = np.ones(num_particles) + + @property + def num_particles(self): + return len(self.particles) + + def predict(self, f, u): + # for each particle, predict next state of the particle given the control input + self.particles = [f(state, u) for state in self.particles] + return self.particles + + def measurement_update(self, h, z): + # for each particle, compute the probability to see measurement z from the particle state + self.weights = [h(z, state) for state in self.particles] + return self.weights + + def sampling(self): + # importance sampling (survival of the fittest) + particles = [] + + # resampling + + # Wheel algorithm (from Udacity) + # idx = np.random.randint(0, self.num_particles) + # b, wmax = 0., max(self.weights) + # for i in range(self.num_particles): + # b += np.random.random() * 2 * wmax + # while self.weights[idx] < b: + # b -= self.weights[idx] + # idx = (idx+1) % self.num_particles + # particles.append(self.particles[idx]) + + # resampling O(N*log(N)) algorithm: I observe that this one was better than the Wheel algorithm + # compute cumulative probability + sumProb = sum(self.weights) + cumulativeProb = [w/sumProb for w in self.weights] # normalize + for i in range(1, self.num_particles): + cumulativeProb[i] += cumulativeProb[i-1] + + # resample + for i in range(self.num_particles): + idx = bisect.bisect(cumulativeProb, np.random.uniform(0,1)) + particles.append(self.particles[idx]) + + # return particles (=states) + self.particles = particles + return self.particles + + def compute(self, f, u, h, z): + self.predict(f, u) + self.measurement_update(h, z) + self.sampling() + return self.particles diff --git a/pyrobolearn/filters/unscented_kalman_filter.py b/pyrobolearn/filters/unscented_kalman_filter.py new file mode 100644 index 0000000..fa788d6 --- /dev/null +++ b/pyrobolearn/filters/unscented_kalman_filter.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python +"""Define the Unscented Kalman Filter. +""" + +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 UKF(object): + r"""Unscented Kalman Filter (UKF) + + Type: Derivative-free Gaussian filter (=Bayes filter for continuous spaces) + + This is an extension and generalization to the standard Kalman filter which also works with nonlinear systems. + This filter is an alternative to deal with non-linear process/measurement models, using :math:`\sigma`-points + to approximate the probability distribution. Specifically, as EKF, it linearizes the transformation of a Gaussian, + but instead of doing it via a Taylor expansion, it "performs a stochastic linearization through the use of + a weighted statistical linear regression process". [1] + + The dynamic model :math:`p(x_t | x_{t-1}, u_t)` and the measurement model :math:`p(z_t | x_t)` are given by: + + .. math:: + + x_t = f(x_{t-1}, u_t) + \epsilon_t \qquad \mbox{where} \qquad \epsilon_t \sim \mathcal{N}(0, R_t) + z_t = h(x_t) + \delta_t \qquad \mbox{where} \qquad \delta_t \sim \mathcal{N}(0, Q_t) + + where :math:`f` and :math:`h` are nonlinear functions. + + The UKF works by generating :math:`2n+1` :math:`sigma`-points, and pass them through the corresponding dynamical + and measurement functions, and compute from them a weighted empirical mean and covariance. + + Notes: + - Compared to EKF, this filter does not require to compute the Jacobians for the dynamical and measurement + models, and is thus a derivative-free filter. "If the belief is highly non-Gaussian, then the UKF + representation is too restrictive and the filter can perform arbitrarily poorly" [1] + - An extension to multimodal posteriors is known as multi-hypothesis Kalman filter which uses a mixture of + Gaussians. See also, the histogram and particle filters that are well-suited to represent complex multimodal + beliefs. [1] + + Complexity: "The asympototic complexity of the UKF algorithm is the same as for the EKF. In practice, the EKF is + often slightly faster than the UKF. Nevertheless, the UKF is still highly efficient" [1] + + References: + [1] "Probabilistic Robotics", Thrun et al., 2006 (sec 3.4) + """ + + def __init__(self, mean, covariance, dynamic_noise_cov, measurement_noise_cov, alpha=1., kappa=0, beta=2): + """ + Initialize the Unscented Kalman Filter. + + Args: + mean (float[N]): initial mean of the state belief + covariance (float[N,N]): initial covariance matrix of the state belief + dynamic_noise_cov (float[N,N]): dynamic noise covariance matrix + measurement_noise_cov (float[K,K]): measurement noise covariance matrix + alpha (float): control the spread of the sigma points (recommended value: 1e-3 or 1) + kappa (float): control the spread of the sigma points + beta (int): this parameter can be chosen to encode additional (higher order) knowledge about the + distribution underlying the Gaussian representation. + """ + self.mu = mean + self.Sigma = covariance + self.n = self.mu.size + self.tau = alpha**2 * (self.n + kappa) - self.n + self.weight_mean_0 = self.tau / (self.n + self.tau) + self.weight_cov_0 = self.tau / (self.n + self.tau) + (1 - alpha**2 + beta) + self.weight = 1./(2.*(self.n + self.tau)) + self.gamma = np.sqrt(self.n + self.tau) + + def generate_sigma_points(self): + """ + Generate the sigma points for the UKF. + + Returns: + float[2N+1, N]: sigma points + """ + term = self.gamma * np.sqrt(self.Sigma.T) + sigma_points = np.vstack((self.mu, self.mu + term, self.mu - term)) + return sigma_points + + def predict(self, f, u): + """ + Predict (a priori) the next state (without taking into account measurements) using the nonlinear dynamic model + with added Gaussian noise. + + Args: + f (callable class/function): (nonlinear) dynamical function. + u (np.array): control array + + Returns: + float[N]: a priori mean of the Gaussian belief + float[N,N]: a priori covariance of the Gaussian belief + """ + # generate sigma points + sigma_points = self.generate_sigma_points() + + # feed each sigma point to the dynamical function + X = np.array([f(sigma, u) for sigma in sigma_points]) + + # compute the weighted empirical mean and covariance + self.mu = self.weight_mean_0 * X[0] + self.weight * X[1:].sum(axis=0) + diff0 = (X[0] - self.mu).reshape(-1,1) + diff = (X[1:] - self.mu).T + self.Sigma = self.weight_cov_0 * diff0.dot(diff0.T) + self.weight * diff.dot(diff.T) + self.R + + # return the a priori Gaussian belief on the next state + return self.mu, self.Sigma + + def measurement_update(self, h, z): + """ + Predict (a posteriori) the next state by incorporating the measurement :math:`z_t`. + + Args: + h (callable class/function): (nonlinear) measurement function. + z (np.array): measurement array + + Returns: + float[N]: a posteriori mean of the Gaussian belief + float[N,N]: a posteriori covariance matrix of the Gaussian belief + """ + # generate sigma points + sigma_points = self.generate_sigma_points() + + # feed each sigma point to the measurement function + Z = np.array([h(sigma) for sigma in sigma_points]) + + # weighted empirical mean for the measurement + z_mean = self.weight_mean_0 * Z[0] + self.weight * Z[1:].sum(axis=0) + + # deviation error between the measurement and the a priori predicted state (aka innovation) + y = z - z_mean + + # compute the weighted cross-covariance + term0 = ((sigma_points[0] - self.mu).reshape(-1,1)).dot( (Z[0] - z_mean).reshape(1, -1) ) + term = ((sigma_points[1:] - self.mu).T).dot(Z[1:] - z_mean) + C = self.weight_cov_0 * term0 + self.weight * term + + # Innovation covariance matrix + diff0 = (Z[0] - z_mean).reshape(-1, 1) + diff = (Z[1:] - z_mean).T + S = self.weight_cov_0 * diff0.dot(diff0.T) + self.weight * diff.dot(diff.T) + self.Q + + # Kalman gain (which specifies the degree to which the measurement is incorporated into the new state) + K = C.dot(np.linalg.inv(S)) + + # compute the weighted empirical mean and covariance + self.mu = self.mu + K.dot(y) + self.Sigma = self.Sigma - K.dot(S.dot(K.T)) + + # return the a posteriori Gaussian belief on the next state + return self.mu, self.Sigma + + def compute(self, f, u, h, z): + """ + Perform a prediction and measurement update step. + + Args: + f (callable class/function): (nonlinear) dynamical function. + u (np.array): control array + h (callable class/function): (nonlinear) measurement function. + z (np.array): measurement array + + Returns: + float[N]: a posteriori mean vector of the Gaussian belief + float[N,N]: a posteriori covariance matrix of the Gaussian belief + """ + self.predict(f, u) + self.measurement_update(h, z) + return self.mu, self.Sigma diff --git a/pyrobolearn/models/README.md b/pyrobolearn/models/README.md new file mode 100644 index 0000000..cd6b020 --- /dev/null +++ b/pyrobolearn/models/README.md @@ -0,0 +1,3 @@ +## Learning models + +In this folder, we provide the various learning models. These can be categorized into two categories: movement primitives and general function approximators. \ No newline at end of file diff --git a/pyrobolearn/models/__init__.py b/pyrobolearn/models/__init__.py new file mode 100644 index 0000000..abcbccf --- /dev/null +++ b/pyrobolearn/models/__init__.py @@ -0,0 +1,44 @@ + +# General model +from model import Model + +# General Learning models # + +# Linear +from linear import Linear + +# PCA +from pca import PCA + +# Polynomial +from polynomial import Polynomial, PolynomialFunction + +# Gaussian +from gaussian import Gaussian, MVN # MVN is an alias + +# GMM/GMR +from gmm import GMM + +# GP +from gp import GPR + +# HMM +from hmm import * + +# DNN +from nn import * + + +# Learning model for trajectories (movement primitives) # + +# CPG +from cpg import * + +# DMP +from dmp import * + +# ProMP +from promp import * + +# KMP +from kmp import * diff --git a/pyrobolearn/models/basic_models.py b/pyrobolearn/models/basic_models.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/models/cpg.py b/pyrobolearn/models/cpg.py new file mode 100644 index 0000000..566993e --- /dev/null +++ b/pyrobolearn/models/cpg.py @@ -0,0 +1,716 @@ +#!/usr/bin/env python +"""Define the Central Pattern Generator Node and Network classes + +Central Pattern Generators (CPGs) allows to model rhythmic movement primitives, and are often used in locomotion. +This file provides the CPG Node and Network, where the network is composed of CPG nodes connected / phased together. +""" + +import numpy as np +import torch +import matplotlib.pyplot as plt +from matplotlib.widgets import Slider, CheckButtons +from matplotlib.animation import FuncAnimation + + +__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 CPGNode(object): + r"""Central Pattern Generator Node + + Each CPG node :math:`i` is driven by the following differential equations: + * phase: :math:`\dot{\phi}_i &= \omega_i + \sum_j a_j w_{ij} \sin(\phi_j - \phi_i - \varphi_{ij})` + * amplitude: :math:`\ddot{a}_i &= K_a (A_i - a_i) - D_a \dot{a}_i` + * offset: :math:`\ddot{o}_i &= K_o (O_i - o_i) - D_o \dot{o}_i` + * target angle: :math:`\theta_i &= o_i + a_i \cos(\phi_i)` + + References: + [1] "Central pattern generators for locomotion control in animals and robots: a review", Ijspeert, 2008 + """ + + def __init__(self, id, phi=0, offset=0, amplitude=1., timesteps=100, freq=None): + self.id = id + self.timesteps = timesteps + self.dt = 1./self.timesteps + self.t = 0. + + # gains + self.D_amp, self.D_offset = 20., 20. + self.K_amp, self.K_offset = self.D_amp**2/4., self.D_offset**2/4. + + # state parameters + self.phi, self.curr_phi = phi, phi + self.amp, self.damp, self.curr_amp = amplitude, 0, amplitude + self.offset, self.doffset, self.curr_offset = offset, 0, offset + + # control parameters + self.des_offset = offset + self.des_amp = amplitude + self.des_freq = 1. if freq is None else freq # 1./self.timesteps if freq is None else freq + self.des_omega = 2 * np.pi * self.des_freq + + # init values + self.init_phi = self.phi + # self.init_offset = self.offset + # self.init_amp = self.amp + + # angle + self.theta = self.offset + self.amp * np.cos(self.phi) + + # dict containing the CPG nodes connected to this node with their coupling weight and bias + self.nodes = {} + self.sliderNodes = {} + + # plot + self.frame = None + self.fig = None + self.do_plot_offset, self.do_plot_amp, self.do_plot_phi, self.do_plot_theta = False, False, False, True + self.updated = True + + # keep in memory the previous `timesteps` values of offset, amplitude, phi, and theta + self.offsets = [self.offset] * self.timesteps + self.amps = [self.amp] * self.timesteps + self.phis = [self.phi] * self.timesteps + self.thetas = [self.theta] * self.timesteps + + ############## + # Properties # + ############## + + @property + def des_offset(self): + """Return the desired offset.""" + return self._des_offset + + @des_offset.setter + def des_offset(self, offset): + """Set the desired offset.""" + if offset is None: + offset = 0. + if not isinstance(offset, (int, float)): + raise TypeError("Expecting the desired offset to be an integer or float, got instead: " + "{}".format(type(offset))) + self._des_offset = offset + + @property + def des_amp(self): + """Return the desired amplitude.""" + return self._des_amp + + @des_amp.setter + def des_amp(self, amplitude): + """Set the desired amplitude.""" + if amplitude is None: + amplitude = 0. + if not isinstance(amplitude, (int, float)): + raise TypeError("Expecting the desired offset to be an integer or float, got instead: " + "{}".format(type(amplitude))) + self._des_amp = amplitude + + @property + def des_freq(self): + """Return the desired frequency.""" + return self._des_freq + + @des_freq.setter + def des_freq(self, freq): + """Set the desired frequency.""" + if freq is None: + freq = 1. + if not isinstance(freq, (int, float)): + raise TypeError("Expecting the desired frequency to be an integer or float, got instead: " + "{}".format(type(freq))) + self._des_freq = freq + self.des_omega = 2 * np.pi * self._des_freq + + @property + def num_coupling_nodes(self): + """Return the number of coupling nodes.""" + return len(self.nodes) + + # @property + # def phi(self): + # """Return the phase.""" + # return self._phi + # + # @phi.setter + # def phi(self, phi): + # """Set the phase.""" + # self.curr_phi, self.phi = phi, phi + # self.theta = self.offset + self.amp * np.cos(self.phi) + + def set_phi(self, phi): + self.curr_phi, self.phi = phi, phi + self.theta = self.offset + self.amp * np.cos(self.phi) + + @property + def num_parameters(self): + return 3 + 2 * len(self.nodes) + + ########### + # Methods # + ########### + + def add_node(self, cpg_node, weight=0., bias=0.): + """Add a coupling node.""" + self.nodes[cpg_node] = {'weight': weight, 'bias': bias} + + def remove_node(self, cpg_node): + """Remove a coupling node""" + if cpg_node in self.nodes: + self.nodes.pop(cpg_node) + + def reset(self): + """Reset the phase of the CPG node; this can be useful for phase resetting.""" + # Re-initialize the CPG + self.curr_phi, self.phi = self.init_phi, self.init_phi + self.theta = self.offset + self.amp * np.cos(self.phi) + + def parameters(self): + """Returns an iterator over the model parameters.""" + # proper node parameters + yield self.des_amp + yield self.des_offset + yield self.des_freq + + # coupling parameters + for node in self.nodes: + yield self.nodes[node]['weight'] + yield self.nodes[node]['bias'] + + def named_parameters(self): + """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" + # proper node parameters + yield str(self) + ':amplitude', self.des_amp + yield str(self) + ':offset', self.des_offset + yield str(self) + ':frequency', self.des_freq + + # coupling parameters + for node in self.nodes: + yield str(self) + ':weight:' + str(node), self.nodes[node]['weight'] + yield str(self) + ':bias:' + str(node), self.nodes[node]['bias'] + + 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 of the parameters (weights, biases, offsets).""" + if to_numpy: + return np.array(self.list_parameters()) + else: + return torch.from_numpy(np.array(self.list_parameters())) + + def set_vectorized_parameters(self, vector): + """Set the vector parameters.""" + # set the parameters from the vectorized one + if len(vector) != self.num_parameters: + raise ValueError("Expecting the size of the vectorized parameters to match the number of parameters " + "of this node. Instead of having {}, I got {}.".format(self.num_parameters, len(vector))) + self.des_amp = vector[0] + self.des_offset = vector[1] + self.des_freq = vector[2] + + # coupling parameters + for idx, node in enumerate(self.nodes): + self.nodes[node]['weight'] = vector[2*idx + 3] + self.nodes[node]['bias'] = vector[2*idx + 4] + + def step(self): + """ + Perform a step by integrating (i.e. Euler integration) the differential equations governing the CPG. + + The CPG equations for node :math:`i` are: + + .. math:: + + \dot{\phi}_i &= \omega_i + \sum_j a_j w_{ij} \sin(\phi_j - \phi_i - \varphi_{ij}) \\ + \ddot{a}_i &= K_a (A_i - a_i) - D_a \dot{a}_i \\ + \ddot{o}_i &= K_o (O_i - o_i) - D_o \dot{o}_i \\ + \theta_i &= o_i + a_i \cos(\phi_i) \\ + + where + :math:`\phi` is the phase, + :math:`\omega` is the desired angular velocity (desired frequency), + :math:`A` and :math:`a` are the desired and current amplitude, + :math:`O` and :math:`o` are the desired and current offset, + :math:`K` and :math:`D` are the stiffness and damping gains (which are normally related such that the system + is critically damped), + :math:`w_{ij}` and :math:`\varphi_{ij}` are the coupling weights and phase biases, and finally, + :math:`\theta` is the resulting (joint) angle (to be sent to the controller). + + .. note:: for each node, update() has to be called only after step() has been called for all the nodes! + """ + + # offset + ddoffset = self.K_offset * (self.des_offset - self.offset) - self.D_offset * self.doffset + self.doffset += ddoffset * self.dt + self.curr_offset += self.doffset * self.dt + + # amplitude + ddamp = self.K_amp * (self.des_amp - self.amp) - self.D_amp * self.damp + self.damp += ddamp * self.dt + self.curr_amp += self.damp * self.dt + + # phase + dphi = self.des_omega + for node, coupling in self.nodes.items(): + dphi += node.amp * coupling['weight'] * np.sin(node.phi - self.phi - coupling['bias']) + self.curr_phi = (self.curr_phi + dphi * self.dt) % (2*np.pi) + + # self.t += self.dt + + def update(self): + curr_t = self.t + self.dt + curr_theta = self.offset + self.amp * np.cos(self.phi) + + # update plot + if self.fig is not None: + if self.do_plot_theta: + self.ax_plot.plot([self.t, curr_t], [self.theta, curr_theta], 'b') # , animated=True) + if self.do_plot_amp: + self.ax_plot.plot([self.t, curr_t], [self.amp, self.curr_amp], 'g') # , animated=True) + if self.do_plot_offset: + self.ax_plot.plot([self.t, curr_t], [self.offset, self.curr_offset], 'r') # , animated=True) + if self.do_plot_phi: + self.ax_plot.plot([self.t, curr_t], [self.phi, self.curr_phi], 'm') # , animated=True) + + # move axis to get a feeling of online plotting. Warning: this slows down rendering + # self.ax_plot.set_xlim(curr_t - 0.5, curr_t + 0.5) + + # update state params + self.phi = self.curr_phi + self.amp = self.curr_amp + self.offset = self.curr_offset + self.theta = curr_theta + self.t = curr_t + + # update data + # if len(self.phis) >= self.timesteps: self.phis = self.phis[1:] + # if len(self.amps) >= self.timesteps: self.amps = self.amps[1:] + # if len(self.offsets) >= self.timesteps: self.offsets = self.offsets[1:] + # if len(self.thetas) >= self.timesteps: self.thetas = self.thetas[1:] + # self.phis.append(self.phi) + # self.amps.append(self.amp) + # self.offsets.append(self.offset) + # self.thetas.append(self.theta) + # self.updated = True + + # update sliders + if self.fig is not None: + self.slider_offset.set_val(self.offset) + self.slider_amp.set_val(self.amp) + self.slider_phi.set_val(self.phi) + + # update canvas + # self.fig.canvas.draw() + # plt.show(block=False) + + def plot(self): + if self.fig is None: + self.fig = plt.figure('CPG '+str(self.id)) + + # Plot signals # + h = 0.65 + self.ax_plot = self.fig.add_axes([0.1, h, 0.8, 0.3]) + self.ax_plot.set_ylim(-np.pi, np.pi) + self.ax_plot.set_xlim(0, 1) + ax = self.fig.add_axes([0.91, h+0.06, 0.08, 0.18]) + # ax.axis('off') + self.check_offset = CheckButtons(ax, ['o', 'a', 'phi', 'theta'], + [self.do_plot_offset, self.do_plot_amp, self.do_plot_phi, self.do_plot_theta]) + + def check_buttons(val): + if val == 'o': + self.do_plot_offset = not self.do_plot_offset + # self.offset_line.set_visible(self.do_plot_offset) + elif val == 'a': + self.do_plot_amp = not self.do_plot_amp + # self.amp_line.set_visible(self.do_plot_amp) + elif val == 'phi': + self.do_plot_phi = not self.do_plot_phi + # self.phi_line.set_visible(self.do_plot_phi) + elif val == 'theta': + self.do_plot_theta = not self.do_plot_theta + # self.theta_line.set_visible(self.do_plot_theta) + self.check_offset.on_clicked(check_buttons) + + # x = np.linspace(0., 1., self.timesteps) + # self.theta_line = self.ax_plot.plot(x, self.thetas)[0] + # self.amp_line = self.ax_plot.plot(x, self.amps)[0] + # self.offset_line = self.ax_plot.plot(x, self.offsets)[0] + # self.phi_line = self.ax_plot.plot(x, self.phis)[0] + # + # self.theta_line.set_visible(self.do_plot_theta) + # self.amp_line.set_visible(self.do_plot_amp) + # self.offset_line.set_visible(self.do_plot_offset) + # self.phi_line.set_visible(self.do_plot_phi) + # + # def update_plot(_): + # lines = [] + # if self.updated: + # if self.do_plot_theta: + # self.theta_line.set_ydata(self.thetas) + # lines.append(self.theta_line) + # if self.do_plot_amp: + # self.amp_line.set_ydata(self.amps) + # lines.append(self.amp_line) + # if self.do_plot_offset: + # self.offset_line.set_ydata(self.offsets) + # lines.append(self.offset_line) + # if self.do_plot_phi: + # self.phi_line.set_ydata(self.phis) + # lines.append(self.phi_line) + # self.updated = False + # return lines + # + # self.anim = FuncAnimation(self.fig, update_plot, interval=100, blit=True) + + # State parameters # + # offset + h -= 0.1 + # ax = self.fig.add_axes([0.2, h, 0.6, 0.03]) + ax = self.fig.add_axes([0.1, h, 0.35, 0.03]) + self.slider_offset = Slider(ax, 'o', -np.pi, np.pi, valinit=self.offset, dragging=False) + + def set_offset(val): + self.offset = self.slider_offset.val + + self.slider_offset.on_changed(set_offset) + + # amplitude + # h -= 0.05 + # ax = self.fig.add_axes([0.2, h, 0.6, 0.03]) + ax = self.fig.add_axes([0.55, h, 0.35, 0.03]) + self.slider_amp = Slider(ax, 'a', 0, np.pi, valinit=self.amp, dragging=False) + + def set_amp(val): + self.amp = self.slider_amp.val + + self.slider_amp.on_changed(set_amp) + + # phi + h -= 0.05 + # ax = self.fig.add_axes([0.2, h, 0.6, 0.03]) + ax = self.fig.add_axes([0.1, h, 0.35, 0.03]) + self.slider_phi = Slider(ax, 'phi', 0, 2*np.pi, valinit=self.phi, dragging=False) + + def set_phi(val): + self.phi = self.slider_phi.val + + self.slider_phi.on_changed(set_phi) + + # Control parameters # + # desired offset + h -= 0.05 + # ax = self.fig.add_axes([0.2, h, 0.6, 0.03]) + ax = self.fig.add_axes([0.1, h, 0.35, 0.03]) + self.slider_des_offset = Slider(ax, 'O', -np.pi, np.pi, valinit=self.des_offset) + + def set_des_offset(val): + self.des_offset = self.slider_des_offset.val + + self.slider_des_offset.on_changed(set_des_offset) + + # desired amplitude + # h -= 0.05 + # ax = self.fig.add_axes([0.2, h, 0.6, 0.03]) + ax = self.fig.add_axes([0.55, h, 0.35, 0.03]) + self.slider_des_amp = Slider(ax, 'A', 0, np.pi, valinit=self.des_amp) + + def set_des_amp(val): + self.des_amp = self.slider_des_amp.val + + self.slider_des_amp.on_changed(set_des_amp) + + # desired frequency + h -= 0.05 + # ax = self.fig.add_axes([0.2, h, 0.6, 0.03]) + ax = self.fig.add_axes([0.1, h, 0.35, 0.03]) + self.slider_des_freq = Slider(ax, 'f', 0, 50, valinit=self.des_freq, valfmt='%0.0f') + + def set_des_freq(val): + self.des_freq = self.slider_des_freq.val + self.des_omega = 2 * np.pi * self.des_freq + + self.slider_des_freq.on_changed(set_des_freq) + + # Coupling parameters # + class Coupling(object): + """ + Class to be used with sliders for the coupling parameters (weights and biases) + """ + def __init__(self, node, nodes, sliderNodes): + self.node = node + self.nodes = nodes + self.sliderNodes = sliderNodes + + def set_weight(self, val): + self.nodes[self.node]['weight'] = self.sliderNodes[self.node]['weight'].val + + def set_bias(self, val): + self.nodes[self.node]['bias'] = self.sliderNodes[self.node]['bias'].val + + for node, coupling in self.nodes.items(): + h -= 0.05 + # slider for coupling weight + ax = self.fig.add_axes([0.1, h, 0.35, 0.03]) + slider = Slider(ax, 'w'+str(self.id)+str(node.id), -5, 5, valinit=coupling['weight'], valfmt='%0.1f') + c = Coupling(node, self.nodes, self.sliderNodes) + slider.on_changed(c.set_weight) + self.sliderNodes[node] = {'weight': slider} + # slider for phase bias + ax = self.fig.add_axes([0.55, h, 0.35, 0.03]) + slider = Slider(ax, 'b' + str(self.id) + str(node.id), -np.pi, np.pi, valinit=coupling['bias'], valfmt='%0.2f') + slider.on_changed(c.set_bias) + self.sliderNodes[node]['bias'] = slider + + plt.show(block=False) + # plt.draw() + # self.fig.show() + # self.fig.canvas.draw() # Problem: not reactive to keyboard/mouse when moving sliders... + + ############# + # Operators # + ############# + + def __str__(self): + """Return a string describing the class.""" + return self.__class__.__name__ + "(" + str(self.id) + ")" + + +class CPGNetwork(object): + r"""Central Pattern Generator Network + + """ + + def __init__(self, nodes, timesteps=100): + nodes = nodes if isinstance(nodes, dict) else self.fully_connected_network(nodes) + + # create each node + self.nodes, init_params = {}, {'phi', 'offset', 'amplitude', 'freq'} + for node_id in nodes.keys(): + d = {key: val for key, val in nodes[node_id].items() if key in init_params} + self.nodes[node_id] = CPGNode(node_id, timesteps=timesteps, **d) + + # couple the nodes + for node_id, node in self.nodes.items(): + if 'nodes' in nodes[node_id]: # check if 'nodes' in dict + for coupling_node in nodes[node_id]['nodes']: # coupling_node = {'id':..., 'weight':..., 'bias': ...} + w = coupling_node['weight'] if 'weight' in coupling_node else 0. + b = coupling_node['bias'] if 'bias' in coupling_node else 0. + node.add_node(self.nodes[coupling_node['id']], weight=w, bias=b) + + self.fig = None + self.nodes_id = self.nodes.keys() + self.nodes_id.sort() + + @property + def num_parameters(self): + """Return the total number of parameters in this CPG network.""" + return sum([node.num_parameters for node in self.nodes.values()]) + + def parameters(self): + """Returns an iterator over the model parameters.""" + for node in self.nodes.values(): + yield np.array(list(node.parameters())) + + def named_parameters(self): + """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" + for node in self.nodes.values(): + yield str(node), np.array(list(node.parameters())) + + 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 of the parameters (amplitudes, offsets, frequencies, weights, biases).""" + return np.concatenate([node.get_vectorized_parameters(to_numpy=to_numpy) for node in self.nodes.values()]) + + def set_vectorized_parameters(self, vector): + """Set the vector parameters.""" + # set the parameters from the vectorized one + if len(vector) != self.num_parameters: + raise ValueError("Expecting the size of the vectorized parameters to match the number of parameters " + "of this node. Instead of having {}, I got {}.".format(self.num_parameters, len(vector))) + + # convert from torch tensor to numpy array if necessary + if isinstance(vector, torch.Tensor): + if vector.requires_grad: + vector = vector.detach().numpy() + else: + vector = vector.numpy() + + # set the parameters from the vectorized one + idx = 0 + for node in self.nodes.values(): + size = node.num_parameters + node.set_vectorized_parameters(vector[idx:idx+size]) + idx += size + + def add_node(self, node): + """Add a node to the CPG network.""" + # TODO: update self.nodes_id + self.nodes[node.id] = node + + def reset(self): + """Reset the phase of each CPG node in the network; this can be useful for phase resetting.""" + for node in self.nodes.values(): + node.reset() + + def step(self): + """Perform a step with the CPG network.""" + # perform one step + for node in self.nodes.values(): + node.step() + # perform update + for node in self.nodes.values(): + node.update() + return np.array([self.nodes[idx].theta for idx in self.nodes_id]) + + def plot(self): + """Plot each node.""" + for node in self.nodes.values(): + node.plot() + + @staticmethod + def fully_connected_network(num_nodes, init_phi=0, offset=0, amplitude=1., weight=0, bias=0, freq=1.): + """ + Create an initial dictionary describing a fully connected network of CPG nodes. + + Args: + num_nodes (int): the total number of nodes in the network + init_phi (float): initial phase of CPG node + offset (float): desired and initial offset of CPG node + amplitude (float): desired and initial amplitude of CPG node + weight (float): weight between 2 nodes + bias (float): phase bias between 2 nodes + + Returns: + dict: dictionary describing how the nodes are connected + """ + node_ids = range(1, num_nodes+1) + d = {i: {'phi': init_phi, + 'offset': offset, + 'amplitude': amplitude, + 'freq': freq, + 'nodes': [{'id': n, 'weight': weight, 'bias': bias} for n in (node_ids[:i-1]+node_ids[i:])]} + for i in node_ids} + return d + + +# Tests +if __name__ == "__main__": + # Define and parse command line arguments + import argparse + parser = argparse.ArgumentParser() + parser.add_argument('-t', '--test', help='What to test', type=str, + choices=['1_node', '2_nodes', 'coman_gazebo', 'minitaur_pybullet'], + default='minitaur_pybullet') + parser.add_argument('-T', '--nb_steps', help='Total time number of steps to run the simulation', + type=int, default=100) + parser.add_argument('-dt', '--dt', help='Time to wait before next step', type=float, default=0.01) + args = parser.parse_args() + + dt = args.dt + T = args.nb_steps + + # Check a single node # + if args.test == '1_node': + # create CPG node + node = CPGNode(1) + node.plot() + + # Run for T steps + for _ in range(T): + node.step() + node.update() + # time.sleep(0.1) # Don't use time, instead use plt.pause! + plt.pause(dt) + plt.show() + + # Check 2 nodes # + elif args.test == '2_nodes': + # create CPG network + nodes = {1: {'phi': -np.pi / 2, 'nodes': [{'id': 2, 'weight': 0, 'bias': 0}]}, + 2: {'phi': np.pi / 2, 'nodes': [{'id': 1, 'weight': 0, 'bias': 0}]}} + network = CPGNetwork(nodes) + network.plot() + + # Run for T steps + for _ in range(T): + network.step() + plt.pause(dt) + plt.show() + + # Check coman robot in gazebo # + # Warning: Don't forget to launch gazebo with coman before running this code! + # $ roslaunch coman_gazebo coman_world.launch + elif args.test == 'coman_gazebo': + from pyrobolearn.robots.ros.coman.comanpublisher import ComanPublisher + import rospy + + # create CPG network of 2 nodes + pub = ComanPublisher(joints=['RHipSag', 'LHipSag']) + nodes = {1: {'phi': -np.pi/2, 'nodes': [{'id': 2, 'weight': 1, 'bias': 0}]}, + 2: {'phi': np.pi/2, 'nodes': [{'id': 1, 'weight': 0, 'bias': 0}]}} + network = CPGNetwork(nodes) + # network.plot() + + # Run for T steps + # rate = rospy.Rate(30) + # while not rospy.is_shutdown(): + for _ in range(T): + network.step() + pub.send({'RHipSag': network.nodes[1].theta, + 'LHipSag': network.nodes[2].theta}) + plt.pause(dt) + # rate.sleep() + + # Check CPG with Minitaur # + elif args.test == 'minitaur_pybullet': + from pybullet_envs.bullet.bullet_client import BulletClient + from pybullet_envs.bullet.minitaur import Minitaur + import pybullet + import pybullet_data + import time + + # create and configure pybullet simulator + client = BulletClient(connection_mode=pybullet.GUI) + client.setAdditionalSearchPath(pybullet_data.getDataPath()) + floor = client.loadURDF('plane.urdf') + client.setGravity(0, 0, -9.81) + minitaur = Minitaur(client, urdf_root=pybullet_data.getDataPath()) + + # create CPG network + phis = [0, 0, 0, 0] + nodes = {1: {'phi': phis[0], 'offset': -np.pi/2, 'amplitude': np.pi/4, 'nodes': []}, # front left leg + 2: {'phi': phis[1], 'offset': -np.pi/2, 'amplitude': np.pi/4, 'nodes': []}, # back left leg + 3: {'phi': phis[2], 'offset': np.pi/2, 'amplitude': np.pi/4, 'nodes': []}, # front right leg + 4: {'phi': phis[3], 'offset': np.pi/2, 'amplitude': np.pi/4, 'nodes': []}, # back right leg + } + network = CPGNetwork(nodes) + + # Run simulation + for _ in range(10*T): + # Get angles from CPG network and set them to the minitaur + act = network.step() + for i in range(len(act)//2): # Left + minitaur._SetDesiredMotorAngleById(minitaur._motor_id_list[2*i], act[i]) + minitaur._SetDesiredMotorAngleById(minitaur._motor_id_list[2*i+1], -np.pi-act[i]) + for i in range(len(act)//2, len(act)): # Right + minitaur._SetDesiredMotorAngleById(minitaur._motor_id_list[2*i], act[i]) + minitaur._SetDesiredMotorAngleById(minitaur._motor_id_list[2*i+1], np.pi - act[i]) + + # run one-step forward the simulation + client.stepSimulation() + time.sleep(dt) + # print(client.getEulerFromQuaternion(minitaur.GetBaseOrientation())) diff --git a/pyrobolearn/models/dmp.py b/pyrobolearn/models/dmp.py new file mode 100644 index 0000000..265ae61 --- /dev/null +++ b/pyrobolearn/models/dmp.py @@ -0,0 +1,1711 @@ +#!/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 + + 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 + + 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 + + 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.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.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))) + + 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) + + 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/gaussian.py b/pyrobolearn/models/gaussian.py new file mode 100755 index 0000000..111f10a --- /dev/null +++ b/pyrobolearn/models/gaussian.py @@ -0,0 +1,1479 @@ +#!/usr/bin/env python +"""Define the Multivariate Gaussian / Normal distribution class. + +This distribution is so important in the field of Machine Learning that it is implemented from scratch with all +the possible operations (that I could think of) that can be performed on it. It will notably be used for Gaussian +Mixture Models, Probabilistic Movement Primitives, Kernelized Movement Primitives, etc. +""" + + +import numpy as np +import scipy +from scipy.stats import multivariate_normal as mvn + +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from matplotlib.patches import Ellipse + +# import autograd.numpy as np +# import torch +# import geomstats + + +__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 Gaussian(object): + r"""Multivariate Gaussian distribution + + The multivariate Gaussian distribution also known as the multivariate normal distribution is the most well-known + and probably the most used distribution because of its nice mathematical properties, its appearance in different + arguments/theorems (such as the maximum entropy argument, and central limit theorem), as well as its different + extensions (Gaussian mixture models, Gaussian processes, and so on). + + The multivariate Gaussian distribution is given by: + + .. math:: p(x) = \frac{1}{(2\pi)^\frac{d}{2} |\Sigma|^\frac{1}{2}} + \exp\left( - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right) + + where :math:`x` is the real random vector, :math:`\mu` is the mean, and :math:`\Sigma` is the covariance matrix. + + The Gaussian distribution has multiple interesting mathematical properties, notably [1]: + * The average of random variables tend to a Gaussian distribution by the central limit theorem + * Given the two first moments (i.e. mean and covariance), it is the maximum entropy distribution. + * The sum of two independent Gaussian random variables (with the same dimension) is also Gaussian. This + is the same as saying that the convolution of two Gaussian PDFs is a Gaussian PDF. + * The product of two Gaussians is also Gaussian but it is no more a valid probability distribution. + * The affine transformation of a Gaussian variable is again Gaussian. + * The conditional distribution is also Gaussian. + * The marginal distribution of a multivariate Gaussian with respect to a subset of the variables is itself Gaussian. + * A kernel matrix (which compares the similarity between different samples) can be provided instead of a + covariance matrix (which compares how the different dimensionalities vary between each other). + + Note that given the first and second moment (mean and covariance respectively), the Gaussian distribution is + the maximum entropy distribution. That is, it is the distribution that maximizes the entropy (which makes thus the + least number of assumptions about the data). As a side note, if only the first moment is provided, the one that + maximized the entropy is the Gibbs distribution, and if no moments are provided, then it is the uniform + distribution. This argument is known as the maximum entropy argument. Because the first and second moments can be + quite reliably estimated from the data, the Gaussian distribution is often used in the ML field. + + Note that the conjugate prior for the mean of the Gaussian distribution is also Gaussian. As for the covariance + matrix, its conjugate prior is the inverse Wishart distribution. + + Note that because the covariance is a symmetric, positive semi-definite matrix, it has a Cholesky decomposition. + Thus, it can be expressed as the product of a lower triangular matrix with its transpose :math:`\Sigma = LL^\top`. + This is useful for two reasons: + - any lower triangular matrices multiplied with its transpose results in a symmetric positive semi-definite + matrix, which thus represents a proper covariance matrix. This can be for instance useful when predicting a + full covariance matrix with a neural network. Indeed, it is hard to enforce that type of constraint (i.e. making + sure that the produced covariance matrix is symmetric and positive semi-definite) while optimizing the network. + We can thus instead output a lower-triangular matrix and multiplied by its transpose. + - it allows to solve efficiently a system of linear equations :math:`Ax = b` without having to compute the inverse + (and thus, the determinant). This is achieved in a 2-step way, by first computing :math:`Ly=b` for :math:`y` by + forward substitution, and then computing :math:`L^\top x = y` by backward substitution. + + References: + [1] "Pattern Recognition and Machine Learning", Bishop, 2006 + [2] "Machine Learning: A Probabilistic Perspective", Murphy, 2012, chap 3 and 4 + [3] "The Matrix Cookbook", Petersen et al., 2012, sec 8 + + The implementation of this class was inspired by the following codes: + * `scipy`: https://docs.scipy.org/doc/scipy-0.14.0/reference/generated/scipy.stats.multivariate_normal.html + * `autograd`: https://github.com/HIPS/autograd/blob/master/autograd/scipy/stats/multivariate_normal.py + * `torch`: https://pytorch.org/docs/stable/_modules/torch/distributions/multivariate_normal.html + * `riepybdlib`: https://gitlab.martijnzeestraten.nl/martijn/riepybdlib/blob/master/riepybdlib/statistics.py + * `pbdlib`: https://gitlab.idiap.ch/rli/pbdlib-python/blob/master/pbdlib/mvn.py + """ + + # Set array priority (matrices have __array_priority__ equal to 10.0) + # This is useful when multiplying a matrix with a Gaussian object. + # For more info, see: + # - https://docs.scipy.org/doc/numpy/reference/ufuncs.html + # - https://stackoverflow.com/questions/38229953/array-and-rmul-operator-in-python-numpy + __array_priority__ = 11 + + def __init__(self, mean=None, covariance=None, seed=None, manifold=None, N=None): # coefficient=1. + """ + Initialize the multivariate normal distribution on the given manifold. + + Args: + mean (np.array): mean vector. + covariance (np.array): covariance matrix. + seed (int): random seed. Useful when sampling. + manifold (None): By default, it is the Euclidean space. + N (int, N): the number of data points + """ + # Args: coefficient (float): coefficient in front of the Gaussian PDF. This is useful when multiplying two + # Gaussian PDFs which results in a Gaussian PDF multiplied by a coefficient (and thus, is no more + # a valid probability distribution). + + # set mean + self.mean = mean + + # check that the covariance is symmetric and is PSD + self.cov = covariance + + # set the coefficient + # self.coefficient = coefficient + + # set the seed + self.seed = seed + + # TODO: formulas are different depending on the space we are in + # TODO: the manifold should be an object (see the `geomstats` module) + # For now, it will just be a string and we will focus on the Euclidean space + self.manifold = 'euclidean' #manifold + + # number of data points + self.N = N + + ############## + # Properties # + ############## + + @property + def mean(self): + """Return the mean vector""" + return self._mean + + @mean.setter + def mean(self, mean): + """Set the mean vector""" + if isinstance(mean, (int, float)): + mean = np.array([mean]) + if mean is not None: + mean = np.array(mean) + self._mean = mean + + # alias + mu = mean + + @property + def cov(self): + """Return the covariance (PSD) matrix""" + return self._cov + + @cov.setter + def cov(self, cov): + """Set the covariance matrix""" + if cov is not None: + if isinstance(cov, (int, float)): + cov = np.array([[cov]]) + cov = np.array(cov) + if not self.is_symmetric(cov): + raise ValueError("The given covariance matrix is not symmetric") + if not self.is_psd(cov): + raise ValueError("The given covariance matrix is not positive semi-definite") + + self._cov = cov + + # aliases + # variance = cov + covariance = cov + sigma = cov + + @property + def mode(self): + """value that is the most likely to be sampled""" + return self.mean + + @property + def precision(self): + """Return the precision matrix""" + if self.cov is not None: + return np.linalg.inv(self.cov) + + # alias + prec = precision + + @property + def size(self): + """dimensionality of the gaussian distribution""" + if self.mean is not None: + return len(self.mean) + return 0 + + # alias + dim = size + + @property + def normalization_constant(self): + """normalization constant""" + if self.cov is not None: + return self.compute_normalization_constant(self.cov) + + @property + def seed(self): + """Return the seed""" + return self._seed + + @seed.setter + def seed(self, seed): + """Set the seed""" + self._seed = seed + np.random.seed(self._seed) + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other): + """Copy another Gaussian""" + if not isinstance(other, Gaussian): + raise TypeError("Expecting to copy a Gaussian") + return Gaussian(mean=other.mean, covariance=other.cov) + + @staticmethod + def is_parametric(): + """The Gaussian distribution is a nonparametric model; the mean and covariance summarized the data""" + return True + + @staticmethod + def is_linear(): + """The Gaussian doesn't have parameters. Even if the mean and covariance are considered as parameters, + the model is not linear wrt them""" + return True + + @staticmethod + def is_recurrent(): + """The Gaussian is not recurrent model; current outputs do not depend on previous inputs""" + return False + + @staticmethod + def is_probabilistic(): + """The Gaussian distribution is by definition a probabilistic model""" + return False + + @staticmethod + def is_discriminative(): + """The Gaussian is not a discriminative model; no inputs are involved""" + return True + + @staticmethod + def is_generative(): + """The Gaussian is a generative model, and thus we can sample from it""" + return False + + @staticmethod + def compute_mean(X, axis=0): + r""" + Compute the empirical mean vector given the data. This is also known as the maximum likelihood estimate for + the mean vector :math:`\mu`, i.e. :math:`\max_{\mu} p(X | \mu, \Sigma)`. + + Args: + X (array[N,D]): data matrix of shape NxD (if axis=0) or DxN (if axis=1), where N is the number of samples, + and D is the dimensionality of a data point + axis (int): axis along which the mean is computed + + Returns: + float[D]: mean vector + """ + # if manifold is Euclidean + mean = np.mean(X, axis=axis) + return mean + + @staticmethod + def compute_covariance(X, axis=0, bessels_correction=True): + r""" + Compute the empirical covariance matrix given the data. This is also known as the maximum likelihood estimate + for the covariance matrix :math:`\Sigma`, i.e. :math:`\max_{\Sigma} p(X | \mu, \Sigma)`. + + Args: + X (array[N,D]): data matrix of shape NxD where N is the number of samples, and D is the dimensionality + of a data point + axis (int): axis along which the covariance is computed + bessels_correction (bool): if True, it will compute the covariance using `1/N-1` instead of `N`. + + Returns: + float[D,D]: 2D covariance matrix + """ + # if manifold is Euclidean + cov = np.cov(X, rowvar=bool(axis), bias=not bessels_correction) + return cov + + @staticmethod + def compute_precision(X, axis=0, bessels_correction=True): + r""" + Compute the empirical precision matrix given the data. + + Args: + X (array[N,D]): data matrix of shape NxD where N is the number of samples, and D is the dimensionality + of a data point + axis (int): axis along which the precision is computed + bessels_correction (bool): if True, it will compute the precision using `1/N-1` instead of `N`. + + Returns: + float[D,D]: 2D precision matrix + """ + prec = np.linalg.inv( Gaussian.compute_covariance(X, axis=axis, bessels_correction=bessels_correction) ) + return prec + + @staticmethod + def compute_normalization_constant(covariance): + r""" + Compute the normalization constant based on the covariance, which is given by: + + .. math:: c = \frac{1}{(2\pi)^{\frac{d}{2}} |\Sigma|^{\frac{1}{2}}} + + Args: + covariance (array_like: float[d,d]): covariance matrix + + Returns: + float: normalization constant such that the distribution sums to 1 when integrated. + """ + size = covariance.shape[0] + normalization_constant = 1. / ((2 * np.pi) ** (size / 2.) * np.linalg.det(covariance) ** 0.5) + return normalization_constant + + @staticmethod + def is_symmetric(X, tol=1e-8): + """Check if given matrix X is symmetric. + If a matrix is symmetric, it has real eigenvalues, orthogonal eigenvectors and is always diagonalizable. + """ + return np.allclose(X, X.T, atol=tol) + + # TODO: check if X belongs to the SPD space S^n_{++} + @staticmethod + def is_psd(X, tol=1e-12): + """Check if given matrix is PSD""" + return np.all(np.linalg.eigvals(X) >= 0 - tol) + + ########### + # Methods # + ########### + + def _check_initialized(self): + """Check if the Gaussian distribution has been initialized""" + if self.mean is None: + raise ValueError("Mean has not been initialized") + if self.covariance is None: + raise ValueError("Covariance has not been initialized") + + def parameters(self): + """Returns an iterator over the model parameters.""" + yield self.mean + yield self.covariance + + def named_parameters(self): + """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" + yield "mean", self.mean + yield "covariance", self.covariance + + def is_valid_pdf(self): + r""" + Check if this Gaussian is a valid probability density function. + + Let's :math:`f(x)` denotes the probability density function, then in order to be a valid one, it has + to satisfy the following conditions: + 1. :math:`f(x) \geq 0 \forall x` + 2. :math:`\int_{-\infty}^{\infty} f(x) dx = 1` + + Returns: + bool: True if it is a valid one. + """ + # check that the covariance matrix is SPD + # check that integration is equal to 1 + if self.mean is None or self.cov is None: + return False + if self.is_psd(self.cov) and self.is_symmetric(self.cov): #and self.coefficient == 1.: + return True + + def pdf(self, x): + r""" + Probability density function evaluated at the given `x`. + + This is given by the following formula: + + .. math:: p(x) = \frac{1}{(2\pi)^\frac{d}{2} |\Sigma|^\frac{1}{2}} + \exp\left( - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right) + + where :math:`\Sigma` is the covariance matrix, :math:`\mu` is the mean, and :math:`d` is the dimensionality + of the Gaussian distribution. + + Args: + x (np.array): vector to evaluate the probability density function. + + Returns: + float: probability density evaluated at `x`. + """ + return mvn.pdf(x, self.mean, self.cov) + # return np.exp(self.log_pdf(x)) + + # aliases + prob = pdf + likelihood = pdf + + def log_pdf(self, x): + r""" + Log of the probability density function evaluated at `x`. + + This is given by the following formula: + + .. math:: \log p(x) = - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) - frac{d}{2} \log(2\pi) + - \frac{1}{2} \log |\Sigma| + + where :math:`\Sigma` is the covariance matrix, :math:`\mu` is the mean, and :math:`d` is the dimensionality + of the Gaussian distribution. + + Args: + x (np.array): vector to evaluate the log probability density function. + + Returns: + float: log probability density evaluated at `x`. + """ + return mvn.logpdf(x, self.mean, self.cov) + # prec = np.linalg.inv(self.cov) + # diff = x - self.mean + # log_det = np.log(np.linalg.det(self.cov)) + # return -0.5 * (diff.T.dot(prec).dot(diff) + self.mean.size * np.log(2*np.pi) + log_det) + + # alias + log_prob = log_pdf + log_likelihood = log_pdf + + def cdf(self, x): + r""" + Cumulative Distribution Function. + + Note that this requires at least scipy v1.1.0 + + .. math:: + + C(x) &= \int_{-\inf}^{x} p(x') dx' \\ + + Args: + x (np.array): vector to evaluate the cumulative distribution function. + + Returns: + float: cumulative distribution function evaluated at `x`. + """ + return mvn.cdf(x, self.mean, self.cov) + + def logcdf(self, x): + r""" + Log of the Cumulative Distribution Function. + + Note that this requires at least scipy v1.1.0 + + Args: + x (np.array): vector to evaluate the log of the cumulative distribution function. + + Returns: + float: log of the cumulative distribution function evaluated at `x`. + """ + return mvn.logcdf(x, self.mean, self.cov) + + def sample(self, size=None, seed=None): + """ + Generate `size` samples from the Gaussian distribution. + + Args: + size (int, None): number of samples + seed (int, None): seed for the random number generator + + Return: + array: samples + """ + return mvn.rvs(self.mean, self.cov, size=size, random_state=seed) + # return np.random.multivariate_normal(self.mean, self.cov, size=size) + + def distance(self, x): + r""" + Compute the distance of the given data from the mean by also taking into account the covariance. In the + 'Euclidean' space, this method returns the Mahalanobis distance which is defined as + :math:`D_M(x) = \sqrt{(x - \mu)^T \Sigma^{-1} (x - \mu)}`. + + Args: + x (np.array[D]): data vector + + Returns: + float: distance + """ + if self.manifold == 'euclidean': + diff = x - self.mean + return np.sqrt( diff.dot(self.precision).dot(diff) ) + + def entropy(self): + r""" + Differential entropy associated with the normal distribution. Note that the gaussian distribution is the + maximum entropy distribution given the 2 first moments (i.e. mean and covariance). + + .. math:: + + H(x) &= - \int p(x) \ln p(x) dx \\ + H(x) &= \frac{1}{2} \ln{ (2\pi\exp)^d + \det(\Sigma) } \\ + H(x) &= \frac{1}{2} \ln |\Sigma| + \frac{d}{2} (1 + \ln(2\pi)) + + where :math:`\Sigma` is the covariance matrix, and :math:`d` is the dimensionality of the Gaussian + distribution. + + Returns: + float: differential entropy + """ + # return mvn.entropy(self.mean, self.cov) + return 0.5 * np.log(np.linalg.det(self.cov)) + self.mean.size/2. * (1. + np.log(2*np.pi)) + + def kl_divergence(self, other): + r""" + Compute the Kullback-Leibler divergence between the two multivariate Gaussians. Note that this divergence + is not symmetric. + + Warnings: This only valid if the other distribution is also a Gaussian distribution. + + .. math:: + + D_{KL}(\mathcal{N}_1 || \mathcal{N}_2) = \frac{1}{2} \left{ log \frac{|\Sigma_2|}{|\Sigma_1|} + - d + tr(\Sigma_2^{-1}\Sigma_1) + (\mu_2 - \mu_1)^T \Sigma_2^{-1} (\mu_2 - \mu_1) \right} + + Args: + other (Gaussian): the other gaussian. + + Returns: + float: the divergence between the 2 Gaussian distributions. + """ + if not isinstance(other, Gaussian): + raise TypeError("Expecting another Gaussian distribution") + m = (other.mean - self.mean) + prec2 = np.linalg.inv(other.cov) + return 1./2 * (np.log(np.linalg.det(other.cov) / np.linalg.det(self.cov)) - self.size + + np.trace(prec2.dot(self.cov)) + m.T.dot(prec2).dot(m)) + + def fisher_information_matrix(self): + r""" + Return the Fisher Information Matrix of a multivariate normal distribution. + + Returns: + + Source: + [1] https://en.wikipedia.org/wiki/Fisher_information#Multivariate_normal_distribution + """ + pass + + def condition(self, input_value, output_idx, input_idx=None): + r""" + Compute the conditional distribution. + + Assume the joint distribution :math:`p(x_1, x2)` is modeled as a normal distribution, then + the conditional distribution of :math:`x_1` given :math:`x_2` is given by + :math:`p(x_1|x_2) = \mathcal{N}(\mu, \Sigma)` (which is also Gaussian), where the mean :math:`\mu` and + covariance :math:`\Sigma` are given by: + + .. math:: + + \mu &= \mu_1 + \Sigma_{12} \Sigma_{22}^{-1} (x_2 - \mu_2) \\ + \Sigma &= \Sigma_{11} - \Sigma_{12} \Sigma_{22}^{-1} \Sigma_{21} + + Args: + input_value (float[d2]): array of values :math:`x_2` such that we have :math:`p(x_1|x_2)` + output_idx (int[d1], int): indices that we are interested in, given (i.e. conditioned on) the other ones. + That is, the indices for :math:`x_1` + input_idx (int[d2], int, None): indices that we conditioned on, i.e. corresponding to the values. If None, + it will be inferred. That is, the indices for :math:`x_2` + + Returns: + Gaussian: Conditional Normal distribution + """ + # aliases + value, o, i = input_value, output_idx, input_idx + + value = np.array([value]) if isinstance(value, (int, float)) else np.array(value) + + if i is None: + o = np.array([o]) if isinstance(o, int) else np.array(o) + # from all the indices remove the output indices + i = np.array(list(set(range(self.size)) - set(o))) + i.sort() + + # make sure that the input indices have the same length as the value ones + i = i[:len(value)] + else: + i = np.array([i]) if isinstance(i, int) else np.array(i) + assert len(i) == len(value), "The value array and the idx2 array have different lengths" + + # compute conditional + c = self.cov[np.ix_(o, i)].dot(np.linalg.inv(self.cov[np.ix_(i, i)])) + mu = self.mean[o] + c.dot(value - self.mean[i]) + cov = self.cov[np.ix_(o, o)] - c.dot(self.cov[i, o]) + return Gaussian(mu, cov) + + def marginalize(self, idx): + r""" + Compute and return the marginal distribution (which is also Gaussian) of the specified indices. + + Let's assume that the joint distribution :math:`p(x_1, x_2)` is modeled as a Gaussian distribution, that is: + + .. math:: x \sim \mathcal{N}(\mu, \Sigma) + + where :math:`x = [x_1, x_2]`, :math:`\mu = [\mu_1, \mu_2]` and + :math:`\Sigma=\left[\begin{array}{cc} \Sigma_{11} & \Sigma_{12} \\ \Sigma_{21} & \Sigma_{22} \end{array}\right]` + + then the marginal distribution :math:`p(x_1) = \int_{x_2} p(x_1, x_2) dx_2` is also Gaussian and is given by: + + .. math:: p(x_1) = \mathcal{N}(\mu_1, \Sigma_{11}) + + Args: + idx (int, slice): indices of :math:`x_1` (this value should be between 0 and D-1, where D is + the dimensionality of the data) + + Returns: + Gaussian: marginal distribution (which is also Gaussian) + """ + if isinstance(idx, (int, float)): + idx = [idx] + return Gaussian(self.mean[idx], self.cov[np.ix_(idx,idx)]) + + def multiply(self, other): + r""" + Multiply a Gaussian by another Gaussian, by a square matrix (under an affine transformation), or a float + number. + + The product of two Gaussian PDFs is given by: + + .. math:: \mathcal{N}(\mu_1, \Sigma_1) \mathcal{N}(\mu_2, \Sigma_2) = C \mathcal{N}(\mu, \Sigma) + + where :math:`C = \mathcal{N}(\mu_1; \mu_2, \Sigma_1 + \Sigma_2)` is a constant (scalar), + :math:`\Sigma = (\Sigma_1^{-1} + \Sigma_2^{-1})^-1`, and + :math:`\mu = \Sigma (\Sigma_1^{-1} \mu_1 + \Sigma_2^{-1} \mu_2)`. + + Note that the product of two Gaussians is a Gaussian, but it is usually no more a valid probability density + function. In order to make it a proper probability distribution, we have to normalize it, which results to + remove the constant :math:`C`. + + The product of a Gaussian distribution :math:`\mathcal{N}(\mu, \Sigma)` with a square matrix :math:`A` gives: + + .. math:: Ax \sim \mathcal{N}(A \mu, A \Sigma A^T) + + The product of a Gaussian by a float does nothing as we have to re-normalize it to be a proper distribution. + + Args: + other (Gaussian, array_like of float[D,D], float): Gaussian, square matrix (to rotate or scale), or float + + Returns: + Gaussian: resulting Gaussian distribution + """ + # if other == Gaussian + if isinstance(other, Gaussian): + # coefficient = Gaussian(other.mean, self.cov + other.cov)(self.mean) * self.coefficient + prec1, prec2 = np.linalg.inv(self.cov), np.linalg.inv(other.cov) + cov = np.linalg.inv( prec1 + prec2 ) + mu = cov.dot( prec1.dot(self.mean) + prec2.dot(other.mean) ) + return Gaussian(mu, cov) # , coefficient=coefficient) + + # if other == square matrix + elif isinstance(other, np.ndarray): + return Gaussian(other.dot(self.mean), other.dot(self.cov).dot(other.T)) + + # if other == number + elif isinstance(other, (int, float)): + return self + + else: + raise TypeError("Trying to multiply a Gaussian with {}, which has not be defined".format(type(other))) + + def get_multiplication_coefficient(self, other): + r""" + Return the coefficient :math:`C` that appears when multiplying two Gaussians. + + As a reminder, the product of two Gaussian PDFs is given by: + + .. math:: \mathcal{N}(\mu_1, \Sigma_1) \mathcal{N}(\mu_2, \Sigma_2) = C \mathcal{N}(\mu, \Sigma) + + where :math:`C = \mathcal{N}(\mu_1; \mu_2, \Sigma_1 + \Sigma_2)` is a constant (scalar), + :math:`\Sigma = (\Sigma_1^{-1} + \Sigma_2^{-1})^-1`, and + :math:`\mu = \Sigma (\Sigma_1^{-1} \mu_1 + \Sigma_2^{-1} \mu_2)`. + + Args: + other (Gaussian): other Gaussian + + Returns: + float: resulting coefficient + """ + return Gaussian(other.mean, self.cov + other.cov)(self.mean) + + # def integrate_conjugate_prior(self, other): # TODO: call it marginal_likelihood + def marginal_distribution(self, x, prior): + r""" + Integrate the given Gaussian conjugate prior on the parameters with the current Gaussian PDF. + + .. math:: + + p(y; \theta) &= \int \mathcal{N}(y | \Phi(x) w, \Sigma_y) \mathcal{N}(w | \mu_w, \Sigma_w) dw \\ + &= \mathcal{N}(y | \Phi(x) \mu_w, \Phi(x) \Sigma_w \Phi(x)^T + \Sigma_y) + + Args: + prior (Gaussian): the other Gaussian conjugate prior + + Returns: + Gaussian: resulting Gaussian + """ + if isinstance(prior, Gaussian): + if callable(self.mean): + # TODO works with functions instead of arrays + Phi_x = self.mean.grad(x, self.mean.parameters) + # TODO check when the mean and covariance don't have the same shape + return Phi_x * Gaussian(prior.mean, prior.cov) + Gaussian(0, self.cov) + else: + raise TypeError("Expecting the prior to be a Gaussian distribution on the parameters of the mean " + "of this Gaussian") + + def power(self, exponent): + r""" + Raise to the power this Gaussian. + + Note that raising a Gaussian distribution to the power 2 is the same as multiplying the Gaussian with itself. + + Warnings: if the exponent is small, the resulting covariance will be large. Inversely, if the exponent + is very large the resulting covariance will be small. If the exponent is zero, this results in a uniform + distribution. + + Args: + exponent (float): strictly positive number + + Returns: + Gaussian: resulting gaussian distribution + """ + if exponent <= 0: + raise ValueError("The exponent has to be a strictly positive number") + return Gaussian(mean=self.mean, covariance=1./exponent * self.cov) + + def add(self, other): + r""" + The sum of independent Gaussian random variables (with the same dimension) is also Gaussian. This can + also be used to sum a Gaussian with a vector (affine transformation). + + The sum of two independent Gaussian RVs (with the same dimensionality), such that + :math:`x_1 \sim \mathcal{N}(\mu_1, \Sigma_1)` and :math:`x_2 \sim \mathcal{N}(\mu_2, \Sigma_2)`, is given + by :math:`x_1 + x_2 \sim \mathcal{N}(\mu_1 + \mu_2, \Sigma_1 + \Sigma_2)` + + The sum of a Gaussian distribution :math:`x \sim \mathcal{N}(\mu, \Sigma)` with a vector :math:`v` results in + a translation of this distribution, given by :math:`x \sim \mathcal{N}(\mu + v, \Sigma)`. + + Args: + other (Gaussian, float[d]): the other Gaussian distribution, or a vector. + + Returns: + Gaussian: resulting sum of two independent Gaussian distribution, or resulting sum of a Gaussian with + a vector. + """ + if isinstance(other, Gaussian): + return Gaussian(self.mean + other.mean, self.cov + other.cov) + return Gaussian(self.mean + other, self.cov) + + def affine_transform(self, A, b=None): + r""" + Perform an affine transformation on the Gaussian PDF. If :math:`x \sim \mathcal{N}(\mu, \Sigma)`, then + :math:`Ax+b ~ \mathcal{N}(A\mu + b, A^T \Sigma A)`. + + Args: + A (np.ndarray[D,D]): square matrix + b (np.ndarray[D]): vector + + Returns: + Gaussian: resulting Gaussian PDF + """ + if b is None: + return Gaussian(mean=A.dot(self.mean), covariance=A.dot(self.cov).dot(A.T)) + return Gaussian(mean=A.dot(self.mean) + b, covariance=A.dot(self.cov).dot(A.T)) + + def integrate(self, lower=None, upper=None): + r""" + Integrate the gaussian distribution between the two given bounds. + + Args: + lower (np.array[D], float, None): lower bound (default: -np.inf) + upper (np.array[D], float, None): upper bound (default: np.inf) + + Returns: + float: p(lower <= x <= upper) + """ + # check lower bound + if lower is None: + lower = np.full(self.mean.shape, -np.inf) + elif isinstance(lower, float): + lower = np.full(self.mean.shape, lower) + + # check upper bound + if upper is None: + upper = np.full(self.mean.shape, np.inf) + elif isinstance(upper, float): + upper = np.full(self.mean.shape, upper) + + # integrate + return scipy.stats.mvn.mvnun(lower, upper, self.mean, self.cov)[0] + + def grad(self, x, wrt='x'): + r""" + Compute the gradient of the Gaussian distribution evaluated at the given data. Let's + :math:`p(x; \mu, \Sigma) = \mathcal{N}(x | \mu, \Sigma) = \frac{1}{(2\pi)^\frac{d}{2} |\Sigma|^\frac{1}{2}} + \exp\left( - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right)` be the multivariate Gaussian distribution. + + Then (using [1]), we have: + + .. math:: + + \frac{\partial p(x; \mu, \Sigma)}{\partial x} &= - p(x) \Lambda (x - \mu) \\ + \frac{\partial p(x; \mu, \Sigma)}{\partial \mu} &= p(x) \Lambda (x - \mu) \\ + \frac{\partial p(x; \mu, \Sigma)}{\partial \Sigma} &= \frac{1}{2} p(x) (\Lambda (x-\mu)(x-\mu)^T \Lambda + - \Lambda) \\ + \frac{\partial p(x; \mu, \Lambda)}{\partial \Lambda} &= \frac{1}{2} p(x) (\Sigma - (x-\mu)(x-\mu)^T) + + where :math:`\Lambda = \Sigma^{-1}` is the precision matrix. + + Args: + x (np.array[D]): data vector + wrt (str): specify with respect to which variable we compute the gradient. It can take the following + values 'x', 'mu' or 'mean', 'sigma' or 'covariance', 'lambda' or 'precision'. + + Returns: + np.array: gradient of the same shape (as the variable from which we take the gradient) + + References: + [1] "The Matrix Cookbook" (math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf), Petersen and Pedersen, 2012 + """ + # TODO: check when x is a matrix + wrt = wrt.lower() + if wrt == 'x': + return - self.pdf(x) * self.precision.dot(x - self.mean) + elif wrt == 'mu' or wrt == 'mean': + return self.pdf(x) * self.precision.dot(x - self.mean) + elif wrt == 'sigma' or wrt[:3] == 'cov': + mu, L = self.mean, self.precision + diff = x - mu + return 0.5 * self.pdf(x) * (L.dot(np.outer(diff, diff)).dot(L) - L) + elif wrt == 'lambda' or wrt == 'precision': + diff = x - self.mean + return 0.5 * self.pdf(x) * (self.cov - np.outer(diff, diff)) + else: + raise ValueError("The given 'wrt' argument is not valid (see documentation)") + + def grad_fn(self, wrt='x'): + r""" + Compute the gradient function of the Gaussian wrt the current parameters (mean and covariance). Let's + :math:`p(x; \mu, \Sigma) = \mathcal{N}(x | \mu, \Sigma) = \frac{1}{(2\pi)^\frac{d}{2} |\Sigma|^\frac{1}{2}} + \exp\left( - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right)` be the multivariate Gaussian distribution. + + Then (using [1]), we have: + + .. math:: + + \frac{\partial p(x; \mu, \Sigma)}{\partial x} &= - p(x) \Lambda (x - \mu) \\ + \frac{\partial p(x; \mu, \Sigma)}{\partial \mu} &= p(x) \Lambda (x - \mu) \\ + \frac{\partial p(x; \mu, \Sigma)}{\partial \Sigma} &= \frac{1}{2} p(x) (\Lambda (x-\mu)(x-\mu)^T \Lambda + - \Lambda) + \frac{\partial p(x; \mu, \Lambda)}{\partial \Lambda} &= \frac{1}{2} p(x) (\Sigma - (x-\mu)(x-\mu)^T) + + where :math:`\Lambda = \Sigma^{-1}` is the precision matrix. + + Args: + wrt (str): specify with respect to which variable we compute the gradient. It can take the following + values 'x', 'mu' or 'mean', 'sigma' or 'covariance', 'lambda' or 'precision'. + + Returns: + callable: gradient function that can be evaluated later + + References: + [1] "The Matrix Cookbook" (math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf), Petersen and Pedersen, 2012 + """ + # TODO: check when x is a matrix + wrt = wrt.lower() + if wrt == 'x': + def wrap(pdf, mu, L): + def grad(x): + return - pdf(x) * L.dot(x - mu) + return grad + elif wrt == 'mu' or wrt == 'mean': + def wrap(pdf, mu, L): + def grad(x): + return pdf(x) * L.dot(x - mu) + return grad + elif wrt == 'sigma' or wrt[:3] == 'cov': + def wrap(pdf, mu, L): + def grad(x): + diff = x - mu + return 0.5 * pdf(x) * (L.dot(np.outer(diff, diff)).dot(L) - L) + return grad + elif wrt == 'lambda' or wrt == 'precision': + def wrap(pdf, mu, S): + def grad(x): + diff = x - mu + return 0.5 * pdf(x) * (S - np.outer(diff, diff)) + return grad + return wrap(self.pdf, self.mean, self.cov) + else: + raise ValueError("The given 'wrt' argument is not valid (see documentation)") + return wrap(self.pdf, self.mean, self.precision) + + def hessian(self, x, wrt='x'): + r""" + Compute the Hessian matrix (2nd derivative) of the Gaussian distribution evaluated at the given data. Let's + :math:`p(x; \mu, \Sigma) = \mathcal{N}(x | \mu, \Sigma) = \frac{1}{(2\pi)^\frac{d}{2} |\Sigma|^\frac{1}{2}} + \exp\left( - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right)` be the multivariate Gaussian distribution. + + Then (using [1]), we have: + + .. math:: + + \frac{\partial^2 p(x; \mu, \Sigma)}{\partial x^2} &= p(x) (\Lambda (x-\mu)(x-\mu)^T \Lambda - \Lambda) \\ + \frac{\partial^2 p(x; \mu, \Sigma)}{\partial \mu^2} &= p(x) (\Lambda (x-\mu)(x-\mu)^T \Lambda - \Lambda) + + where :math:`\Lambda = \Sigma^{-1}` is the precision matrix. + + Args: + x (np.array[D]): data vector + wrt (str): specify with respect to which variable we compute the gradient. It can take the following + values 'x', 'mu' or 'mean'. + + References: + [1] "The Matrix Cookbook" (math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf), Petersen and Pedersen, 2012 + """ + # TODO: check when x is a matrix + # TODO: derive the hessian wrt the covariance and precision matrix + if wrt == 'x' or wrt =='mu' or wrt == 'mean': + mu, L = self.mean, self.precision + diff = x - mu + return self.pdf(x) * (L.dot(np.outer(diff, diff)).dot(L) - L) + else: + raise ValueError("The given 'wrt' argument is not valid (see documentation)") + + def hessian_fn(self, wrt='x'): + r""" + Compute the Hessian function of the Gaussian wrt to the current parameters (mean and covariance). Let's + :math:`p(x; \mu, \Sigma) = \mathcal{N}(x | \mu, \Sigma) = \frac{1}{(2\pi)^\frac{d}{2} |\Sigma|^\frac{1}{2}} + \exp\left( - \frac{1}{2} (x - \mu)^T \Sigma^{-1} (x - \mu) \right)` be the multivariate Gaussian distribution. + + Then (using [1]), we have: + + .. math:: + + \frac{\partial^2 p(x; \mu, \Sigma)}{\partial x^2} &= p(x) (\Lambda (x-\mu)(x-\mu)^T \Lambda - \Lambda) \\ + \frac{\partial^2 p(x; \mu, \Sigma)}{\partial \mu^2} &= p(x) (\Lambda (x-\mu)(x-\mu)^T \Lambda - \Lambda) + + where :math:`\Lambda = \Sigma^{-1}` is the precision matrix. + + Args: + wrt (str): specify with respect to which variable we compute the gradient. It can take the following + values 'x', 'mu' or 'mean'. + + References: + [1] "The Matrix Cookbook" (math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf), Petersen and Pedersen, 2012 + """ + # TODO: check when x is a matrix + # TODO: derive the hessian wrt the covariance and precision matrix + if wrt == 'x' or wrt == 'mu' or wrt == 'mean': + def wrap(pdf, mu, L): + def grad(x): + diff = x - mu + return self.pdf(x) * (L.dot(np.outer(diff, diff)).dot(L) - L) + return grad + else: + raise ValueError("The given 'wrt' argument is not valid (see documentation)") + return wrap(self.pdf, self.mean, self.precision) + + def update(self, x): + r""" + Update the Gaussian distribution by taking into account the new given point(s) `x`. Specifically, it updates + the mean and covariance matrix based on the given point(s) `x`. This is only valid if the initial data was + provided, as we need to know the total number of data points that were previously given. + + The updated mean is given by: + + .. math:: \mu_{N + M} = \frac{N}{N + M} \mu_N + \frac{M}{N + M} \mu_M + + where :math:`N` is the initial number of data points, :math:`M` is the number of given data points, + :math:`\mu_N` is the previous mean, and :math:`\mu_M` is the mean computed on the given data. + + The udpated covariance (without the bessels correction) is given by: + + .. math:: + + \Sigma_{N+M} = \frac{N}{N+M} (\Sigma_N + \mu_N\mu_N^T) + \frac{M}{N+M} (\Sigma_M + \mu_M\mu_M^T) + - \mu_{N+M}\mu_{N+M}^T + + Args: + x (np.array): data vector/matrix + """ + if self.N is None: + # if the mean and covariance are not defined, learn from scratch + if self.mean is None and self.cov is None: + self.mle(x) + else: + raise RuntimeError("The number of data points was never specified. This is needed in order to " + "update the mean and covariance in an online fashion") + else: + # compute the number of data points + N = self.N + M = 1 if len(x.shape) == 1 else x.shape[0] + + # compute the updated mean + mu_N, mu_M = self.mean, self.compute_mean(x) + ratio_N, ratio_M = float(N) / (N + M), float(M) / (N + M) + mu = ratio_N * mu_N + ratio_M * mu_M + + # compute the updated covariance + # TODO: check math with bessel correction + sigma_N, sigma_M = self.cov, self.compute_covariance(x) + cov = ratio_N * (sigma_N + np.outer(mu_N, mu_N)) + ratio_M * (sigma_M + np.outer(mu_M, mu_M)) \ + - np.outer(mu, mu) + + # update the mean, covariance, and number of data points + self.mean = mu + self.cov = cov + self.N = N + M + + def mle(self, data): + r""" + Perform maximum likelihood estimate (MLE) given the data. This results to compute the empirical mean and + covariance from the data. + + .. math:: \max_\theta p(X | \theta) + + where :math:`\theta` is the set of parameters, which in this case are the mean and covariance matrix, + i.e. :math:`\theta = \{ \mu, \Sigma \}`, and :math:`X` represents the data set. + + Args: + data (np.array[N,D]): data matrix of shape NxD + + Returns: + float: value of the maximum likelihood estimate obtained + """ + self.mean = self.compute_mean(data) + self.cov = self.compute_covariance(data) + self.N = data.shape[0] + return self.pdf(data) + + # alias + fit = mle + maximum_likelihood = mle + + def map(self, data, mean_prior, covariance_prior): + r""" + Maximum a posteriori estimation (MAP). + + .. math:: \max_theta p(X | \theta) p(\theta) + + where :math:`\theta` is the set of parameters, which in this case are the mean and covariance matrix, + i.e. :math:`\theta = \{ \mu, \Sigma \}`, and :math:`X` represents the data set. + + Args: + data (np.array[N,D]): data matrix of shape NxD + mean_prior: + covariance_prior: + + Returns: + float: value of the MAP estimate obtained + """ + pass + + def bayesian_inference(self, data, mean_prior, covariance_prior): + r""" + Perform bayesian inference to estimate the mean and covariance given the data, the conjugate prior of the + mean (which is also a multivariate normal distribution), and the conjugate prior of the covariance matrix + (which is an inverse Wishart distribution). + + Args: + data (np.array[N,D]): data matrix of shape NxD + mean_prior (Gaussian): + covariance_prior (InverseWishart): # TODO + + Returns: + None + """ + pass + + def ellipsoid_axes(self): # ellipse_confidence # confidence_region + r""" + Compute the axes of the ellipsoid defined by the covariance matrix. Specifically, it returns the main + axes of the ellipsoid (i.e. its orientation) as well as their scaling. + + Returns: + np.array[D]: square root of eigenvalues in descending order + np.array[D,D]: eigenvectors arranged in column vectors in descending order + """ + evals, evecs = np.linalg.eigh(self.cov) + return np.sqrt(evals[::-1]), evecs[:,::-1] + + def plot2D_ellipse(self, ax=None, data=None): + r""" + Project (linearly) the given data on a 2D surface, and plot the 2D confidence ellipse associated with the + Gaussian PDF. + + Args: + ax (matplotlib.axes.Axes): axis of the figure (optional). + data (None): data matrix of shape NxD. If D>2, it will project the data using PCA. If no data is provided, + it will just draw the 2D confidence ellipse. + + Returns: + None + """ + pass + + # def __array_ufunc__(self, *args): + # print(args) + + ############# + # Operators # + ############# + + def __str__(self): + return 'Gaussian of dimension {}'.format(self.size) + + def __len__(self): + """dimensionality of the Gaussian distribution""" + return len(self.mean) + + def __call__(self, x=None, size=None): + """ + If no arguments are given, it returns one sample from the distribution. If a vector is provided, it returns + the probability associated with this one (i.e. how probable it is that the given sample was generated from + this Gaussian distribution), that is the pdf evaluated at the given vector. + + Args: + x (np.array, None): vector to evaluate the probability density function. + size (int, None): number of samples + + Returns: + float, or np.array: probability density evaluated at `x`, or samples + """ + if x is not None: + return self.pdf(x) + return self.sample(size=size) + + def __getitem__(self, idx): + """ + Conditional and marginal distribution. + + Args: + idx (int, slice, tuple): if int or slice, it will return the marginal distribution. If tuple, it + will return the conditional distribution. + + Returns: + Gaussian: conditional or marginal distribution + + Examples: + # joint distribution p(x1,x2) + g = Gaussian(np.array([1.,2.]), np.identity(2)) + + # marginal distribution p(x1) and p(x2) + marg1 = g[0] + marg2 = g[1] + + # conditional distribution p(x1|x2) and p(x2|x1) + sample = g.sample() + cond1 = g[0,1,sample[0]] + cond2 = g[1,0,sample[1]] + """ + if isinstance(idx, tuple): # conditional distribution + if len(idx) == 2: + value, idx1 = idx + idx2 = None + elif len(idx) == 3: + value, idx1, idx2 = idx + else: + raise IndexError("Expecting two or three indices: value, idx1 (, idx2)") + return self.condition(value, idx1, idx2) + else: # marginal distribution + return self.marginalize(idx) + + def __add__(self, other): + """ + The sum of two independent Gaussian random variables (with the same dimension) is also Gaussian. This can + also be used to sum a Gaussian with a vector (affine transformation). + + Args: + other (Gaussian, float[d]): the other Gaussian distribution, or a vector. + + Returns: + Gaussian: resulting sum of two independent Gaussian distribution, or resulting sum of a Gaussian with + a vector. + """ + return self.add(other) + + def __radd__(self, other): + return self.add(other) + + def __mul__(self, other): + """ + Multiply two Gaussian distributions, or multiply a gaussian distribution with a matrix (as done during + an affine transformation). + + Args: + other (Gaussian, array_like of float[D,D]): Gaussian, or square matrix (to rotate or scale) + + Returns: + Gaussian: resulting Gaussian distribution + """ + return self.multiply(other) + + def __rmul__(self, other): + return self.multiply(other) + + def __pow__(self, exponent): + """Apply the power exponent on the Gaussian""" + return self.power(exponent) + + +MVN = Gaussian + + +###################### +# Plotting functions # +###################### + +def plot3D(ax, X, Y, pdf, title=None, xlabel='x1', ylabel='x2', zlabel='x3'): + if isinstance(pdf, (tuple, list)): + pdf = np.max(np.dstack(pdf), axis=-1) + ax.set(title=title, xlabel=xlabel, ylabel=ylabel, zlabel=zlabel) + ax.plot_surface(X, Y, pdf, cmap='viridis', linewidth=0) + + +def plot2DContour(ax, x, y, pdf, title=None, xlabel='x1', ylabel='x2'): + if isinstance(pdf, (tuple, list)): + pdf = np.max(np.dstack(pdf), axis=-1) + ax.set(title=title, xlabel=xlabel, ylabel=ylabel) + ax.contourf(x, y, pdf) + + +def plot3DAnd2DCountour(gaussians, step=500, bound=10, fig=None, title='', block=True): + if not isinstance(gaussians, (list, tuple)): + gaussians = [gaussians] + + # Create grid and multivariate normal + x = np.linspace(-bound, bound, step) + y = np.linspace(-bound, bound, step) + X, Y = np.meshgrid(x, y) + pos = np.empty(X.shape + (2,)) + pos[:, :, 0], pos[:, :, 1] = X, Y + pdf = [gaussian.pdf(pos) for gaussian in gaussians] + + # if more than one gaussian, fuse by taking the maximum + if len(pdf) > 1: + pdf = np.max(np.dstack(pdf), axis=-1) + else: + pdf = pdf[0] + + # create figure + fig = plt.figure(figsize=plt.figaspect(0.5)) # Twice as wide as it is tall. + plt.suptitle(title) + + # 1st subplot (3D) + ax = fig.add_subplot(1, 2, 1, projection='3d') + plot3D(ax, X, Y, pdf, title='p(x1, x2)', xlabel='x1', ylabel='x2', zlabel='p') + + # 2nd subplot (2D) + ax = fig.add_subplot(1, 2, 2) + plot2DContour(ax, x, y, pdf, title='p(x1, x2)', xlabel='x1', ylabel='x2') + + # show plot + fig.tight_layout() + plt.show(block=block) + + +def plot2DEllipse(ax, gaussian, color='g', fill=False, plot_2devs=False, plot_arrows=True): + # alias + g = gaussian + + # compute std deviation and eigenvectors from the gaussian + std_dev, evecs = g.ellipsoid_axes() + + # plot ellipse from standard deviations and eigenvectors + width, height = 2 * std_dev[0], 2 * std_dev[1] + angle = np.rad2deg(np.arccos(evecs[:, 0].dot([1, 0]))) + # if 3rd or 4th quadrant, we need to reverse the sign for the angle + x, y = evecs[:, 0] + if y < 0: + angle = -angle + + facecolor = color if fill else 'none' + + # 2 ellipses (one std dev and two std dev) + ellipse_2std = Ellipse(xy=g.mean, width=2 * width, height=2 * height, angle=angle, edgecolor=color, lw=2, + facecolor=facecolor, alpha=0.5) + ax.add_artist(ellipse_2std) + if plot_2devs: + ellipse_1std = Ellipse(xy=g.mean, width=width, height=height, angle=angle, edgecolor=color, lw=2, + facecolor=facecolor, alpha=0.8) + ax.add_artist(ellipse_1std) + + # if we need to plot arrows + if plot_arrows: + # compute scaled eigenvectors + p = std_dev * evecs + # draw arrows + ax.arrow(g.mean[0], g.mean[1], p[0, 0], p[1, 0], length_includes_head=True, head_width=0.15, color='r') + ax.arrow(g.mean[0], g.mean[1], p[0, 1], p[1, 1], length_includes_head=True, head_width=0.15, color='r') + + return ellipse_2std + + +def plot3DAnd2DConditional(joint_gaussian, cond_gaussian, x1_value = 0, step=500, bound=10, block=True): + + # Create grid and multivariate normal + x = np.linspace(-bound, bound, step) + y = np.linspace(-bound, bound, step) + X, Y = np.meshgrid(x, y) + pos = np.empty(X.shape + (2,)) + pos[:, :, 0], pos[:, :, 1] = X, Y + joint_pdf = joint_gaussian.pdf(pos) + cond_pdf = cond_gaussian.pdf(x) + z_max = pdf.max() + + # create figure + fig = plt.figure(figsize=plt.figaspect(0.5)) # Twice as wide as it is tall. + plt.suptitle('Conditional Gaussian') + + # 1st subplot (3D) + ax = fig.add_subplot(1, 2, 1, projection='3d') + plot3D(ax, X, Y, joint_pdf, title='p(x1,x2)', xlabel='x1', ylabel='x2', zlabel='p') + + # draw plane that cut the gaussian + y1 = np.linspace(-bound, bound, 2) + z = np.linspace(0, z_max, 2) + Y1, Z = np.meshgrid(y1, z) + ax.plot_surface(x1_value, Y1, Z, color='red', alpha=0.4) + # cset = ax.contourf(X, Y, pdf, zdir='x', offset=-bound, cmap=cm.coolwarm) + + # 2nd subplot (2D) + ax = fig.add_subplot(1, 2, 2) + # ax.plot(x, pdf[step / 2 + x1_value * step / (2 * bound)]) + ax.plot(x, cond_pdf) + ax.set(title='p(x2|x1)', xlabel='x2', ylabel='p(x2|x1)') + + # show plot + fig.tight_layout() + plt.show(block=block) + + +# TESTS +if __name__ == '__main__': + # import matplotlib.pyplot as plt + # from mpl_toolkits.mplot3d import Axes3D + # from matplotlib.patches import Ellipse + + # create two 2D Gaussian distributions + m1, c1 = np.array([0.,0.]), np.identity(2)*0.5 + m2, c2 = np.array([1.5,1.5]), np.array([[1.,0.5], [0.5,2.]]) + g1 = Gaussian(m1, c1) + g2 = Gaussian(m2, c2) + + # sample from the Gaussian distributions, and plot them + d1 = g1.sample(size=200) + d2 = g2.sample(size=200) + fig, ax = plt.subplots(1,1) + ax.set(title='sampling from 2 Gaussians', aspect='equal') + ax.scatter(d1[:,0], d1[:,1], color='b', alpha=0.7) + ax.scatter(d2[:,0], d2[:,1], color='r', alpha=0.7) + plt.show() + + # 3D and 2D plots of the Gaussian distributions + plot3DAnd2DCountour([g1, g2]) + + # Use 1 Gaussian # + + # check if the Gaussian distribution produces the same results as `scipy.stats.multivariate_normal` + from scipy.stats import multivariate_normal + # create grid and multivariate normal + bound, step = 10., 500 + x = np.linspace(-bound, bound, step) + y = np.linspace(-bound, bound, step) + X, Y = np.meshgrid(x, y) + pos = np.empty(X.shape + (2,)) + pos[:, :, 0], pos[:, :, 1] = X, Y + # evaluate PDF using Gaussian and Scipy + pdf = g2.pdf(pos) + pdf2 = multivariate_normal(g2.mean, g2.cov).pdf(pos) + print("Same PDF produced by Gaussian and Scipy: {}".format(np.allclose(pdf, pdf2))) + + # check if valid PDF: integrate from -inf to inf the Gaussian distribution (it should be equal to 1) + # \int \int p(x1,x2) dx1 dx2 = 1 + # p(x1, x2) = pdf; dx = 2.*bound/step; dx1 dx2 = (2.*bound/step)**2 + print("Integration from -inf to inf: {}".format(g2.integrate())) + print("Summation: {}".format((2. * bound / step) ** 2 * pdf.sum())) + + # samples from the Gaussian and plot ellipse + samples = g2.sample(size=100) + fig, ax = plt.subplots(1,1) + ax.set(title='Sampling from one Gaussian', aspect='equal') + ax.scatter(samples[:, 0], samples[:, 1], color='b') + plot2DEllipse(ax, g2, fill=True, plot_2devs=True, plot_arrows=True) + plt.show() + + # conditional distribution of the Gaussian p(y|x) + x_value = 0 + g_cond = g2.condition(input_value=x_value, output_idx=1) + plot3DAnd2DConditional(g2, g_cond, x_value) + + # marginalization of the Gaussian by summing and using the normal distribution + # by summing + dx = 2. * bound / step + y_sum = pdf.sum(axis=0) * dx + # using gaussian marginalize function + g_margin = g2.marginalize(idx=0) + y_margin = g_margin.pdf(x) + # plot + fig, axes = plt.subplots(1, 2, figsize=plt.figaspect(0.5)) + axes[0].plot(x, y_sum, color='blue') + axes[0].set(title='Marginalization by integrating', xlabel='x2', ylabel='p(x2)') + axes[1].plot(x, y_margin, color='red') + axes[1].set(title='Marginalization using indexing', xlabel='x2', ylabel='p(x2)') + fig.tight_layout() + plt.show() + + # affine transformation on the Gaussian distribution + b = np.array([-2, -1]) + theta = np.deg2rad(-45) + A = np.array([[np.cos(theta), -np.sin(theta)], + [np.sin(theta), np.cos(theta)]]) + g_aff = A * g2 + b + print("Gaussian under affine transformation: mean={} and cov={}".format(g_aff.mean, g_aff.cov)) + samples = g_aff.sample(size=500) + fig, ax = plt.subplots(1, 1) + ax.set(title='Gaussian under affine transformation', aspect='equal') + ax.scatter(samples[:, 0], samples[:, 1], color='b') + plot2DEllipse(ax, g_aff) + plt.show() + + # use 2 Gaussians # + + # addition of two independent Gaussians + g_sum = g1 + g2 + fig, ax = plt.subplots(1,1) + ax.set(title='addition', xlim=[-5, 5], ylim=[-5, 5], aspect='equal') + e1 = plot2DEllipse(ax, g1, color='g', plot_arrows=False) + e2 = plot2DEllipse(ax, g2, color='b', plot_arrows=False) + e3 = plot2DEllipse(ax, g_sum, color='r', plot_arrows=False) + ax.legend([e1, e2, e3], ['G1', 'G2', 'G1+G2'], loc=2) + plt.show() + + # multiplication of two independent Gaussians + g_mul = g1 * g2 + fig, ax = plt.subplots(1, 1) + ax.set(title='multiplication', xlim=[-5, 5], ylim=[-5, 5], aspect='equal') + e1 = plot2DEllipse(ax, g1, color='g', plot_arrows=False) + e2 = plot2DEllipse(ax, g2, color='b', plot_arrows=False) + e3 = plot2DEllipse(ax, g_mul, color='r', plot_arrows=False) + ax.legend([e1, e2, e3], ['G1', 'G2', 'G1*G2'], loc=2) + plt.show() + + # Fit a Gaussian on given data # + + # create data + g_data = Gaussian(mean=np.array([2,3]), covariance=np.array([[1, -0.5], [-0.5, 1]])) + samples = np.random.multivariate_normal(mean=g_data.mean, cov=g_data.cov, size=1000) + + # fit one Gaussian and plot it along the data + g = Gaussian() + g.fit(samples) + plot3DAnd2DCountour(g_data, title='Gaussian that generated the data', block=False) + plot3DAnd2DCountour(g, title='fitted Gaussian') + + # TODO + # fit Gaussian on different manifolds # + # create Gaussians on ... manifold and plot it diff --git a/pyrobolearn/models/gmm.py b/pyrobolearn/models/gmm.py new file mode 100755 index 0000000..1ef9202 --- /dev/null +++ b/pyrobolearn/models/gmm.py @@ -0,0 +1,1741 @@ +#!/usr/bin/env python +"""Define the Gaussian Mixture Model and Gaussian Mixture Regression + +This file provides the Gaussian Mixture Model, and uses the Gaussian model defined in the `gaussian.py` file. +Gaussian Mixture Regression is achieved by conditioning the GMM to some input. +""" + + +import numpy as np +try: + import cPickle as pickle +except ImportError as e: + import pickle +from sklearn.cluster import KMeans +from sklearn.mixture import GaussianMixture, BayesianGaussianMixture + +# from model import Model +from gaussian import Gaussian + + +__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 GMM(object): + r"""Gaussian Mixture Model + + This class described the Gaussian Mixture Model (GMM); a semi-parametric, probabilistic and generative model [1,2]. + In robotics, for instance, this is often used to model trajectories by jointly encoding the time and state + (position and velocity) [3,4,5,6]. + + It is mathematically described by: + + .. math:: p(x) = \sum_{k=1}^{K} \pi_k \mathcal{N}(\mu_k, \Sigma_k) + + where :math:`K` is the number of components, :math:`\pi_k` are prior probabilities (that is + :math:`0 \leq \pi_k \leq 1`) that sums to 1 (i.e. :math:`\sum_{k=1}^K \pi_k = 1`), + :math:`\mathcal{N}(\mu_k, \Sigma_k)` is the multivariate Gaussian (aka Normal) distribution, with mean + :math:`\mu_k` and covariance :math:`\Sigma_k`. The priors, means, and covariances are grouped to form the + parameter set :math:`\theta = \{\pi_k, \mu_k, \Sigma_k\}_{k=1}^K`. + + + Learning from data: + ------------------- + + There are three main ways to learn the parameters: maximum likelihood estimate (MLE), maximum a posteriori + estimate (MAP), and bayesian inference (using variational inference). Here, we will focus on MLE. + + Given a dataset :math:`X \in \mathbb{R}^{N \times D}`, the log-likelihood of the GMM is given by: + + .. math:: + + \mathcal{L}(\theta) &= \log p(X | \theta) = \log p(X | \pi, \mu, \Sigma) \\ + &= \sum_{n=1}^N \log \sum_{k=1}^K \pi_k \mathcal{N}(x_n | \mu_k, \Sigma_k) + + The summation inside the logarithm in the above loss does not allow for a closed-form solution. We thus turn our + attention to an iterative algorithm that maximizes this last one. + + The Expectation-Maximization (EM) algorithm [1,2] allows to find the maximum likelihood estimate for models having + latent variables. This algorithm consists of 4 main steps: + 1. Initialize the parameters :math:`\theta = \{\pi_k, \mu_k, \Sigma_k\}_{k=1}^K` + 2. Expectation step: evaluate the posterior :math:`p(Z | X, \theta_{old})` while fixing the parameters. + 3. Maximization step: maximize the expected value of the complete-data log-likelihood under the posterior + distribution of the latent variables (found during the Expectation step). That is, + :math:`\max_\theta Q(\theta, \theta_{old}) = \max_\theta \sum_{Z} p(Z | X, \theta_{old}) \log p(X,Z | \theta)`. + 4. Evaluate the log-likelihood loss, and check if it converged. If it didn't, go back to step 2. + + To be more specific, the EM algorithm alternatively computes a lower bound on the log-likelihood for the current + parameters, and then maximize this bound to obtain the new parameter values (see [1], sec 9.4 for more details). + This results in the above algorithm. + + Few notes with respect to the EM algorithm: + * this guarantees an improvement over the but the initialization is quite important. In the literature, we can + often initialize it using the K-means algorithm. + * while other learning algorithms such as gradient ascent could be used, one of the major problem is that they + do not enforce constraints on the priors and covariance matrices during the optimization. + + For other variants of the EM algorithm, please refer to [2], section 11.4.9. + + + Gaussian Mixture Regression (i.e. conditioned GMM): + -------------------------------------------------- + + Gaussian Mixture Regression [3,4] consists to condition the GMM (that models the joint distribution over the input + and output variables :math:`p(x^I, x^O)`) on a part of the variables (for instance, the input variables + :math:`p(x^O | x^I`). Let's :math:`x = [x^I, x^O]`, :math:`\mu_k = [\mu_k^I, \mu_k^O]`, and :math:`\Sigma_k = + \left[ \begin{array}{cc} \Sigma_k^I & \Sigma_k^{IO} \\ \Sigma_k^{OI} & \Sigma_k^O \end{array} \right]`, where + :math:`I` and :math:`O` are the superscripts to refer the input and output respectively. + + .. math:: + + p(x^O | x^I) &= \sum_{k=1}^K p(z_k=1 | x^I) p(x^O | x^I, z_k=1) \\ + &= \sum_{k=1}^K r_k(x^I) \mathcal{N}(\hat{\mu}_k^O(x^I), \hat{\Sigma}_k^O) + + where :math:`r_k(x^I) = \frac{\pi_k \mathcal{N}(x^I|\mu_k^I, \Sigma_k^I)}{\sum_{j=1}^{K} \pi_j + \mathcal{N}(x^I|\mu_j^I,\Sigma_j^I)}` are the responsibilities, :math:`\hat{\mu}_k^O(x^I) = + \mu_k^O + \Sigma_k^{OI} \Sigma_k^I^{-1} (x^I - \mu_k^I)` and :math:`\hat{\Sigma}_k^O = \Sigma_k^O - + \Sigma_k^{OI} (\Sigma_k^I)^{-1} \Sigma_k^{IO}` are the resulting conditioned means and covariances, + respectively. + + This results in another GMM, which can be approximated by a simple Gaussian (see [4] for more info, or the + documentation of the corresponding method: `approximate_by_single_gaussian`): + + .. math:: + + p(x^O | x^I) \approx \mathcal{N}(x^O | \hat{\mu}^O(x^I), \hat{\Sigma}^O(x^I)) + + where :math:`\hat{\mu}^O(x^I) = \sum_{k=1}^K r_k(x^I) \hat{\mu}_k^O(x^I)` and :math:`\hat{\Sigma}^O(x^I) = + \sum_{k=1}^K r_k(x^I) (\hat{\Sigma}_k^O + \hat{\mu}_k^O(x^I) \hat{\mu}_k^O(x^I)^T) - \hat{\mu}^O(x^I) + \hat{\mu}^O(x^I)^T`. + + + Other miscellaneous information: + -------------------------------- + + The conjugate prior of the GMM is the Dirichlet process. + + + References: + [1] "Pattern Recognition and Machine Learning" (chap 2, 3, 9, and 10), Bishop, 2006 + [2] "Machine Learning: a Probabilistic Perspective" (chap 3 and 11), Murphy, 2012 + [3] "Robot Programming by Demonstration: a Probabilistic Approach" (chap 2), Calinon, 2009 + [4] "A Tutorial on Task-Parameterized Movement Learning and Retrieval", Calinon, 2015 + [5] "Programming by Demonstration on Riemannian Manifolds" (PhD thesis, chap 1 and 2), Zeerstraten, 2017 + [6] "Learning Control", Calinon et al., 2018 + + The code was inspired by the following codes: + - `gaussian.py`: defines the Gaussian distribution + - `sklearn.mixture.gmm` and `sklearn.mixture.dpgmm`: http://scikit-learn.org/stable/modules/mixture.html + - `gmr`: https://github.com/AlexanderFabisch/gmr + - `pybdlib`: https://gitlab.idiap.ch/rli/pbdlib-python/tree/master/pbdlib + - `riepybdlib.statistics`: https://gitlab.martijnzeestraten.nl/martijn/riepybdlib + """ + + def __init__(self, num_components=1, priors=None, means=None, covariances=None, gaussians=None, seed=None, + dimensionality=None, manifold=None): + """ + Initialize the Gaussian Mixture Model (GMM). + + Args: + num_components (int): the number of components/gaussians (this argument should be provided if + no priors, means, covariances, or gaussians are provided) + priors (list/tuple of floats, None): prior probabilities (they have to be positives). If not provided, + it will be a uniform distribution. + means (list of np.array[D], None): list of means + covariances (list of np.array[D,D], None): list of covariances + gaussians (list of Gaussian, None): list of gaussians. If provided, the `means` and `covariances` + parameters don't have to be provided. + seed (int): random seed. Useful when sampling and for EM algo. + dimensionality (int, None): dimensionality of the data (this can be inferred during the training process) + manifold (None): By default, it is the Euclidean space. + """ + super(GMM, self).__init__() + + # variables: number of components, dimensionality, and the number of data + self.K = num_components + self.N = 0 + + # set seed + self.seed = seed + + # set gaussians + self._gaussians = [] + if gaussians is None: + if means is not None and covariances is not None: + self._gaussians = [Gaussian(mean=mean, covariance=cov) for mean, cov in zip(means, covariances)] + self.K = len(self._gaussians) + else: + self._gaussians = gaussians + self.K = len(self._gaussians) + + # set priors + self.priors = priors + + # check if the priors and gaussians have the same number of components + if priors is not None and gaussians is not None: + if len(priors) != len(gaussians): + raise ValueError("The number of priors and gaussians are differents") + + ############## + # Properties # + ############## + + @property + def seed(self): + """Return the seed""" + return self._seed + + @seed.setter + def seed(self, seed): + """Set the random seed""" + self._seed = seed + np.random.seed(self._seed) + + @property + def num_components(self): + """Return the number of components, i.e. the number of Gaussians""" + return self.K + + # alias + size = num_components + + @property + def dimensionality(self): + """Return the dimensionality of the mean""" + if len(self._gaussians) > 0: + return self._gaussians[0].dim + return 0 + + # alias + dim = dimensionality + + @property + def num_parameters(self): + """Return the number of free parameters""" + D = self.dimensionality + return (self.K - 1) + self.K * (D + 1./2 * D * (D + 1)) + + @property + def num_data(self): + """Return the number of data points""" + return self.N + + @property + def priors(self): + r"""Return the priors :math:`\pi_k`""" + return self._priors + + @priors.setter + def priors(self, priors): + """Set the priors""" + if priors is not None: + priors = np.array(priors, dtype=np.float64) + + # check shape + if len(priors.shape) != 1: + raise ValueError("Expecting 1d array for the priors, instead got a shape of {}".format(priors.shape)) + + # check if the priors are positives + if not np.all(priors >= 0): + raise ValueError("Some priors are not positives") + + # renormalize just in case + if len(priors) > 0: + priors /= np.sum(priors) + + # set the priors and the number of components + self._priors = priors + self.K = len(self._priors) + else: + if self.num_components > 0: + self._priors = np.ones(self.num_components) / self.num_components + else: + self._priors = np.array([]) + + @property + def gaussians(self): + r"""Return the Gaussian distributions :math:`\mathcal{N}(\mu_k, \Sigma_k)`""" + return self._gaussians + + @property + def means(self): + """Return the means (shape: KxD): the mean of each Gaussian""" + return np.array([gaussian.mean for gaussian in self._gaussians]) + + @property + def covariances(self): + """Return the covariances (shape: KxDxD): the covariance of each Gaussian""" + return np.array([gaussian.covariance for gaussian in self._gaussians]) + + @property + def precisions(self): + """Return the precisions (shape: KxDxD): the precision of each Gaussian""" + return np.array([gaussian.precision for gaussian in self._gaussians]) + + @property + def mean(self): + r"""Return the expected value (i.e. mean) of the GMM: :math:`\mu = \sum_{k=1}^K \pi_k \mu_k`""" + return np.sum(self.priors * self.means.T, axis=1) + + @property + def covariance(self): + r"""Return the covariance of the GMM: :math:`cov = \sum_{k=1}^K \pi_k (\Sigma_k + \mu_k\mu_k^T) - \mu\mu^T`""" + cov = np.sum([prior * (g.covariance + np.outer(g.mean, g.mean)) + for prior, g in zip(self.priors, self.gaussians)], axis=0) + return cov - np.outer(self.mean, self.mean) + + @property + def precision(self): + r"""Return the precision of the GMM: :math:`\Lambda = \Sigma^{-1}` (see `covariance` property)""" + return np.linalg.inv(self.covariance) + + @property + def gaussian(self): + """Perform moment matching to approximate a GMM as a Gaussian""" + return Gaussian(self.mean, self.covariance) + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other): + """Copy the given GMM""" + if not isinstance(other, GMM): + raise ValueError("Expecting to copy another GMM") + pass + + @staticmethod + def is_parametric(): + r""" + The GMM is a semi-parametric model, where the parametric part is due to the :math:`\pi_k`, and the + non-parametric part is due to the Gaussian distributions with mean and covariance :math:`\mu_k` and + :math:`\Sigma_k`. + + Returns: + True + """ + # TODO: hum... GMM are semi-parametric models... + return True + + @staticmethod + def is_linear(): + r""" + The parameters of the GMM are :math:`\theta = \{\pi_k, \mu_k, \Sigma_k\}_{k=1}^K`, where the GMM is linear + only with respect to the :math:`\pi_k`. + """ + # TODO: not clear + return True + + @staticmethod + def is_recurrent(): + """ + A GMM is not a recurrent model, that is, it is not a model that 'remembers' previous inputs. + """ + return False + + @staticmethod + def is_probabilistic(): + """ + A GMM is a probabilistic model. + """ + return True + + @staticmethod + def is_discriminative(): + r""" + The GMM is a generative model which encodes the joint distribution :math:`p(x,y)` between the input :math:`x` + and output :math:`y`. A discriminative model can be obtained by conditioning one of the variable by the other + one :math:`p(y|x)` or :math:`p(x|y)`. + """ + return False + + @staticmethod + def is_generative(): + r""" + The GMM is a generative model which encodes the joint distribution :math:`p(x,y)` between the input :math:`x` + and output :math:`y`. Because it is generative, we can sample from it. + """ + return True + + ########### + # Methods # + ########### + + def _check_initialized(self): + """Check if the GMM has been initialized""" + if self.priors is None: + raise ValueError("Priors have not been initialized") + if self.gaussians is None: + raise ValueError("The Gaussian distributions have not been initialized") + + def save(self, filename): + """ + Save the model in memory. + + Args: + filename (str): file to save the model in. + """ + pickle.dump(self, open(filename, 'wb')) + + @staticmethod + def load(filename): + """ + Load a model from memory. + + Args: + filename (str): file that contains the model. + """ + return pickle.load(filename) + + def parameters(self): + r""" + Return an iterator over the model parameters, which in GMM are the priors :math:`\pi_k`, the means + :math:`\mu_k` and the covariance matrices :math:`\Sigma_k`, :math:`\forall k \in {1,...,K}`. + """ + yield self.priors, self.means, self.covariances + + def named_parameters(self): + r""" + Return an iterator over the model parameters, yielding both the name and the parameter itself. In the case + of a GMM, the parameters are the priors :math:`\pi_k`, the means :math:`\mu_k` and the covariance matrices + :math:`\Sigma_k`, :math:`\forall k \in {1,...,K}`. + """ + yield "priors", self.priors + yield "means", self.means + yield "covariances", self.covariances + + def likelihood(self, x): + r""" + Compute the likelihood of the GMM. + + .. math:: p(X | \theta) = \prod_{n=1}^N \sum_{k=1}^K \pi_k \mathcal{N}(x_n | \mu_k, \Sigma_k) + + where :math:`X \in \mathbb{R}^{N \times D}` is the data matrix, and :math:`\theta = {\pi_k, \mu_k, \Sigma_k}` + are the parameters of the GMM. + + Args: + x (np.array[N,D]): data vector/matrix to evaluate the likelihood. + + Returns: + float: likelihood + """ + return np.exp(self.log_likelihood(x)) + + # alias + pdf = likelihood + + def log_likelihood(self, x): # score() in sklearn + r""" + Compute the log-likelihood of the GMM. + + .. math:: \log p(X | \theta) = \sum_{n=1}^N \log \sum_{k=1}^K \pi_k \mathcal{N}(x_n | \mu_k, \Sigma_k) + + where :math:`X \in \mathbb{R}^{N \times D}` is the data matrix, and :math:`\theta = {\pi_k, \mu_k, \Sigma_k}` + are the parameters of the GMM. + + Args: + x (np.array[N,D]): data vector/matrix to evaluate the likelihood. + + Returns: + float: log-likelihood + """ + gaussians = np.array([gaussian(x) for gaussian in self.gaussians]).T + return np.sum(self.priors * gaussians) + + # alias + log_pdf = log_likelihood + + def joint_pdf(self, x, z): + r""" + Compute the joint probability distribution :math:`p(X,Z)`. This is the same as the complete-data likelihood + of the GMM. + + .. math:: + + P(X,Z | \theta) &= \prod_{n=1}^N p(x_n, z_n | \theta) \\ + &= \prod_{n=1}^N p(z_n) p(x_n | z_n, \theta) \\ + &= \prod_{n=1}^N \prod_{k=1}^K \pi_k^{z_{nk}} \mathcal{N}(x_n | \mu_k, \Sigma_k)^{z_{nk}} + + where :math:`X \in \mathbb{R}^{N \times D}` is the data matrix, :math:`Z \in \mathbb{R}^{N \times K}` is the + associated hidden variable matrix where each entry is a binary value :math:`\{0,1\}` and each row sums up to 1, + and :math:`\theta = {\pi_k, \mu_k, \Sigma_k}` are the parameters of the GMM. + + Args: + x (np.array[D], np.array[N,D]): data vector/matrix + z (int, np.int[N], np.int[N,K]): hidden variable index / indices, or hidden variable matrix where each + row is a one hot encoding vector (i.e. all the elements of the row are 0 except one which has a value + of 1) + + Returns: + float: joint probability distribution (aka complete-data likelihood) + """ + # if hidden variable is an index + if isinstance(z, int): + joints = self.priors[z] * self.gaussians[z].pdf(x) # shape: 1 if data vector, or N if data matrix + return np.prod(joints) + + if isinstance(z, np.ndarray) and len(z.shape) <= 2: + # quick check about dimensions + Nz = 1 if len(z.shape) == 1 else z.shape[0] + Nx = 1 if len(x.shape) == 1 else x.shape[0] + if Nx != Nz: + raise ValueError("The number of samples between the data and the hidden variables should be " + "the same. Got instead {} and {} respectively".format(Nx, Nz)) + + # get hidden variable indices + z_idx = z.argmax(axis=1) if len(z.shape) == 2 else z + + # compute individual joint distribution + likelihoods = np.array([g.pdf(x) for g in self.gaussians]).T # shape: K if data vector, or NxK if matrix + priors = np.array([self.priors[z_id] for z_id in z_idx]) # shape: 1 if data vector, or N if matrix + joints = priors * likelihoods[range(Nx), z_idx] # shape: N + + # return product of joint distributions + return np.prod(joints) + else: + raise TypeError("The given z should be an integer representing the hidden variable index, or an " + "array of N integers representing the hidden variable indices, or a matrix of shape NxK " + "where each row is a one hot encoding vector") + + # alias + complete_data_likelihood = joint_pdf + + def log_joint_pdf(self, x, z): + r""" + Compute the log joint probability distribution :math:`\log p(X,Z)`. This is the same as the complete-data + log-likelihood of the GMM. + + This is given by: + + .. math:: + + \log p(X,Z|\theta) = \sum{n=1}^N \sum{k=1}^K z_{nk} (\log \pi_k + \log \mathcal{N}(x_n | \mu_k, \Sigma_k)) + + where :math:`X \in \mathbb{R}^{N \times D}` is the data matrix, :math:`Z \in \mathbb{R}^{N \times K}` is the + associated hidden variable matrix where each entry is a binary value :math:`\{0,1\}` and each row sums up to 1, + and :math:`\theta = {\pi_k, \mu_k, \Sigma_k}` are the parameters of the GMM. + + Args: + x (np.array): data vector/matrix to evaluate the complete-data log-likelihood. + z (int, np.int[N], np.int[N,K]): hidden variable index / indices, or hidden variable matrix where each + row is a one hot encoding vector (i.e. all the elements of the row are 0 except one which has a value + of 1) + + Returns: + float: log joint probability distribution (aka complete-data log-likelihood) + """ + return np.log(self.joint_pdf(x, z)) + + # alias + complete_data_log_likelihood = log_joint_pdf + + def posterior_pdf(self, x, z): + r""" + Evaluate the posterior distribution on the hidden variables :math:`Z`. This one also factorizes with respect + to the number of data points [1], and is given by: + + .. math:: p(Z | X, \theta) = \prod_{n=1}^N p(z_n | x_n, \theta) + + where :math:`\theta` are the parameters of the GMM. + + Note that compared to the `responsibilities` method, this returns the complete posterior (i.e. a float number). + + Args: + x (np.array): data vector/matrix + z (int, np.int[N], np.int[N,K]): hidden variable index / indices, or hidden variable matrix where each + row is a one hot encoding vector (i.e. all the elements of the row are 0 except one which has a value + of 1) + + Returns: + float: posterior + + References: + [1] "Pattern Recognition and Machine Learning" (eq. 9.75), Bishop, 2006 + """ + # if hidden variable is an index + if isinstance(z, int): + return np.prod(self.responsibilities(x, k=z)) + + if isinstance(z, np.ndarray) and len(z.shape) <= 2: + # quick check about dimensions + Nz = 1 if len(z.shape) == 1 else z.shape[0] + Nx = 1 if len(x.shape) == 1 else x.shape[0] + if Nx != Nz: + raise ValueError("The number of samples between the data and the hidden variables should be " + "the same. Got instead {} and {} respectively".format(Nx, Nz)) + + # get hidden variable indices + z_idx = z.argmax(axis=1) if len(z.shape) == 2 else z + + # compute individual posteriors + posteriors = self.responsibilities(x, z_idx, axis=0) + + # return product of posteriors + return np.prod(posteriors) + else: + raise TypeError("The given z should be an integer representing the hidden variable index, or an " + "array of N integers representing the hidden variable indices, or a matrix of shape NxK " + "where each row is a one hot encoding vector") + + def log_posterior_pdf(self, x, z): + r""" + Evaluate the log posterior distribution on the hidden variables :math:`Z`. This one also factorizes with + respect to the number of data points [1], and is given by: + + .. math:: \log p(Z | X, \theta) = \sum_{n=1}^N \log p(z_n | x_n, \theta) + + where :math:`\theta` are the parameters of the GMM. + + Note that compared to the `responsibilities` method, this returns the complete posterior (i.e. a float number). + + Args: + x (np.array): data vector/matrix + z (int, np.int[N], np.int[N,K]): hidden variable index / indices, or hidden variable matrix where each + row is a one hot encoding vector (i.e. all the elements of the row are 0 except one which has a value + of 1) + + Returns: + float: log posterior + + References: + [1] "Pattern Recognition and Machine Learning" (eq. 9.75), Bishop, 2006 + """ + return np.log(self.posterior_pdf(x, z)) + + def expected_complete_data_log_likelihood(self, x): + r""" + Expectation of the complete data log-likelihood under the posterior distribution of the latent variables. + This is the quantity that is being maximized during the M step of the EM algorithm. + + .. math:: + + Q(\theta, \theta_{old}) &= \sum_{Z} p(Z | X, \theta_{old}) \log p(X,Z | \theta) \\ + &= \sum_{n=1}^N \sum_{k=1}^K \r_k(x_n) (\log \pi_k + \log \mathcal{N}(x_n | \mu_k, \Sigma_k)) + + This is linked to the lower bound :math:`\mathcal{L}(q, \theta)` where when :math:`q(Z) = p(Z|X,\theta_{old})`, + we have: :math:`\mathcal{L}(q, \theta) = Q(\theta, \theta_{old}) + H(q)`. We can thus see that maximizing + the expectation of the complete data log-likelihood wrt the parameters :math:`\theta` is the same as + maximizing the lower bound wrt :math:`\theta` while holding :math:`q(Z)` fixed, which is performed during + the M step of the EM algorithm. [1] + + Note that the summation is no longer inside the logarithm as it was the case for :math:`\log p(X|\theta)`, + and closed-form solutions can be obtained that maximizes this loss :math:`Q(\theta, \theta_{old})` with + respect to the parameters :math:`\theta`. + + In the GMM case, this results in: + + .. math:: + + \mu_k &= \frac{1}{N_k} \sum_{n=1}^N r_k(x_n) x_n \\ + \Sigma_k &= \frac{1}{N_k} \sum_{n=1}^N r_k(x_n) (x_n - \mu_k)(x_n - \mu_k)^T \\ + \pi_k &= \frac{N_k}{N} + + where :math:`N_k = \sum_{n=1}^N r_k(x_n)`, and :math:`r_k(x_n) = p(z_k = 1|x_n)` are the responsibilities. + + Args: + x (np.array[N,D], np.array[D]): data vector/matrix + + References: + [1] "Pattern Recognition and Machine Learning" (chap 9.4), Bishop, 2006 + + Returns: + float: expected value of the complete data log-likelihood (under the posterior distribution of the latent + variables) + """ + # get useful variables + responsibilities = self.responsibilities(x) # shape: K if one data point, otherwise NxK + likelihoods = np.array([g.pdf(x) for g in self.gaussians]).T # shape: K if one data point, otherwise NxK + priors = self.priors # shape: K + + # compute each term of the expectation of the complete data log-likelihood under the posterior distribution + # of the latent variables. + q = responsibilities * (np.log(priors) + np.log(likelihoods)) # shape: K if data point, else NxK + + # sum over all latent variables and data points + return np.sum(q) + + def aic(self, x): + r""" + Return the Akaike Information Criterion (AIC) for the current model on the data x. The lower the better. + + .. math:: AIC = - 2 \log(\mathcal{L}(x, \theta)) + 2 n_p + + where :math:`\mathcal{L}(x, \theta)` is the likelihood of the model, :math:`n_p` is the number of free + parameters required for a GMM of :math:`K` components, i.e. :math:`n_p = (K-1) + K(D + 1/2 D(D+1))`, :math:`N` + is the number of data points, and :math:`D` is the dimensionality of the data. + + Args: + x (np.array): data vector/matrix. + + Returns: + float: AIC score + """ + return - 2 * self.log_likelihood(x) + 2 * self.num_parameters + + def bic(self, x): + r""" + Return the Bayesian Information Criterion (BIC) score which can be used to estimate the number of Gaussians. + The lower this number is, the better. + + .. math:: BIC = - 2 \log(\mathcal{L}(x, \theta)) + n_p \log(N) + + where :math:`\mathcal{L}(x, \theta)` is the likelihood of the model, :math:`n_p` is the number of free + parameters required for a GMM of :math:`K` components, i.e. :math:`n_p = (K-1) + K(D + 1/2 D(D+1))`, :math:`N` + is the number of data points, and :math:`D` is the dimensionality of the data. + + Args: + x (np.array): data vector/matrix. + + Returns: + float: BIC score + """ + return - 2 * self.log_likelihood(x) + self.num_parameters * np.log(self.num_data) + + def init_random(self, data, seed=None, reg=1e-8): + r""" + Initialize the GMM randomly. + + Args: + data (np.array): data matrix + seed (int, None): seed for random generator + reg (float): regularization term (useful to not have singular covariance matrices) + """ + # initialize random generator + np.random.seed(seed) + + # uniform priors + self._priors = np.ones(self.num_components) / self.num_components + + # compute mean and covariance of the data + mean = Gaussian.compute_mean(data, axis=0) + cov = Gaussian.compute_covariance(data, axis=0, bessels_correction=True) + + # generate means from a multivariate normal + means = np.random.multivariate_normal(mean, cov, size=self.num_components) + + # covariances (same as the above covariance + reg) + cov = cov + reg * np.identity(self.dim) + covariances = np.array([cov] * self.num_components) + + # create gaussians + self._gaussians = [Gaussian(mean=mu, covariance=cov) for mu, cov in zip(means, covariances)] + + def init_linear(self, data): + r""" + Initialize the GMM uniformly and linearly in the space. This initialization scheme is deterministic. + + Args: + data (np.array[N,D]): data matrix + """ + # compute lower and upper bounds + lower_bound, upper_bound = np.min(data, axis=0), np.max(data, axis=0) + + # distribute the means + means = [] + + # compute the covariances + covariances = [] + + # create gaussians + self._gaussians = [Gaussian(mean=mu, covariance=cov) for mu, cov in zip(means, covariances)] + + def init_uniformly(self, data, axis=0): + r""" + Initialize the GMM uniformly in the space with respect to the axis dimension. If the data represents + trajectories, the first dimension is the time and it will distributed uniformly with respect to that one. + + Args: + data (np.array[N,D]): data matrix + axis (int): axis specifying the dimension + """ + # compute lower and upper bounds + lower_bound, upper_bound = np.min(data, axis=0), np.max(data, axis=0) + + # distribute linearly with respect to the specified axis/dimension + distance = upper_bound[axis] - lower_bound[axis] + x = distance / (self.num_components + 1.) * np.arange(self.num_components) + + # compute centers for the other dimensions + idx = np.arange(len(x)) != axis + centers = (upper_bound[idx] - lower_bound[idx]) / 2. + + # compute means (combine the centers with the distribution over the specified axis) + means = [] + + # compute covariances + covariances = [] + + # create gaussians + self._gaussians = [Gaussian(mean=mu, covariance=cov) for mu, cov in zip(means, covariances)] + + def init_curvature(self, data): + r""" + Initialize the GMM using the curvature of the trajectories. This only works for sequential spatial data. + + Args: + data (np.array[T,D], np.array[N,T,D]): data matrix or vector + """ + pass + + def init_sklearn(self, data): + r""" + Initialize the GMM using the sklearn library. + + Args: + data (np.array[T,D], np.array[N,T,D]): data matrix or vector + """ + pass + + def init_time_warping(self, data): + r""" + Initialize the GMM using Dynamic Time Wrapping [1]. This only works for sequential temporal and spatial data. + + Args: + data: + + References: + [1] "Robot Programming by Demonstration: A Probabilistic Approach", Calinon, 2009, Chap 2.9.3 + """ + pass + + def init_kmeans(self, data, seed=None): + r""" + Initialize the GMM using K-means algorithm. + + Args: + data (np.array[N,D]): data matrix + seed (int, None): seed for random generator + """ + # initialize random generator + np.random.seed(seed) + + # fit the data using k-means + km = KMeans(n_clusters=self.num_components) + km.fit(data) + + # uniform priors + self._priors = np.ones(self.num_components) / self.num_components + + # identity covariances + covariances = np.array([np.identity(self.dim)] * self.num_components) + + # means = position of the cluster centers + means = km.cluster_centers_ + + # create gaussians + self._gaussians = [Gaussian(mean=mu, covariance=cov) for mu, cov in zip(means, covariances)] + + def init(self, data, method='k-means', seed=None, reg=1e-8): + r""" + Initialize the GMM using the specified method + + Args: + data (np.array[N,D]): data matrix + method (str, None): 'k-means', 'random', None. If None, it starts from where the Gaussians are placed. + seed (str): seed for random generator + reg (float): regularization term (useful to not have singular covariance matrices) + """ + if method is None: + return + method = method.lower() + if method == 'random': + self.init_random(data, seed, reg=reg) + elif method == 'k-means' or method == 'kmeans': + self.init_kmeans(data, seed) + else: + raise NotImplementedError("The given initialization method has not been implemented") + + def expectation_maximization(self, X, reg=1e-8, num_iters=1000, threshold=1e-4, + init='kmeans', seed=None, verbose=False): + r""" + Fit the GMM to the provided data by performing the expectation maximization algorithm. The EM algorithm allows + to find the maximum likelihood estimate for models that have latent variables. This one consists of 4 main + steps: + 1. Initialize the parameters :math:`\theta = \{\pi_k, \mu_k, \Sigma_k\}_{k=1}^K` + 2. Expectation step: evaluate the posterior :math:`p(Z | X, \theta_{old})` while fixing the parameters. + 3. Maximization step: maximize the expected value of the complete-data log-likelihood under the posterior + distribution of the latent variables (found during the Expectation step). That is, + :math:`\max_\theta Q(\theta, \theta_{old}) = \max_\theta \sum_{Z} p(Z | X, \theta_{old}) \log p(X,Z | \theta)`. + 4. Evaluate the log-likelihood loss, and check if it converged. If it didn't, go back to step 2. + + To be more specific, the EM algorithm alternatively computes a lower bound on the log-likelihood for the + current parameters, and then maximize this bound to obtain the new parameter values (see [1], sec 9.4 for + more details). Note that the lower bound and the expected value of the complete-data log-likelihood under + the posterior distribution of the latent variables are related. + + In the GMM case, the E-step consists to compute the responsibilities: :math:`r_k(x_n)`, and the M-step + consists to compute the following equations (which are the closed-form solutions to the maximization of + the lower bound): + + .. math:: + + \mu_k &= \frac{1}{N_k} \sum_{n=1}^N r_k(x_n) x_n \\ + \Sigma_k &= \frac{1}{N_k} \sum_{n=1}^N r_k(x_n) (x_n - \mu_k)(x_n - \mu_k)^T \\ + \pi_k &= \frac{N_k}{N} + + where :math:`N_k = \sum_{n=1}^N r_k(x_n)`, and :math:`r_k(x_n) = p(z_k = 1|x_n)` are the responsibilities. + + Args: + X (np.array[N,D]): data matrix + reg (float): regularization term + num_iters (int): number of iterations + threshold (float): convergence threshold + init (str, None): how the Gaussians should be initialized. Possible values are 'random', 'kmeans', and + None. If None, it will use the initial positions of the provided gaussians. + seed (int, None): seed for random generator + verbose (bool): if we should print details during the optimization process + + Returns: + dict: dictionary containing info collected during the optimization process, such as the history of losses, + the number of iterations it took to converge, if it succeeded, etc. + """ + # quick check + if len(X.shape) != 2: + raise ValueError("Expecting a 2D array of shape NxD for the data") + + # compute dictionary results + results = {'losses': [], 'success': False, 'num_iters': 0} + self.N = X.shape[0] + + # 1. Initialize + self.init(X, method=init, seed=seed, reg=reg) + if init is None: + np.random.seed(seed) + + # compute initial loss + loss = self.log_likelihood(X) + prev_loss = loss + results['losses'].append(loss) + + for it in range(num_iters): + # 2. E-step + r_kn = self.responsibilities(X) # shape: NxK + + # 3. M-step + N_k = np.sum(r_kn, axis=0) # shape: K + mu_k = (1./N_k * X.T.dot(r_kn)).T # shape: KxD + cov_k = np.array([(r_kn[:, k] * (X - mu_k[k]).T).dot((X - mu_k[k])) for k in range(self.K)]) # KxDxD + cov_k = (1./N_k * cov_k.T).T # KxDxD + self._priors = N_k / self.N # shape: K + self._gaussians = [Gaussian(mean=mu, covariance=cov) for mu, cov in zip(mu_k, cov_k)] + + # 4. check convergence + loss = self.log_likelihood(X) + results['losses'].append(loss) + if np.abs(loss - prev_loss) <= threshold: + if verbose: + print("Convergence achieved at iteration {} with associated loss: {}".format(it+1, loss)) + results['num_iters'] = it+1 + results['success'] = True + return results + + # update previous loss + prev_loss = loss + + return results + + # aliases + em = expectation_maximization + fit = expectation_maximization + mle = expectation_maximization + maximum_likelihood = expectation_maximization + + def predict(self, x): + r""" + Predict from which component the data is from, and return the index of this component/Gaussian. + + Args: + x (np.array): data vector/matrix + + Returns: + int, np.array: component index/indices + """ + posteriors = self.responsibilities(x) # shape NxK if data matrix, or K if data vector + if len(posteriors.shape) == 2: + idx = np.argmax(posteriors, axis=1) + else: + idx = np.argmax(posteriors) + return idx + + def predict_prob(self, x): + r""" + Predict from which component the data is from with the associated probability. + + Args: + x (np.array): data vector/matrix + + Returns: + int, np.int[N]: component index/indices + float, np.float[N]: associated probability + """ + posteriors = self.responsibilities(x) # shape NxK if data matrix, or K if data vector + if len(posteriors.shape) == 2: + idx = np.argmax(posteriors, axis=1) + else: + idx = np.argmax(posteriors) + return idx, posteriors[idx] + + def cumulative_prior(self): + r""" + Return the cumulative distribution function on the prior :math:`\sum_{k=1}^{m} \pi_k \forall m \in {1, ..., K}`. + Note that the order in the sum is important here. + + Returns: + np.float[K]: cumulative distribution function on the prior + """ + return np.cumsum(self.priors) + + def sample_hidden(self, size=None, seed=None): + r""" + Sample from hidden random variable :math:`Z` (i.e. from the priors). + + Args: + size (int, None): number of samples + seed (int, None): seed for the random number generator + + Returns: + int, np.int[N]: component indices + """ + np.random.seed(seed) + + # sample from uniform distribution + if size is None: size = 1 + random = np.random.rand(size) + + # compute cumulative distribution function on priors + cumsum = self.cumulative_prior() + prior_idx = list(range(len(cumsum))) + + # sample the components + idx = np.array([prior_idx[cumsum < rand_number][-1] for rand_number in random]) + + # if one sample, just return this one + if idx.size == 1: + return idx[0] + + # otherwise, return all of them + return idx + + def sample(self, size=None, seed=None, kind=None): + r""" + Generate `size` samples from the GMM. This uses the ancestral/forward sampling method; that is, it first + samples from the hidden variables :math:`\hat{z} \sim p(z)`, and then from the conditional distribution + :math:`\hat{x} \sim p(x|\hat{z})`. + + Args: + size (int, None): number of samples + seed (int, None): seed for the random number generator + kind (str): if kind == 'complete', it returns the data along with from which component (i.e. Gaussian) + it was sampled from. Otherwise, it just returns the data. + + Return: + np.array[D], np.array[N,D]: samples + int, np.int[N]: component index/indices (if the argument kind == 'complete') + """ + idx = self.sample_hidden(size=size, seed=seed) + if isinstance(idx, int): # just one + return self.gaussians[idx].sample() # shape: D + return np.array([self.gaussians[i].sample() for i in idx]) # shape: NxD + + def responsibilities(self, x, k=None, axis=1): + r""" + Compute the responsibilities (posterior probability of component k once we have observed the data `x`). + These are given by: + + .. math:: + + r_k(x) &= p(z_k=1 | x) \\ + &= \frac{p(x, z_k=1)}{p(x)} \\ + &= \frac{p(z_k=1) p(x | z_k=1)}{\sum_{j=1}^{K} \p(z_j=1) p(x | z_j=1)} + &= \frac{\pi_k \mathcal{N}(x|\mu_k,\Sigma_k)}{\sum_{j=1}^{K} \pi_j \mathcal{N}(x|\mu_j,\Sigma_j)} + + Args: + x (np.array): data vector/matrix + k (np.array, int, slice, None): component index(ices). If None, compute the responsibilities wrt to each + component. + axis (int): This argument is useful when the argument 'k' is an array; k can then be an array of size `N` + or between 1 and `K`. If `N` is bigger than `K` then we can infer what the user wants, however if `N` + is smaller than `K`, then we have to specify what the nature of `k` is; Does the user wants back a + `Nxk` matrix or `Nx1` vector? That is, the indices in k are the ones that we are interested to get + back, or they are indices for each sample? Because the number of data points is often bigger than `K`, + this argument is not used, but in the case `N` is smaller than `K`, then by default axis == 1, which + means that it will return a `Nxk` matrix. If axis == 0, it will return an array of size `N` which + contains the responsibilities :math:`h_{k_n}(x_n)`. + + Returns: + float, np.array: responsibility(ies). It will be a float number if only one datapoint and one component + index were provided. An array of size `k` if one data point and `k` component indices were provided. + An array of size `N` if multiple data points and one component index or `N` component indices were + given. A matrix of shape `Nxk` if multiple data points and `k` component indices were given. + """ + gaussian_pdfs = np.array([g.pdf(x) for g in self.gaussians]).T # shape: K if 1 data point, or NxK if multiple + joint = self.priors * gaussian_pdfs # shape: K if 1 data point, or NxK if multiple + marginal = np.sum(joint, axis=1) # shape: 1 if 1 data point, or N if multiple + + if k is None: + return (joint.T / marginal).T # shape: K if 1 data point, or NxK if multiple + else: + N = 1 if len(x.shape) == 1 else x.shape[0] + if N == 1: # one data point + return joint[k] / marginal # shape: k + else: # multiple data points + K = self.num_components + if N == len(k): + if N <= K and axis == 1: + return (joint[:, k].T / marginal).T # shape: Nxk + return joint[range(N), k] / marginal # shape: N + return (joint[:,k].T / marginal).T # shape: Nxk + + def condition(self, x_in, idx_out, idx_in=None): + r""" + Condition the GMM which results in GMR. Return the conditioned GMM. If the user wants to approximate it + by a single gaussian, he/she can call the property `gaussian` or the `approximate_by_single_gaussian()` + methods. These two's are equivalent. + + Gaussian Mixture Regression [1,2] consists to condition the GMM (that models the joint distribution over the + input and output variables :math:`p(x^I, x^O)`) on a part of the variables (for instance, the input variables + :math:`p(x^O | x^I`). Let's :math:`x = [x^I, x^O]`, :math:`\mu_k = [\mu_k^I, \mu_k^O]`, and :math:`\Sigma_k = + \left[ \begin{array}{cc} \Sigma_k^I & \Sigma_k^{IO} \\ \Sigma_k^{OI} & \Sigma_k^O \end{array} \right]`, where + :math:`I` and :math:`O` are the superscripts to refer the input and output respectively. + + .. math:: + + p(x^O | x^I) &= \sum_{k=1}^K p(z_k=1 | x^I) p(x^O | x^I, z_k=1) \\ + &= \sum_{k=1}^K r_k(x^I) \mathcal{N}(\hat{\mu}_k^O(x^I), \hat{\Sigma}_k^O) + + where :math:`r_k(x^I) = \frac{\pi_k \mathcal{N}(x^I|\mu_k^I, \Sigma_k^I)}{\sum_{j=1}^{K} \pi_j + \mathcal{N}(x^I|\mu_j^I,\Sigma_j^I)}` are the responsibilities, :math:`\hat{\mu}_k^O(x^I) = + \mu_k^O + \Sigma_k^{OI} \Sigma_k^I^{-1} (x^I - \mu_k^I)` and :math:`\hat{\Sigma}_k^O = \Sigma_k^O - + \Sigma_k^{OI} (\Sigma_k^I)^{-1} \Sigma_k^{IO}` are the resulting conditioned means and covariances, + respectively. + + Args: + x_in (float[d2]): array of values :math:`x^I` such that we have :math:`p(x^O|x^I)` + idx_out (int[d1]): indices that we are interested in (indices of :math:`x^O` in :math:`x`) given + (i.e. conditioned on) the other ones + idx_in (int[d2]): indices that we conditioned on corresponding to the values. If None, it will be inferred. + + Returns: + GMM: conditioned gaussian mixture model + + References: + [1] "Robot Programming by Demonstration: a Probabilistic Approach" (chap 2), Calinon, 2009 + [2] "A Tutorial on Task-Parameterized Movement Learning and Retrieval", Calinon, 2015 + """ + priors = self.responsibilities(x_in) + gaussians = [g.condition(x_in, idx_out, idx_in) for g in self.gaussians] + return GMM(priors=priors, gaussians=gaussians) + + def marginalize(self, idx): + r""" + Compute and return the marginal distribution (which is also a GMM) of the specified indices. + + Let's assume that the joint distribution :math:`p(x_1, x_2)` is modeled as a GMM, that is: + + .. math:: x \sim \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k) + + where :math:`x = [x_1, x_2]`, :math:`\mu = [\mu_1^{(k)}, \mu_2^{(k)}]` and + :math:`\Sigma=\left[\begin{array}{cc} \Sigma_{11}^{(k)} & \Sigma_{12}^{(k)} \\ \Sigma_{21}^{(k)} & + \Sigma_{22}^{(k)} \end{array}\right]` + + then the marginal distribution :math:`p(x_1) = \int_{x_2} p(x_1, x_2) dx_2` is also a GMM and is given by: + + .. math:: p(x_1) = \sum_{k=1}^K \pi_k \mathcal{N}(\mu_1^{(k)}, \Sigma_{11}^{(k)}) + + Args: + idx (int, slice): indices of :math:`x_1` (this value should be between 0 and D-1, where D is + the dimensionality of the data) + + Returns: + GMM: marginal distribution (which is also a GMM) + """ + gaussians = [gaussian.marginalize(idx) for gaussian in self.gaussians] + return GMM(priors=self.priors, gaussians=gaussians) + + def multiply(self, other): + r""" + Multiply a GMM by another Gaussian or GMM, by a square matrix (under an affine transformation), or a float + number. + + 1. The product of a GMM by a Gaussian is given by: + + .. math:: + + \mathcal{N}(\mu, \Sigma) \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k) + &= \sum_{k=1}^K \pi_k \mathcal{N}(\mu, \Sigma) \mathcal{N}(\mu_k, \Sigma_k) \\ + &= \sum_{k=1}^K \pi_k c_k \mathcal{N}(\hat{\mu}_k, \hat{\Sigma}_k) + + where :math:`c_k = \mathcal{N}(\mu; \mu_k, \Sigma + \Sigma_k)` are constant (scalar) coefficients, + :math:`\hat{\Sigma}_k = (\Sigma^{-1} + \Sigma_k^{-1})^-1`, and + :math:`\hat{\mu}_k = \hat{\Sigma}_k (\Sigma^{-1} \mu + \Sigma_k^{-1} \mu_k)`. In order for this result to + be a proper probability distribution, we have to normalize it, which gives: + + .. math:: p(x) = \sum_{k=1}^K \hat{\pi}_k \mathcal{N}(\hat{\mu}_k, \hat{\Sigma}_k) + + where :math:`\hat{\pi}_k = \frac{c_k \pi_k}{\sum_{j=1}^K c_j \pi_j}`. + + + 2. Similarly, the product of two GMMs is given by: + + .. math:: + + \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k) \sum_{j=1}^J \pi_j \mathcal{N}(\mu_j, \Sigma_j) + &= \sum_{k=1}^K \sum_{j=1}^J \pi_k \pi_j \mathcal{N}(\mu_k, \Sigma_k) \mathcal{N}(\mu_j, \Sigma_j) \\ + &= \sum_{k=1}^K \sum_{j=1}^J c_{kj} \pi_k \pi_j \mathcal{N}(\mu_{kj}, \Sigma_{kj}) + + where where :math:`c_{kj} = \mathcal{N}(\mu_k; \mu_j, \Sigma_k + \Sigma_j)` are constants (scalars), + :math:`\Sigma_{kj} = (\Sigma_k^{-1} + \Sigma_j^{-1})^-1`, and + :math:`\mu_{kj} = \Sigma_{kj} (\Sigma_k^{-1} \mu_k + \Sigma_j^{-1} \mu_j)`. In order for this result to + be a proper probability distribution, we have to normalize it, which gives: + + .. math:: p(x) = \sum_{k=1}^K \sum_{j=1}^J \pi_{kj} \mathcal{N}(\mu_{kj}, \Sigma_{kj}) + + where :math:`\pi_{kj} = \frac{c_{kj} \pi_k \pi_j}{\sum_{m=1}^K \sum_{n=1}^J c_{mn} \pi_m \pi_n}`. + + Note that the product of two GMMs increase the number of components which is equal to :math:`K*J`. If + the user wants to multiply two GMMs element-wise, please see the `multiply_element_wise` method. + + + 3. The product of a GMM with a square matrix :math:`A` gives: + + .. math:: Ax \sim \sum_{k=1}^K \pi_k \mathcal{N}(A \mu_k, A \Sigma_k A^T) + + + 4. The product of a GMM by a float does nothing as we have to re-normalize it to be a proper distribution. + + Args: + other (Gaussian, GMM, np.float[D,D], float): Gaussian, GMM, square matrix (to rotate or scale), or float + + Returns: + GMM: resulting GMM + """ + # if other == Gaussian + if isinstance(other, Gaussian): + coefficients = np.array([g.get_multiplication_coefficient(other) for g in self.gaussians]) # shape: K + normalization = np.sum(self.priors * coefficients) + priors = self.priors * coefficients / normalization + gaussians = [g * other for g in self.gaussians] + return GMM(priors=priors, gaussians=gaussians) + + # if other == GMM + elif isinstance(other, GMM): + coefficients, priors, gaussians = [], [], [] + for prior1, gaussian1 in zip(self.priors, self.gaussians): + for prior2, gaussian2 in zip(other.priors, other.gaussians): + prior = prior1 * prior2 + coeff = gaussian1.get_multiplication_coefficient(gaussian2) + gaussian = gaussian1 * gaussian2 + + priors.append(prior) + coefficients.append(coeff) + gaussians.append(gaussian) + + priors, coefficients = np.array(priors), np.array(coefficients) + normalization = np.sum(priors * coefficients) + priors = priors * coefficients / normalization + return GMM(priors=priors, gaussians=gaussians) + + # if other == square matrix + elif isinstance(other, np.ndarray): + return self.affine_transform(other) + + # if other == number + elif isinstance(other, (int, float)): + return self + + else: + raise TypeError("Trying to multiply a Gaussian with {}, which has not be defined".format(type(other))) + + def multiply_element_wise(self, other): + r""" + Multiply element wise two GMMs. If a Gaussian, square matrix, or float is given, it will just call + the `multiply` method. + + Compared to the `multiply` method, the element-wise multiplication between two GMMs does not increase the + number of components. + + Args: + other (Gaussian, GMM, np.float[D,D], float): Gaussian, GMM, square matrix (to rotate or scale), or float + + Returns: + GMM: resulting GMM + """ + if isinstance(other, GMM): + coefficients, priors, gaussians = [], [], [] + for (prior1, gaussian1), (prior2, gaussian2) in zip(self, other): + prior = prior1 * prior2 + coeff = gaussian1.get_multiplication_coefficient(gaussian2) + gaussian = gaussian1 * gaussian2 + + priors.append(prior) + coefficients.append(coeff) + gaussians.append(gaussian) + + priors, coefficients = np.array(priors), np.array(coefficients) + normalization = np.sum(priors * coefficients) + priors = priors * coefficients / normalization + return GMM(priors=priors, gaussians=gaussians) + else: + return self.multiply(other) + + def add(self, other): + r""" + Add a GMM with a Gaussian, or a GMM with a vector (affine transformation). + + 1. The sum of a GMM with a Gaussian (which is independent) results in: + + .. math:: + + \mathcal{N}(\mu, \Sigma) + \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k) + = \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k + \mu, \Sigma_k + \Sigma) + + The sum of two independent Gaussian RVs (with the same dimensionality), such that + :math:`x_1 \sim \mathcal{N}(\mu_1, \Sigma_1)` and :math:`x_2 \sim \mathcal{N}(\mu_2, \Sigma_2)`, is given + by :math:`x_1 + x_2 \sim \mathcal{N}(\mu_1 + \mu_2, \Sigma_1 + \Sigma_2)` + + 2. The sum of a GMM :math:`x \sim \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k)` with a vector :math:`v` + results in a translation of this distribution, given by + :math:`x \sim \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k + v, \Sigma_k)`. + + Args: + other (Gaussian, float[d]): the other Gaussian distribution, or a vector. + + Returns: + GMM: resulting GMM + """ + if isinstance(other, Gaussian): + gaussians = [Gaussian(g.mean + other.mean, g.cov + other.cov) for g in self.gaussians] + return GMM(priors=self.priors, gaussians=gaussians) + elif isinstance(other, np.ndarray): + gaussians = [Gaussian(g.mean + other) for g in self.gaussians] + return GMM(priors=self.priors, gaussians=gaussians) + else: + raise NotImplementedError("Addition not defined for the given type {}".format(type(other))) + + def affine_transform(self, A, b=None): + r""" + Perform an affine transformation on the GMM. For a GMM, we have + :math:`x \sim \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k)`, then + :math:`Ax+b \sim \sum_{k=1}^K \pi_k \mathcal{N}(A \mu_k + b, A \Sigma_k A^T)`. + + Args: + A (np.ndarray[D,D]): square matrix + b (np.ndarray[D]): vector + + Returns: + GMM: resulting GMM + """ + gaussians = [g.affine_transform(A, b) for g in self.gaussians] + return GMM(priors=self.priors, gaussians=gaussians) + + def integrate(self, lower=None, upper=None): + r""" + Integrate the GMM between the two given bounds. + + .. math:: + + \int p(x) dx &= \int_{x_0}^{x_f} \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k) dx \\ + &= \sum_{k=1}^K \pi_k \int_{x_0}^{x_f} \mathcal{N}(\mu_k, \Sigma_k) dx \\ + &= \sum_{k=1}^K \pi_k \p_k(x_0 <= x <= x_f) + + Args: + lower (np.array[D], float, None): lower bound (default: -np.inf) + upper (np.array[D], float, None): upper bound (default: np.inf) + + Returns: + float: p(lower <= x <= upper) + """ + probs = np.array([g.integrate(lower, upper) for g in self.gaussians]) + return np.sum(self.priors * probs) + + def grad(self, x, k=None, wrt='x'): + r""" + Compute the gradient of the GMM evaluated at the given data 'x' with respect to the specified variable and + component 'k'. Let's :math:`p(x; {\pi_k, \mu_k, \Sigma_k}_{k=1}^K) = \sum_{k=1}^K \pi_k \mathcal{N}(x | \mu_k, + \Sigma_k)` be the GMM. Then (using [1]), we have: + + .. math:: + + \frac{\partial p(x)}{\partial x} &= - \sum_{k=1}^K \pi_k \mathcal{N}_k \Lambda_k (x - \mu_k) \\ + \frac{\partial p(x)}{\partial \pi_j} &= \mathcal{N}_j \\ + \frac{\partial p(x)}{\partial \mu_j} &= \pi_j \mathcal{N}_j \Lambda_j (x - \mu_j) \\ + \frac{\partial p(x)}{\partial \Sigma_j} &= \frac{\pi_j}{2} \mathcal{N}_j (\Lambda_j (x-\mu_j)(x-\mu_j)^T + \Lambda_j - \Lambda_j) \\ + \frac{\partial p(x)}{\partial \Lambda_j} &= \frac{\pi_j}{2} \mathcal{N}_j (\Sigma_j - (x-\mu_j)(x-\mu_j)^T) + + where :math:`\Lambda = \Sigma^{-1}` is the precision matrix, and + :math:`\mathcal{N}_j = \mathcal{N}(\mu_j, \Sigma_j)` is a multivariate Gaussian distribution. + + Args: + x (np.array[D]): data vector + k (int, None): index of the component. If None, it will return the gradient for each component if 'wrt' + is different from 'x'. + wrt (str): specify with respect to which variable we compute the gradient. It can take the following + values 'x', 'pi' or 'prior', 'mu' or 'mean', 'sigma' or 'covariance', 'lambda' or 'precision'. + + Returns: + np.array: gradient of the same shape (as the variable from which we take the gradient) + + References: + [1] "The Matrix Cookbook" (math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf), Petersen and Pedersen, 2012 + """ + # TODO: check when x is a matrix + + # if the derivative wrt a component is specified + if k is not None: + if wrt == 'pi' or wrt == 'prior': + return self.gaussians[k].pdf(x) + return self.gaussians[k].grad(x, wrt=wrt) + + wrt = wrt.lower() + if wrt == 'x': + return np.sum([prior * g.grad(x, wrt=wrt) for prior, g in zip(self.priors, self.gaussians)], axis=0) + elif wrt == 'pi' or wrt == 'prior': + return np.array([gaussian.pdf(x) for gaussian in self.gaussians]) + elif wrt == 'mu' or wrt == 'mean' or wrt == 'sigma' or wrt[:3] == 'cov' \ + or wrt == 'lambda' or wrt == 'precision': + return np.array([prior * g.grad(x, wrt=wrt) for prior, g in zip(self.priors, self.gaussians)]) + else: + raise ValueError("The given 'wrt' argument is not valid (see documentation)") + + def grad_log_likelihood(self, x): + pass + + def hessian(self, x, wrt='x'): + pass + + def update(self, x): + r""" + Online update of the GMM given new data points. + + Args: + x (np.array): data vector/matrix + """ + pass + + def approximate_by_single_gaussian(self): + r""" + Approximate the GMM by a single Gaussian. This is the same as the `gaussian` property. Let's the GMM be + defined as :math:`p(x) = \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k)`. + + The mean is then given by :math:`\mu = \mathbb{E}_{x \sim p(x)}[x]`: + + .. math:: + + \mathbb{E}_{x \sim p(x)}[x] &= \int x p(x) dx \\ + &= \int x \sum_{k=1}^K \pi_k \mathcal{N}(\mu_k, \Sigma_k) dx \\ + &= \sum_{k=1}^K \pi_k \int x \mathcal{N}(\mu_k, \Sigma_k) dx \\ + &= \sum_{k=1}^K \pi_k \mu_k + + The covariance is given by :math:`\Sigma = \mathbb{E}_{x \sim p(x)}[xx^T] - \mathbb{E}_{x \sim p(x)}[x] + \mathbb{E}_{x \sim p(x)}[x]^T`. Let's focus on :math:`\mathbb{E}_{x \sim p(x)}[xx^T]`. + + .. math:: + + \mathbb{E}_{x \sim p(x)}[xx^T] &= \int xx^T p(x) dx \\ + &= \int xx^T \sum_{k=1}^K \pi_k \mathcal{N}(x|\mu_k, \Sigma_k) dx \\ + &= \sum_{k=1}^K \pi_k \int xx^T \mathcal{N}(x|\mu_k, \Sigma_k) dx \\ + &= \sum_{k=1}^K \pi_k \mathbb{E}_{x \sim \mathcal{N}(x|\mu_k, \Sigma_k)}[xx^T] + &= \sum_{k=1}^K \pi_k (\Sigma_k + \mu_k \mu_k^T) + + Thus, the covariance is given by: :math:`\Sigma(x) = \sum_{k=1}^K \pi_k (\Sigma_k + \mu_k \mu_k^T) - \mu \mu^T` + + Returns: + Gaussian: gaussian distribution + """ + return Gaussian(mean=self.mean, covariance=self.covariance) + + # alias + moment_matching = approximate_by_single_gaussian + + def entropy(self): + r""" + Differential entropy associated with the GMM distribution. + + .. math:: + + H(x) &= - \int p(x) \ln p(x) dx \\ + &= - \sum_{k=1}^K \pi_k \int \mathcal{N}_k(x) \ln ( \sum_{k=1}^K \pi_k \mathcal{N}_k(x) ) dx + + where :math:`\mathcal{N}_k(x) = \mathcal{N}(x | \mu_k, \Sigma_k)`. + + Returns: + float: differential entropy + """ + pass + + def kl_divergence(self, other): + r""" + Compute the Kullback-Leibler divergence between the two GMMs. Note that the KL divergence between two GMMs is + not analytically tractable, and that the KL divergence is not symmetric. + + Warnings: This only valid if the other distribution is also a GMM. + + .. math:: + + D_{KL}(p_1 || p_2) = - \int p_1 \ln \left( \frac{p_2}{p_1} \right) dx + + Args: + other (GMM): the other gaussian mixture model. + + Returns: + float: the divergence between the 2 GMMs. + """ + pass + + ############# + # Operators # + ############# + + def __str__(self): + """Return name of this class""" + return self.__class__.__name__ + + def __call__(self, x=None, z=None, size=None): + r""" + If no arguments are provided, it returns one sample from the distribution. If the data vector or matrix is + provided, it returns the associated probability for each sample (i.e. the likelihood that the given sample(s) + was/were generated from this GMM), that is :math:`p(x)`. If the index `k` is also provided in addition to the + data :math:`x`, it returns the joint probability :math:`p(x, z_k=1)`. + + Args: + x (np.float[N,D], np.float[D]): data matrix/vector + z (int, np.int[N], None): hidden variable (component index/indices) + size (int, None): number of samples + + Returns: + float, or np.array: probability evaluated at `x`, or samples + """ + if x is not None: + return self.joint_pdf(x, z) + return self.sample(size=size) + + def __len__(self): + """ + Return the number of components. + + Returns: + int: number of components + """ + return len(self.priors) + + def __iter__(self): + """ + Iterate over each component of the GMM. + + Returns: + float: prior probability + Gaussian: gaussian distribution + """ + for prior, gaussian in zip(self.priors, self.gaussians): + yield prior, gaussian + + def __getitem__(self, idx): + r""" + Return the prior probability :math:`\pi_k` and the corresponding Gaussian `N(\mu_k, \Sigma_k)` associated + to the given index. + + Args: + idx (int): index + + Returns: + float: prior probability + Gaussian: gaussian distribution + """ + return self.priors[idx], self.gaussians[idx] + + def __add__(self, other): + """ + Add a GMM with a Gaussian, or a GMM with a vector (affine transformation). + + Args: + other (Gaussian, float[d]): the other Gaussian, or vector. + + Returns: + GMM: resulting GMM + """ + return self.add(other) + + def __radd__(self, other): + return self.add(other) + + def __mul__(self, other): + r""" + Multiply two GMMs, or a GMM by a Gaussian, matrix, or float. See the `multiply` method for more information. + + Warnings: the multiplication of two GMMs performed here is NOT the one that multiply the components + element-wise. For this one, have a look at `multiply_element_wise` method, or the `__and__` operator. + + Args: + other (GMM, Gaussian, np.float[D,D], float): GMM, Gaussian, or square matrix (to rotate or scale), or float + + Returns: + GMM: resulting GMM + """ + return self.multiply(other) + + def __rmul__(self, other): + return self.multiply(other) + + def __and__(self, other): + r""" + Multiply two GMMs, or a GMM by a Gaussian, matrix, or float. See the `multiply_element_wise` method for more + information. Note that the multiplication between two GMMs performed here will multiply the components + element-wise. + + Args: + other (GMM, Gaussian, np.float[D,D], float): GMM, Gaussian, or square matrix (to rotate or scale), or float + + Returns: + GMM: resulting GMM + """ + return self.multiply_element_wise(other) + + def __rand__(self, other): + return self.multiply_element_wise(other) + + +class VBGMM(GMM): + r"""Variational Bayesian Gaussian Mixture Model + + "Variational inference is an extension of expectation-maximization that maximizes a lower bound on model evidence + (including priors) instead of data likelihood. The principle behind variational methods is the same as + EM (that is both are iterative algorithms that alternate between finding the probabilities for each point to be + generated by each mixture and fitting the mixture to these assigned points), but variational methods add + regularization by integrating information from prior distributions. This avoids the singularities often found in + EM solutions but introduces some subtle biases to the model. Inference is often notably slower, but not usually + as much so as to render usage unpractical." from [1] + + References: + [1] sklearn + """ + + def __init__(self, num_components): + super(VBGMM, self).__init__(num_components) + + +class TPGMM(GMM): + r"""Task-Parametrized Gaussian Mixtured Model + + References: + [1] "A Tutorial on Task-Parameterized Movement Learning and Retrieval", Calinon, 2015 + """ + + def __init__(self, num_frames, num_components): + super(TPGMM, self).__init__(num_components) + self.num_systems = num_frames + + +###################### +# Plotting functions # +###################### + +def plotGMM(gmm, ax=None, title='GMM', color='b'): + r"""Plot GMM""" + if ax is None: + fig, ax = plt.subplots(1, 1) + ax.set(title=title, xlim=[-2, 2], ylim=[-2, 2], aspect='equal') + for g in gmm.gaussians: + plot2DEllipse(ax, g, color=color, plot_arrows=False) + + +# TESTS +if __name__ == "__main__": + from gaussian import plot2DEllipse + import matplotlib.pyplot as plt + + # create manually a GMM + dim, num_components = 2, 3 + gmm = GMM(gaussians=[Gaussian(mean=np.random.uniform(-1., 1., size=dim), + covariance=0.1*np.identity(dim)) for _ in range(num_components)]) + gmm1 = GaussianMixture(n_components=num_components) + + # plot initial GMM + plotGMM(gmm, title='Initial GMM') + plt.show() + + # create data + N, eps = 8, 0.1 + t = np.linspace(0., 1., 100) + y = np.array([np.sin(2 * np.pi * t) + eps * np.random.rand(len(t)) for _ in range(N)]) # shape: NxT + + # plot data + plt.plot(t, y.T) + plt.title('Training data') + plt.show() + + # combine data (with shape: N'xD, where N' is the N'=N*T) + X = np.hstack((np.array([t] * N).reshape(-1, 1), y.reshape(-1, 1))) + + # init GMM + gmm.init(X, method='random') # method='k-means') + plotGMM(gmm, title='GMM after K-Means') + plt.show() + + # fit a GMM using EM + result = gmm.fit(X, init=None) + + # plot losses + plt.plot(result['losses']) + plt.show() + + # plot trained GMM + fig, ax = plt.subplots(1, 1) + plotGMM(gmm, ax=ax, title='Trained GMM') + ax.plot(t, y.T) + plt.show() + + # fit + from matplotlib.patches import Ellipse + + + def draw_ellipse(position, covariance, ax=None, **kwargs): + """Draw an ellipse with a given position and covariance""" + ax = ax or plt.gca() + + # Convert covariance to principal axes + if covariance.shape == (2, 2): + U, s, Vt = np.linalg.svd(covariance) + angle = np.degrees(np.arctan2(U[1, 0], U[0, 0])) + width, height = 2 * np.sqrt(s) + else: + angle = 0 + width, height = 2 * np.sqrt(covariance) + + # Draw the Ellipse + for nsig in range(1, 4): + ax.add_patch(Ellipse(position, nsig * width, nsig * height, + angle, **kwargs)) + + + def plot_gmm(gmm, X, label=True, ax=None): + ax = ax or plt.gca() + labels = gmm.fit(X).predict(X) + if label: + ax.scatter(X[:, 0], X[:, 1], c=labels, s=40, cmap='viridis', zorder=2) + else: + ax.scatter(X[:, 0], X[:, 1], s=40, zorder=2) + ax.axis('equal') + + w_factor = 0.2 / gmm.weights_.max() + for pos, covar, w in zip(gmm.means_, gmm.covariances_, gmm.weights_): + draw_ellipse(pos, covar, alpha=w * w_factor) + + + plot_gmm(gmm1, X) + plt.show() + + # samples from the GMM and plot + + + # GMR: condition on the input variable and plot + + # GMR: condition on the output variable and plot diff --git a/pyrobolearn/models/gp.py b/pyrobolearn/models/gp.py new file mode 100644 index 0000000..0c2b58e --- /dev/null +++ b/pyrobolearn/models/gp.py @@ -0,0 +1,649 @@ +#!/usr/bin/env python +"""Define the Gaussian Process (GP) learning model. + +This file provides the Gaussian Process (GP) model; a non-parametric, discriminative, and probabilistic model. + +As for neural networks, several frameworks can be used, such as `GPy` (which uses `numpy`), `GPyTorch` (which uses +`pytorch`), or `GPFlow` (which uses `tensorflow`). We decided to use `GPyTorch` because of the `pytorch` framework +popularity in the research community field, its flexibility, its similarity with numpy (but with automatic +differentiation: autograd), GPU capabilities, and more Pythonic approach. +""" + +import copy + +try: + import cPickle as pickle +except ImportError as e: + import pickle + +import numpy as np +import torch +import gpytorch +# import GPy +# from model import Model + +__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 GP(object): + r"""Gaussian Process model + + This is a wrapper around the GPyTorch gaussian process models. The Gaussian process is a generalization of + the multivariate Gaussian distribution. It is a non-parametric, probabilistic, and discriminative model. + + This works by putting a prior distribution on the function: + + .. math:: f ~ GP(0, K(X,X)) + + where :math:`K(.,.)` is the kernel matrix where each entry contains :math:`k(x_i, x_j)`, i.e. the kernel function + evaluated at the corresponding points. As it can be seen the kernel matrix grows with the number of samples. + + See Also: + - `GPy` (which uses numpy) [4] + - `GPFlow` (which uses TensorFlow) [5] + + References: + [1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006 + [2] GPyTorch: https://github.com/cornellius-gp/gpytorch + [3] GPyTorch examples: https://github.com/cornellius-gp/gpytorch/tree/master/examples + [4] GPy: https://gpy.readthedocs.io/en/deploy/ + [5] GPFlow: http://gpflow.readthedocs.io/en/latest/intro.html + """ + pass + + +class GPC(GP): + r"""Gaussian Process Classification + + References: + [1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006 + [2] GPyTorch: https://github.com/cornellius-gp/gpytorch + [3] GPyTorch examples: https://github.com/cornellius-gp/gpytorch/tree/master/examples + """ + pass + + +class ExactGPModel(gpytorch.models.ExactGP): + r"""Create GP prior model. + """ + + def __init__(self, mean, kernel, likelihood=None, x=None, y=None): + # create likelihood if not already set. + if likelihood is None: + likelihood = gpytorch.likelihoods.GaussianLikelihood() + super(ExactGPModel, self).__init__(train_inputs=x, train_targets=y, likelihood=likelihood) + + # set mean and kernel covariance function + self.mean = mean + self.kernel = kernel + + @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))`.""" + return self._mean + + @mean.setter + def mean(self, mean): + r"""Set the GP prior mean; that is :math:`\mu(x)` from :math:`p(f|x) = \mathcal{N}(\mu(x), K(x,x))`.""" + if mean is None: + mean = gpytorch.means.ConstantMean() + if not isinstance(mean, gpytorch.means.Mean): + raise TypeError("Expecting the mean to be an instance of `gpytorch.means.Mean`, got instead " + "{}".format(type(mean))) + self._mean = mean + + @property + def kernel(self): + r"""Return the kernel function :math:`K(.,.)` (=prior covariance of the GP).""" + return self._kernel + + @kernel.setter + def kernel(self, kernel): + r"""Set the kernel function :math:`K(.,.)` (=prior covariance of the GP).""" + if kernel is None: + kernel = gpytorch.kernels.RBFKernel() # + gpytorch.kernels.WhiteNoiseKernel() + kernel = gpytorch.kernels.ScaleKernel(kernel) + if not isinstance(kernel, gpytorch.kernels.Kernel): + raise TypeError("Expecting the kernel to be an instance of `gpytorch.kernels.Kernel`, got instead " + "{}".format(type(kernel))) + self._kernel = kernel + + 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) + covar_x = self.kernel(x) + # return gpytorch.distributions.MultivariateNormal(mean_x, covar_x) + return gpytorch.random_variables.GaussianRandomVariable(mean_x, covar_x) + + +class GPR(GP): + r"""Gaussian Process Regression + + The Gaussian process is a generalization of the multivariate Gaussian distribution. It is a non-parametric, + probabilistic, and discriminative model. + + This works by putting a prior distribution on the function: + + .. math:: f|X ~ GP(0, K(X,X)) + + where :math:`K(.,.)` is the kernel matrix where each entry contains :math:`k(x_i, x_j)`, i.e. the kernel function + evaluated at the corresponding points. As it can be seen the kernel matrix grows with the number of samples. + + The log likelihood is given by: + + .. math:: \log p(y | f) = \mathcal{N}(y | 0, K(X,X) + \sigma I) + + Learning the hyperparameters of the kernel are carried out by maximizing the marginal log likelihood, which is + given by: + + .. math:: \log p(y | X) = \int p(y | f) p(f | X) df + + The predictive distribution is carried out by assuming that the observed target values :math:`y` and + the function values :math:`f^*` at the test locations :math:`X^*` are from the same joint Gaussian distribution. + By conditioning this distribution with respect to the old dataset :math:`X, y` and the test locations :math:`X^*`, + we can derive :math:`p(f^* | X, y, X^*)` which is the predictive output distribution given the new data points + :math:`X^*`. + + Notes: + * GP takes into account correlations in the input domain but not in the output space + * The time complexity to learn a GP is :math:`O(N^3)` because of the matrix inversion during training. + * GMM vs GP: + * Both are probabilistic models. + * GMM is a generative semi-parametric model while GP is a discriminative non-parametric model. + * GMM captures the correlation between the inputs and outputs, while GP only captures correlation in the + input space. + * GMR models the variability/correlation between the predicted outputs while the GP provides uncertainty + on the predicted outputs. The predicted outputs in a GP are independent unless using a heteroscedastic + GP or a generalized Wishart process is used. + + GPyTorch:: + + For most GP regression models, you will need to construct the following GPyTorch objects: + 1. A GP Model (`gpytorch.models.ExactGP`) - This handles most of the inference. + 2. A Likelihood (`gpytorch.likelihoods.GaussianLikelihood`) - This is the most common likelihood used for GP + regression. + 3. A Mean - This defines the prior mean of the GP. If you don't know which mean to use, a + `gpytorch.means.ConstantMean` is a good place to start. + 4. A Kernel - This defines the prior covariance of the GP. If you don't know which kernel to use, a + `gpytorch.kernels.ScaleKernel(gpytorch.kernels.RBFKernel())` is a good place to start. + 5. A MultivariateNormal Distribution (`gpytorch.distributions.MultivariateNormal`) - This is the object used to + represent multivariate normal distributions. + + + References: + [1] "Gaussian Processes for Machine Learning", Rasmussen and Williams, 2006 + [2] GPy: https://gpy.readthedocs.io/en/deploy/ + [3] GPyTorch: https://github.com/cornellius-gp/gpytorch + [4] GPFlow: http://gpflow.readthedocs.io/en/latest/intro.html + """ + + def __init__(self, mean=None, kernel=None, model=None, likelihood=None): + """ + Initialize the GPR. + + Args: + mean (None, gpytorch.means.Mean): mean prior. If None, it will be set to `gpytorch.means.ConstantMean()`. + kernel (None, gpytorch.kernels.Kernel): kernel prior. If None it will be set to + `gpytorch.kernels.ScaleKernel(gpytorch.kernels.RBFKernel() + gpytorch.kernels.WhiteNoiseKernel())` + model (None, gpytorch.module.Module): the prior GP model. If None, it will create `ExactGPModel()`, a GP + model using the provided mean, kernel, and likelihood. + likelihood (None, gpytorch.likelihoods.Likelihood): the likelihood pdf. If None, it will use the + `gpytorch.likelihoods.GaussianLikelihood()` + """ + # check model + if model is None: + self.model = ExactGPModel(mean, kernel, likelihood) + else: + self.model = model + + # set model into evaluation mode + self.eval() + + # set the marginal log likelihood pdf: p(y|x) = \int p(y|f,x) p(f|x) df + self.mll = gpytorch.mlls.ExactMarginalLogLikelihood(self.likelihood_prob, self.prior) + + ############## + # Properties # + ############## + + @property + def model(self): + """Return the GP model; i.e. the prior probability density function p(f|x).""" + return self._model + + @model.setter + def model(self, model): + """Set the GP model; i.e. the prior probability density function p(f|x).""" + if not isinstance(model, gpytorch.module.Module): + raise TypeError("Expecting the GP model to be an instance of `gpytorch.module.Module`, got instead " + "{}".format(type(model))) + self._model = model + + # alias + prior = model + + @property + def likelihood_prob(self): + """Return the likelihood probability density function p(y|f,x).""" + return self.model.likelihood + + @likelihood_prob.setter + def likelihood_prob(self, likelihood): + r"""Set the likelihood probability density function p(y|f,x).""" + if likelihood is None: + likelihood = gpytorch.likelihoods.GaussianLikelihood() + if not isinstance(likelihood, gpytorch.likelihoods.Likelihood): + raise TypeError("Expecting the likelihood to be an instance of `gpytorch.likelihood.Likelihood`, got " + "instead {}".format(type(likelihood))) + self.model.likelihood = likelihood + + @property + def log_marginal_likelihood_prob(self): + r"""Return the log marginal log likelihood pdf: log p(y|x).""" + return self.mll + + @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))`.""" + return self.model.mean + + @mean.setter + def mean(self, mean): + r"""Set the GP prior mean; that is :math:`\mu(x)` from :math:`p(f|x) = \mathcal{N}(\mu(x), K(x,x))`.""" + if mean is None: + mean = gpytorch.means.ConstantMean() + if not isinstance(mean, gpytorch.means.Mean): + raise TypeError("Expecting the mean to be an instance of `gpytorch.means.Mean`, got instead " + "{}".format(type(mean))) + self.model.mean = mean + + @property + def kernel(self): + r"""Return the kernel function :math:`K(.,.)` (=prior covariance of the GP).""" + return self.model.kernel + + @kernel.setter + def kernel(self, kernel): + r"""Set the kernel function :math:`K(.,.)` (=prior covariance of the GP).""" + if kernel is None: + kernel = gpytorch.kernels.ScaleKernel(gpytorch.kernels.RBFKernel() + gpytorch.kernels.WhiteNoiseKernel()) + if not isinstance(kernel, gpytorch.kernels.Kernel): + raise TypeError("Expecting the kernel to be an instance of `gpytorch.kernels.Kernel`, got instead " + "{}".format(type(kernel))) + self.model.kernel = kernel + + @property + def dim(self): + """Return the dimension of the kernel""" + return self.kernel.dim + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other, deep=True): + """copy the other GP""" + if not isinstance(other, GPR): + raise TypeError("Expecting a GPR model.") + if deep: + return copy.deepcopy(other) + return copy.copy(other) + + @staticmethod + def is_parametric(): + """The Gaussian process is a non parametric model.""" + return False + + @staticmethod + def is_linear(): + """The Gaussian process does not have any parameters and thus no linear parameters""" + return False + + @staticmethod + def is_recurrent(): + """The Gaussian process is not a recurrent model""" + return False + + @staticmethod + def is_probabilistic(): + """The Gaussian process is a probabilistic model""" + return True + + @staticmethod + def is_discriminative(): + """The Gaussian process is a discriminative model which predicts :math:`p(y|x)`""" + return True + + @staticmethod + def is_generative(): + """The Gaussian process is not a generative model, and thus we can not sample from it""" + # TODO: actually we can sample a function from it given the initial data (in kernel matrix) + return False + + @staticmethod + def load(filename): + """ + Load a model from memory. + + Args: + filename (str): file that contains the model. + """ + return pickle.load(filename) + + @staticmethod + def _convert_to_torch(x): + if isinstance(x, np.ndarray): + return torch.from_numpy(x).float() + return x + + @staticmethod + def _convert_to_numpy(x): + if isinstance(x, torch.Tensor): + if x.requires_grad: + return x.detach().numpy() + return x.numpy() + return x + + @staticmethod + def _convert(x, to_numpy=True): + if to_numpy: + return GPR._convert_to_numpy(x) + return x + + ########### + # Methods # + ########### + + def train(self, mode=True): + """Set the model into training mode.""" + if mode: + self.model.train(True) + self.likelihood_prob.train(True) + else: + self.model.train(False) + self.likelihood_prob.train(False) + + def eval(self): + """Set the mode into evaluation mode.""" + self.model.eval() + self.likelihood_prob.eval() + + def hyperparameters(self): + """Return an iterator over the hyperparameters""" + return self.model.hyperparameters() + + def named_hyperparameters(self): + """Return an iterator over the model parameters, yielding both the name and the parameter itself.""" + return self.model.named_hyperparameters() + + def likelihood(self, x, y, to_numpy=False): + r"""Evaluate the likelihood p(y|f,x).""" + likelihood = torch.exp(self.log_likelihood(x, y, to_numpy=False)) + return self._convert(likelihood, to_numpy=to_numpy) + + def log_likelihood(self, x, y, to_numpy=False): + r"""Evaluate the log likelihood: log p(y|f,x).""" + x = self._convert_to_torch(x) + y = self._convert_to_torch(y) + f = self.model(x) + log_likelihood = self.likelihood_prob.log_probability(f, y) + return self._convert(log_likelihood, to_numpy=to_numpy) + + def marginal_likelihood(self, x, y, to_numpy=False): + r"""Evaluate the marginal likelihood: p(y|x).""" + ml = torch.exp(self.log_marginal_likelihood(x, y, to_numpy=False)) + return self._convert(ml, to_numpy=to_numpy) + + def log_marginal_likelihood(self, x, y, to_numpy=False): + r"""Evaluate the log marginal likelihood: log p(y|x).""" + x = self._convert_to_torch(x) + y = self._convert_to_torch(y) + f = self.model(x) + mll = self.mll(f, y) + return self._convert(mll[0], to_numpy=to_numpy) + + def fit(self, x, y, num_iters=100, tolerance=1e-5, optimizer=None, verbose=False): + r"""Fit the input and output data; find optimal model hyperparameters.""" + # check input and output data + x = self._convert_to_torch(x) + y = self._convert_to_torch(y) + + # set training data + # self.model.set_train_data(x, y) + self.model = ExactGPModel(self.mean, self.kernel, self.likelihood_prob, x, y) + + # set into training mode + self.train(mode=True) + + # define optimizer + need_closure = False + if optimizer is None or (isinstance(optimizer, str) and optimizer.lower() == 'lbfgs'): + optimizer = torch.optim.LBFGS([{'params': self.model.parameters()}], max_iter=num_iters, + tolerance_change=tolerance) + need_closure = True + + def closure(): + optimizer.zero_grad() + output = self.model(x) + loss = -self.mll(output, y) + loss.backward() + return loss + + elif isinstance(optimizer, str) and optimizer.lower() == 'adam': + optimizer = torch.optim.Adam([{'params': self.model.parameters()}], lr=0.1) + + if not isinstance(optimizer, torch.optim.Optimizer): + raise TypeError("Expecting the optimizer to be an instance of `torch.optim.Optimizer`.") + + # optimize + for i in range(num_iters): + # zero gradients from previous iteration + optimizer.zero_grad() + + # predict output from model p(f|x) + output = self.model(x) + + # compute loss + loss = - self.mll(output, y) + + # call backward on the loss to fill the gradients + loss.backward() + + # print info if specified + if verbose: + # print('Iter %d/%d - Loss: %.3f lengthscale: %.3f noise: %.3f' % (i + 1, num_iters, loss.item(), + # self.model.kernel.base_kernel.lengthscale.item(), self.model.likelihood.noise.item())) + print('Iter %d/%d - Loss: %.3f' % (i + 1, num_iters, loss.item())) + + # perform a step with the optimizer + if need_closure: + optimizer.step(closure) + else: + optimizer.step() + + # set into evaluation mode (=predictive posterior mode) + self.eval() + + def predict(self, x, to_numpy=True): + r""" + Predict mean output array :math:`\mu(y)` given input array :math:`x`. That is, it returns the mean of the + predictive posterior distribution :math:`E[p(y|x)]`. + + Args: + x (np.ndarray, torch.Tensor): input array + to_numpy (bool): if True, return a np.array + + Returns: + np.ndarray, torch.Tensor: output mean array + """ + x = self._convert_to_torch(x) + + # compute p(f|x) + f = self.model(x) + + # compute p(y|f,x) + y = self.likelihood_prob(f) + + # return mean + if to_numpy: + return self._convert_to_numpy(y.mean()) + return y.mean() + + def predict_prob(self, x, to_numpy=True): + r""" + Predict p(y|x) by returning the mean and the covariance arrays. + + Args: + x (np.ndarray, torch.Tensor): input array + to_numpy (bool): if True, return a np.array + + Returns: + np.ndarray, torch.Tensor: output mean array + np.ndarray, torch.Tensor: output covariance array + """ + x = self._convert_to_torch(x) + + # compute p(f|x) + f = self.model(x) + + # compute p(y|f,x) + y = self.likelihood_prob(f) + + # return mean and covariance + if to_numpy: + return self._convert_to_numpy(y.mean()), self._convert_to_numpy(y.var()) # y.covar()) + return y.mean(), y.var() # y.covar() + + def forward(self, x): + r""" + Return the predictive distribution p(y|x). + + Args: + x (np.ndarray, torch.Tensor): input array + + Returns: + gpytorch.random_variables.GaussianRandomVariable: multivariate normal (Gaussian) distribution + """ + x = self._convert_to_torch(x) + + # compute p(f|x) + f = self.model(x) + + # return p(y|f,x) + return self.likelihood_prob(f) + + def sample(self, x, num_samples=1, to_numpy=True): + """Sample the function vector from the GP; i.e. f ~ p(f|x).""" + x = self._convert_to_torch(x) + f = self.model(x) + if to_numpy: + return self._convert_to_numpy(f.sample(num_samples)) + return f.sample(num_samples) + + ############# + # Operators # + ############# + + def __str__(self): + """Return name of class""" + return self.__class__.__name__ + + +# class GPRTorch(GPR): +# r"""Gaussian Process Regression using GPyTorch +# +# This provides a wrapper around the GPyTorch module. +# +# References: +# [1] "GPyTorch: Blackbox Matrix-Matrix Gaussian Process Inference with GPU Acceleration", Gardner et al., 2018 +# [2] GPyTorch: https://github.com/cornellius-gp/gpytorch +# [3] GPyTorch Examples: https://github.com/cornellius-gp/gpytorch/tree/master/examples +# """ +# +# def __init__(self, model): +# super(GPRTorch, self).__init__(model) +# +# def _predict(self, x=None): +# return self.model(x) + + +# class GPRy(GPR): +# r"""Gaussian Process Regression using GPy. +# +# References: +# [1] "GPy: A Gaussian process framework in python", Sheffield, 2014 +# [2] GPy: https://github.com/SheffieldML/GPy +# """ +# +# def __init__(self, model): +# super(GPRy, self).__init__(model) +# +# def _predict(self, x=None, full_cov=False): +# return self.model.predict(x, full_cov)[0] + + +# TESTS +if __name__ == '__main__': + import matplotlib.pyplot as plt + from utils.converter import torch_to_numpy + + # create input and output data + x = torch.linspace(0, 1, 100) + y = torch.sin(x * (2 * np.pi)) + torch.randn(x.size()) * 0.2 + + x, y = x.numpy(), y.numpy() + + # plot true data + plt.plot(x, y, 'x') + + # create GPR + model = GPR() + + # plot prior possible functions + f = model.sample(x, num_samples=10, to_numpy=True) + plt.plot(x, f.T) + + # compute log likelihoods + print("\nBefore training:") + print("Log likelihood: {}".format(model.log_likelihood(x, y, to_numpy=True))) + print("Log marginal likelihood: {}".format(model.log_marginal_likelihood(x, y, to_numpy=True))) + + # fit the data + optimizer = 'adam' # 'lbfgs' + model.fit(x, y, num_iters=100, optimizer=optimizer, verbose=True) + + # compute log likelihoods + print("\nAfter training:") + print("Log likelihood: {}".format(model.log_likelihood(x, y, to_numpy=True))) + print("Log marginal likelihood: {}".format(model.log_marginal_likelihood(x, y, to_numpy=True))) + + # sample function and plot it + f = model.sample(x, num_samples=1, to_numpy=True) + plt.plot(x, f.T, 'k', linewidth=2.) + + # predict prob + x_test = torch.linspace(0, 1, 51).numpy() + mean_y, var_y = model.predict_prob(x_test, to_numpy=True) + std_y = np.sqrt(var_y) + plt.plot(x_test, mean_y, 'b') + plt.fill_between(x_test, mean_y-2*std_y, mean_y+2*std_y, facecolor='green', alpha=0.5) + plt.ylim([-3, 3]) + plt.show() + + # Another way to predict + pred = model.forward(x_test) + lower, upper = pred.confidence_region() + + plt.plot(x, y, 'k*') + plt.plot(x_test, torch_to_numpy(pred.mean()), 'b') + plt.fill_between(x_test, torch_to_numpy(lower), torch_to_numpy(upper), alpha=0.5) + plt.ylim([-3, 3]) + plt.show() diff --git a/pyrobolearn/models/hmm.py b/pyrobolearn/models/hmm.py new file mode 100644 index 0000000..bd5eb77 --- /dev/null +++ b/pyrobolearn/models/hmm.py @@ -0,0 +1,130 @@ +# This file describes the Hidden Markov Model + +from gaussian import Gaussian +from model import Model +from hmmlearn.hmm import GaussianHMM + + +class HMM(object): + r"""Hidden Markov Models + + Description: emission probabilities, transition probabilities,... + + References: + [1] "Pattern Recognition and Machine Learning" (chap 13), Bishop, 2006 + + The code was inspired by the following codes: + * `hmmlearn`: https://github.com/hmmlearn/hmmlearn + * `ghmm`: http://ghmm.sourceforge.net/ + * `pbdlib`: https://gitlab.idiap.ch/rli/pbdlib-python/tree/master/pbdlib + """ + + def __init__(self, emission_prob=None): + if emission_prob is None or (isinstance(emission_prob, str) and emission_prob.lower() == 'gaussian'): + emission_prob = Gaussian() + + ############## + # Properties # + ############## + + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other): + if not isinstance(other, HMM): + raise TypeError("Trying to copy an object which is not a HMM") + + @staticmethod + def isParametric(): + """The HMM is a parametric model""" + return True + + @staticmethod + def isLinear(): + """The HMM is a non-linear model""" + return True + + @staticmethod + def isRecurrent(): + """The HMM is recurrent; current outputs depends on previous inputs and states""" + return True + + @staticmethod + def isProbabilistic(): + """The HMM a probabilistic model""" + return False + + @staticmethod + def isDiscriminative(): + """The HMM is a discriminative model""" + return False + + @staticmethod + def isGenerative(): + """The HMM is a generative model which models the joint distributions on states and outputs. + This means we can sample from it.""" + return True + + ########### + # Methods # + ########### + + def likelihood(self): + pass + + # alias + pdf = likelihood + + def joint_pdf(self, X, Z): + pass + + def sample(self, size=None, seed=None): + """ + Sample from the HMM. + + Args: + size: + seed: + + Returns: + + """ + pass + + def expectation_step(self): + """ + Expectation step in the expectation-maximization algorithm. + + Returns: + + """ + pass + + def maximization_step(self): + pass + + def expectation_maximization(self, X): + """Expectation-Maximization (EM) algorithm""" + pass + + def forward_backward(self): + """Forward backward algorithm""" + pass + + def sum_product(self): + """Sum-product algorithm""" + pass + + def viterbi(self): + """Viterbi algorithm""" + pass + + +class HSMM(HMM): + r"""Hidden semi-Markov Models + + """ + pass \ No newline at end of file diff --git a/pyrobolearn/models/kmp.py b/pyrobolearn/models/kmp.py new file mode 100644 index 0000000..354c7bd --- /dev/null +++ b/pyrobolearn/models/kmp.py @@ -0,0 +1,1021 @@ +#!/usr/bin/env python +"""Define the kernelized movement primitive class. + +This file provides the Kernelized Movement Primitive (KMP) model, and uses the Gaussian mixture model as well as +the Gaussian distribution defined respectively in `gmm.py` and `gaussian.py`. +""" + +import numpy as np +from scipy.linalg import block_diag +import copy + +# from model import Model +from gmm import GMM, Gaussian + + +__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 RBF(object): + """ + RBF kernel. + + .. math:: k(x1, x2) = \sigma^2 * \exp(- ||x_1 - x_2||^2 / l) + + where :math:`x1` and :math:`x2` are two vectors, :math:`\sigma^2` is the variance, and :math:`l` is + the lengthscale. + """ + + def __init__(self, variance=1., lengthscale=1.): + """ + Initialize the RBF kernel. + + Args: + variance (float): variance + lengthscale (float): lengthscale + """ + self.var = variance + self.l = lengthscale + + def k(self, x1, x2=None): + """ + Compute kernel function: :math:`k(x1, x2) = \sigma^2 * \exp(- ||x_1 - x_2||^2 / l)` + + where :math:`x1` and :math:`x2` are two vectors, :math:`\sigma^2` is the variance, and :math:`l` is + the lengthscale. + + Args: + x1 (float, np.array): 1st value + x2 (float, np.array, None): 2nd value. if None, it will take x1. + + Returns: + float: similarity measure between the two given values. + """ + if x2 is None: + x2 = x1 + diff = x1 - x2 + return self.var * np.exp(- np.inner(diff, diff) / self.l) + + def __call__(self, x1, x2=None): + """Return output from kernel function""" + return self.k(x1, x2) + + +class KMP(object): + r"""Kernelized Movement Primitives + + Kernelized Movement Primitives allows to encode a movement/trajectory using kernels. The use of kernels makes it + practical for high-dimensional inputs. + + KMP is a non-parametric (but a semi-parametric approach is used to initialize it) probabilistic discriminative + model. + + References: + [1] "Kernelized Movement Primitives", Huang et al., 2017 + """ + + def __init__(self, kernel_fct=None, database=None): + """ + Initialize the KMP. + + Args: + kernel_fct (None, callable): kernel function. If None, it will use the `GPy.kern.RBF` with a variance + of 1, and a lengthscale of 2. + """ + super(KMP, self).__init__() + + self._input_dim = 0 + self._output_dim = 0 + + # set kernel fct + self.K = kernel_fct if kernel_fct is not None else RBF(variance=1., lengthscale=2.) + + # reference database + if database is None: + self._database = [] + else: + self._database = database + + # Inverse Kernel matrix (useful when computing the prediction) + self.K_inv = None + self.prior_reg = 1. + + # translation vector and rotation matrix + self.bias = 0 + self.rot = None + + ############## + # Properties # + ############## + + @property + def kernel_fct(self): + """Return the kernel fct used for KMP""" + return self.K + + @property + def database(self): + """Return the reference database""" + return self._database + + @property + def input_dim(self): + """Return the input dimension""" + return self._input_dim + + @property + def output_dim(self): + """Return the output dimension""" + return self._output_dim + + @property + def bias_vector(self): + """Return the bias vector added to the predicted mean by the KMP""" + return self.bias + + @property + def rotation_matrix(self): + """Return the rotation matrix applied to the predicted mean and covariance by the KMP""" + return self.rot + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other): + """Copy the other KMP""" + kmp = KMP(kernel_fct=other.kernel_fct) + kmp._database = copy.deepcopy(other.database) + kmp.bias = other.bias + kmp.rot = other.rot + kmp.K_inv = np.copy(other.K_inv) + + @staticmethod + def is_parametric(): + """The KMP is a non-parametric model which uses a kernel""" + return False + + @staticmethod + def is_linear(): + """The KMP has no parameters, and thus has no linear parameters""" + return False + + @staticmethod + def is_recurrent(): + """The KMP is not a recurrent model where the output depends on the given input and previous outputs. + Sequential data are encoded in the kernel.""" + return False + + @staticmethod + def is_probabilistic(): # same as is_stochastic + """The KMP returns a mean and covariance matrix which parametrizes a normal distribution""" + return True + + @staticmethod + def is_discriminative(): + r"""The KMP is a discriminative model which predicts :math:`p(y|x)` where :math:`x` is the input, + and :math:`y` is the output""" + return True + + @staticmethod + def is_generative(): + """The KMP is not a generative model, and thus we can not sample from it""" + return False + + @staticmethod + def create_reference_database(X, Y, gmm=None, gmm_num_components=10, dist=None, database_threshold=1e-3, + database_size_limit=100, sample_from_gmm=False, gmm_init='kmeans', gmm_reg=1e-8, gmm_num_iters=1000, + gmm_convergence_threshold=1e-4, seed=None, verbose=True, block=True): + r""" + Create reference database from the data. This database contains a list of input data with their + corresponding predicted output distribution by the reference model (which in this case is a GMM). + + X (np.array[N,T,I], list of np.array[T,I]): input data matrix of shape NxTxI, where N is the number of + trajectories, T is its length, and I is the input data dimension. + Y (np.array[N,T,O], list of np.array[T,O]): corresponding output data matrix of shape NxTxO, where N is + the number of trajectories, T is its length, and O is the output data dimension. + gmm (None, GMM): the reference generative model. If None, it will create a GMM. + gmm_num_components (int): the number of components for the underlying reference GMM. + dist (callable, None): callable function which accepts two data points from X, and compute the distance + between them. If None and `sample_from_gmm` is False, it will use the 2-norm. + database_threshold (float): threshold associated with the `dist` argument above. If the distance between + a new data point and data point in the database is below the threshold, it will be added to + the database. + database_size_limit (int): limit size of the database. + sample_from_gmm (bool): If we should sample from the generative model to get the inputs to put in the + database. If True, it doesn't use the `dist` and `database_threshold` parameters. + gmm_init (str): how the Gaussians should be initialized. Possible values are 'random' or 'kmeans'. + gmm_reg (float): regularization term for the GMM (that are added to the Gaussians) + gmm_num_iters (int): the maximum number of iterations to train the reference model (GMM) + gmm_convergence_threshold (float): convergence threshold when training the reference model (GMM) + seed (int, None): random seed for the initialization and training of the GMM, and when sampling + verbose (bool): if we should print details during the optimization process + block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to + continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where + `N` is the size of the kernel matrix. + + Returns: + list[(np.ndarray, Gaussian)]: database which is a list of tuples where each one contains an input data + array and the corresponding predicted output Gaussian (by GMR) + """ + # TODO: replace gmm by joint generative model + + # check given arguments + X, Y = np.array(X), np.array(Y) + if X.shape[:2] != Y.shape[:2]: + if X.shape[0] != Y.shape[0]: + raise ValueError("The number of trajectories are different between the input and output data") + else: + raise ValueError("The length of trajectories between the input and output data do not match.") + + # get useful variables (number of trajectories, their length, input dimension, output dimension) + N, T, I = X.shape + O = Y.shape[2] + N_tot = N * T # total number of data points + + # create GMM + if gmm is None: + gmm = GMM(num_components=gmm_num_components) + + # reshape the data to be N_tot x D where D = I + O + data = np.dstack((X, Y)) # shape: NxTxD + data = data.reshape(-1, I + O) # shape: NTxD + + # train gmm + gmm.fit(data, reg=gmm_reg, num_iters=gmm_num_iters, threshold=gmm_convergence_threshold, init=gmm_init, + seed=seed, verbose=verbose) + + # create reference database + database = [] + + # if sample from GMM + if sample_from_gmm: + database = gmm.sample(size=database_size_limit)[:, range(I)] # shape: NdxI + + # use the distance function to check if we should add the input data into the database + else: + # define distance function + if dist is None: + def dist(x1, x2): + return np.linalg.norm(x1 - x2) + + # check inputs to put in the reference database (time complexity: O((NT)^2)) + for x_traj in X: + for x_curr in x_traj: + # compare current input with previous inputs, and add in database if unique enough + can_add = True + for x_prev in database: + if dist(x_curr, x_prev) < database_threshold: + can_add = False + break + if can_add: + database.append(x_curr) + + # if the size of the database is bigger than database size limit, sample uniformly from it + if len(database) > database_size_limit: + idx = np.random.choice(range(len(database)), size=database_size_limit, replace=False) + database = database[idx] + + # update database to also contain prediction from GMR + database = [(x, (gmm.condition(x, idx_out=range(I, O))).approximate_by_single_gaussian()) + for x in database] + + # return constructed reference database + return database + + @staticmethod + def combine(x, kmps, frames): + r""" + Combine different local KMPs. + + Warnings: This assumes that the input and output data can be mapped from local frames to a base frame + by an affine transformation. This does not work with inputs or outputs that represents something else + than coordinates. For instance, it does not work if the inputs are images or sensor values. + + Args: + x (np.array[I], np.array[N,I]): new input data vector or matrix + kmps (KMP, list of KMP): list of local KMPs + frames (tuple, list of tuples): list of tuples where each tuple contains a rotation matrix and a bias + translation vector + + Returns: + Gaussian: resulting predicted Gaussian + """ + if len(kmps) != len(frames): + raise ValueError("The number of local frames is different from the number of KMPs") + + gaussians = [] + for kmp, frame in zip(kmps, frames): + # get rotation matrix and translation vector + A, b = frame + if not isinstance(A, np.ndarray) or len(A.shape) != 2: + raise TypeError("Expecting A to be a rotation matrix (2D array)") + if not isinstance(b, np.ndarray) or len(A.shape) != 1: + raise TypeError("Expecting b to be a translation vector (1D array)") + + # predict the gaussian distribution in the local frame + gaussian = kmp.predict_proba(x, return_gaussian=True) + + # project back the distribution on the base frame + gaussian = A * gaussian + b + + gaussians.append(gaussian) + + # compute the product of all gaussians which is the optimal solution + gaussian = np.prod(gaussians) + return gaussian + + ########### + # Methods # + ########### + + def fit(self, X, Y, gmm=None, gmm_num_components=10, prior_reg=1., dist=None, database_threshold=1e-3, + database_size_limit=100, sample_from_gmm=False, gmm_init='kmeans', gmm_reg=1e-8, gmm_num_iters=1000, + gmm_convergence_threshold=1e-4, seed=None, verbose=True, block=True): + r""" + Fit the given data composed of inputs and outputs. + + This works by minimizing the KL-divergence between a parametric probabilistic discriminative model + and the predicted output distribution of a reference probabilistic model. First, the reference model (e.g. + a Gaussian mixture model) is trained on the given data (i.e. inputs :math:`x \in \mathbb{R}^{I} and outputs + :math:`y \in \mathbb{R}^{O}`). A reference database is then constructed containing `N` data inputs with the + corresponding output Gaussian distribution resulting from GMR given the data inputs. + + Then, a parametric model is given by :math:`y(x) = \Phi(x)^T w` where a Gaussian distribution is put on the + weights :math:`w \in \mathbb{R}^{BO}` such that :math:`w \sim \mathcal{N}(\mu_w, \Sigma_w)`, and thus + :math:`y(x) \sim \mathcal{N}(\Phi(x)^T \mu_w, \Phi(x)^T \Sigma_w \Phi(x))`. The matrix + :math:`\Phi(x) \in \mathbb{R}^{BO \times O}` is a block diagonal matrix containing basis functions + on its diagonal. + + The loss that is being minimized by KMP is given by: + + .. math:: + + \mathcal{L}(\mu_w, \Sigma_w) = \sum_{n=1}^N KL[p(y|x_n;\theta) || p_{ref}(y | x_n)] + + \tau ( (\mu_w^T\mu_w) + tr(\Sigma_w) ) + + where :math:`\theta = \{\mu_w, \Sigma_w\}` are the parameters that are being optimized, + :math:`p_{ref}(y | x_n) = \mathcal{N}(\mu_n, \Sigma_n)` is the predicted reference distribution + (e.g. Gaussian by GMR), and :math:`\tau` is the prior regularization term. + + Once the parametric model has been optimized, the optimal mean and covariance of the weights are given by: + + .. math:: + + \mu_w = \Omega (\Omega^T \Omega + \tau \Sigma)^{-1} \mu + \Sigma_w = N (\Omega \Sigma \Omega^T + \tau I)^{-1} + + where :math:`\Omega = [\Phi(x_1) ... \Phi(x_N)] \in \mathbb{R}^{BO \times NO}`, + :math:`\Sigma = blockdiag(\Sigma_1, ..., \Sigma_N) \in \mathbb{R}^{NO \times NO}`, and + :math:`\mu = [\mu_1^T ... \mu_N^T]^T \in \mathbb{R}^{NO \times 1}`. + + Thus, the predicted output mean and covariance on a new input :math:`x^*` is given by: + + .. math:: + + \mu_y &= \Phi(x^*)^T \mu_w = \Phi(x^*) \Omega (\Omega^T \Omega + \tau \Sigma)^{-1} \mu \\ + \Sigma_y &= \Phi(x^*)^T \Sigma_w \Phi(x^*) = N \Phi(x^*)^T (\Omega\Sigma\Omega^T+\tau I)^{-1} \Phi(x^*) + + And by using the kernel trick (and the Woodbury identity for the covariance), this resumes to: + + .. math:: + + \mu_y &= k^* (K + \tau \Sigma)^{-1} \mu \\ + \Sigma_y &= \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + + where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, + :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated + on the new input, and where :math:`k(x_i, x_j) = \hat{k}(x_i, x_j) I_O` with the identity matrix + `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. + + Args: + X (np.array[N,T,I], list of np.array[T,I]): input data matrix of shape NxTxI, where N is the number of + trajectories, T is its length, and I is the input data dimension. + Y (np.array[N,T,O], list of np.array[T,O]): corresponding output data matrix of shape NxTxO, where N is + the number of trajectories, T is its length, and O is the output data dimension. + gmm (None, GMM): the reference generative model. If None, it will create a GMM. + gmm_num_components (int): the number of components for the underlying reference GMM. + prior_reg (float): prior regularization term + dist (callable, None): callable function which accepts two data points from X, and compute the distance + between them. If None and `sample_from_gmm` is False, it will use the 2-norm. + database_threshold (float): threshold associated with the `dist` argument above. If the distance between + a new data point and data point in the database is below the threshold, it will be added to + the database. + database_size_limit (int): limit size of the database. + sample_from_gmm (bool): If we should sample from the generative model to get the inputs to put in the + database. If True, it doesn't use the `dist` and `database_threshold` parameters. + gmm_init (str): how the Gaussians should be initialized. Possible values are 'random' or 'kmeans'. + gmm_reg (float): regularization term for the GMM (that are added to the Gaussians) + gmm_num_iters (int): the maximum number of iterations to train the reference model (GMM) + gmm_convergence_threshold (float): convergence threshold when training the reference model (GMM) + seed (int, None): random seed for the initialization and training of the GMM, and when sampling + verbose (bool): if we should print details during the optimization process + block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to + continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where + `N` is the size of the kernel matrix. + + References: + [1] "Kernelized Movement Primitives", Huang et al., 2017 + """ + # TODO: replace gmm by joint generative model + + # create reference database + self._database = self.create_reference_database(X, Y, gmm=gmm, gmm_num_components=gmm_num_components, + dist=dist, database_threshold=database_threshold, + database_size_limit=database_size_limit, + sample_from_gmm=sample_from_gmm, gmm_init=gmm_init, + gmm_reg=gmm_reg, gmm_num_iters=gmm_num_iters, + gmm_convergence_threshold=gmm_convergence_threshold, + seed=seed, verbose=verbose, block=block) + + # compute kernel inverse from database + K, K_inv = self.learn_from_database(self._database, prior_reg=prior_reg, verbose=verbose, block=block) + self.K_inv = K_inv # shape: NOxNO + + # aliases + learn = fit + imitate = fit + + def learn_from_database(self, database=None, prior_reg=1., verbose=True, block=True): + r""" + Learn the Kernel matrix from the database. Specifically, it computes :math:`K` and + :math:`(K + \tau \Sigma)^{-1}`. The latter is because this is used for the prediction part; for the predicted + mean and covariance, and is better to compute it during the learning phase than the prediction phase. + + Args: + database (list of tuples): list of tuples which contain the input data array and the associated predicted + output distribution by the reference model. + prior_reg (float): prior regularization term + verbose (bool): if we should print details during the optimization process + block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to + continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where + `N` is the size of the kernel matrix. + + Returns: + np.array[NO,NO]: Kernel matrix :math:`K` + np.array[NO,NO]: Inverse Kernel matrix :math:`(K + \tau \Sigma)^-1` + """ + # Quick checks + if database is None: + database = self.database + if len(database) == 0: + raise ValueError("There are no elements in the database") + if prior_reg <= 0: + raise ValueError("The prior regularization term needs to be strictly bigger than 0") + + # output dimension and size of database + output_dim = database[0][1].size + N = len(self._database) + + # check the size of the kernel matrix + if N * output_dim > 1000 and verbose: + print("Warning: trying to inverse a {} by {} 2D matrix... This could be computationally " + "expensive...".format(N * output_dim, N * output_dim)) + if block: + raw_input("Please press enter to continue with the inversion of the matrix. Ctrl+C to stop " + "the program") + + # compute mean, covariance, and kernel from database + self.mu = np.array([gaussian.mean for _, gaussian in self.database]).reshape(-1, 1) # shape: NO x 1 + cov = block_diag(*[gaussian.cov for _, gaussian in self.database]) # shape: NOxNO + I_O = np.identity(output_dim) # shape: OxO + K = np.array([[self.K(xi, xj) * I_O for xj, _ in self.database] + for xi, _ in self.database]) # shape: NOxNO + + # compute kernel inverse + K_inv = np.linalg.inv(K + prior_reg * cov) # shape: NOxNO + + # remember variables for prediction + self.N, self.prior_reg = len(database), prior_reg + + # return kernel and kernel inverse + return K, K_inv + + def loss(self): + r""" + Compute the KL loss between the fitted KMP and the reference database. + + .. math:: + + \mathcal{L} = \sum_{n=1}^N KL[\mathcal{N}(\mu_n^*, \Sigma_n^*) || \mathcal{N}_{ref}(\mu_n, \Sigma_n)] + + where :math:`\mu_n^* = k^* (K + \tau \Sigma)^{-1} \mu` and + :math:`\Sigma_n^* = \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T)` are the predicted + mean and covariance by the KMP, and :math:`\mu_n` and :math:`\Sigma_n` are the predicted mean and covariance + by GMR. + + Returns: + float: KL loss + """ + loss = 0. + + # go through the database + for x, gaussian in self.database: + # predict gaussian by KMP + kmp_gaussian = self.predict_proba(x, return_gaussian=True) + + # compute KL divergence between gaussian from database (which is a result of GMR), and the gaussian + # predicted by the KMP + kl_loss = gaussian.kl_divergence(kmp_gaussian) + + # add individual loss + loss += kl_loss + + # return total loss + return loss + + def _compute_k(self, x): + r""" + Compute the k matrix between the given new input and the inputs from the reference database. + + Args: + x (np.array[I], np.array[N,I]): input data vector or matrix + + Returns: + np.array: k vector + """ + # compute k vector (which compares given input data with previous ones) + I = np.identity(self.output_dim) + k = np.array([self.K(x, x_prev) * I for x_prev, _ in self.database]) # shape: NxOxO + k = k.reshape(-1,1).T # shape: OxNO + return k + + def predict(self, x): + r""" + Predict output mean :math:`\mu_y` given input data :math:`x^*`. + + .. math:: + + \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ + + where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, + :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated + on the new input, and where :math:`k(x_i, x_j) = \hat{k}(x_i, x_j) I_O` with the identity matrix + `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. + + Args: + x (np.array[I], np.array[N,I]): new input data vector or matrix + + Returns: + np.array[O], np.array[N,O]: output mean(s) + """ + # if only one sample + if len(x.shape) == 1: + x = [x] + + # compute predicted mean(s) + means = [] + for xi in x: + # compute k vector (which compares given input data with previous ones) + k = self._compute_k(xi) + + # return mean + mean = k.dot(self.K_inv).dot(self.mu) + means.append(mean) + + # return the same shape as input + means = np.array(means) + if means.shape[0] == 1: + means = means[0] + + # return the predicted mean(s) + return means + + def predict_proba(self, x, return_gaussian=True): + r""" + Predict the probability of output :math:`\mathcal{N}(\mu_y, \Sigma_y)` given input data :math:`x^*`. + + .. math:: + + \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ + \Sigma_y(x^*) &= \frac{N}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + + where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, + :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated + on the new input, and where :math:`k(x_i, x_j) = \hat{k}(x_i, x_j) I_O` with the identity matrix + `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. + + Args: + x (np.array[I], np.array[N,I]): input data vector or matrix + + Returns: + if return_gaussian: + Gaussian, or list of Gaussian: gaussian(s) for each input + else: + np.array[O], np.array[N,O]: output mean(s) + np.array[O,O], np.array[N,O]: output covariance(s) + """ + # if only one sample + only_one_sample = False + if len(x.shape) == 1: + only_one_sample = True + x = [x] + + # useful variables + coeff = self.N / self.prior_reg + I = np.identity(self.output_dim) + + # compute predicted mean(s) and covariance(s) + means, covs = [], [] + for xi in x: + # compute k vector + k = self._compute_k(xi) + k_input = self.K(xi, xi) * I + + # compute mean and covariance + mean = k.dot(self.K_inv).dot(self.mu) + cov = coeff * (k_input - k.dot(self.K_inv).dot(k.T)) + + means.append(mean) + covs.append(cov) + + # return the same shape as input + means, covs = np.array(means), np.array(covs) + if only_one_sample: + means, covs = means[0], covs[0] + + # if need to return Gaussian(s) + if return_gaussian: + if only_one_sample: + return Gaussian(mean=means, covariance=covs) + return [Gaussian(mean=mean, covariance=cov) for mean, cov in zip(means, covs)] + + # else, return mean(s) and covariance(s) + return means, covs + + def modulate(self, x, y_mean, y_cov, dist=None, threshold=1, update_database=False, prior_reg=1., + verbose=True, block=True): + r""" + Modulate the prediction given new data point with their associated covariances. + + Warnings: once the KMP has been modulated, the user has to relearn it on the original database if he/she + wishes to predict again on the old data. This can be done by calling `kmp.learn_from_database(kmp.database)`, + where `kmp.database` is the original database. + + Args: + x (np.array[I], np.array[N,I]): input data vector or matrix + y_mean (np.array[O], np.array[N,O]): mean of new data point(s) + y_cov (np.array[O,O], np.array[N,O,O]): covariance of new data point(s). A small covariance means the user + wants a high precision around the new data point. + dist (callable, None): callable function which accepts two data points from X, and compute the distance + between them. If None and `sample_from_gmm` is False, it will use the 2-norm. + threshold (float): threshold associated with the `dist` argument above. If the distance between + a new data point and data point in the database is below the threshold, it will be added to + the database. + update_database (bool): If True, it will modify permanently the original database by including the new + given points. + prior_reg (float): prior regularization term + verbose (bool): if we should print details during the optimization process + block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to + continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where + `N` is the size of the kernel matrix. + """ + # quick checks + if len(self.database) == 0: + raise ValueError("There are no elements in the reference database") + if len(x.shape) == 1: + x = [x] + if len(y_mean.shape) == 1: + y_mean = [y_mean] + if len(y_cov.shape) == 2: + y_cov = [y_cov] + if len(x) != len(y_mean): + raise ValueError("The number of input data points does not match the number of output data points") + if len(y_mean) != len(y_cov): + raise ValueError("The number of means and covariances for the output data points doesn't match") + + # define distance function + if dist is None: + def dist(x1, x2): + return np.linalg.norm(x1 - x2) + + # copy database + database = copy.deepcopy(self.database) + + # for each new input + for xi, yi, y_ci in zip(x, y_mean, y_cov): + # check the closest input inside the reference database (time complexity: O((NT)^2)) + idx_closest = 0 + x_closest = database[idx_closest][0] + dist_closest = dist(x_closest, xi) + for idx, (x_curr, _) in enumerate(database): + # if the current distance between the new point and the current point is smaller than the previous + # closest one, update the closest point + dist_curr = dist(x_curr, xi) + if dist_curr < dist_closest: + idx_closest = idx + x_closest = x_curr + dist_closest = dist(x_closest, xi) + + # check with the threshold if the closest point should be replaced by the new input data point, + # or if the new point should just be appended in the database + gaussian = Gaussian(mean=yi, covariance=y_ci) + if dist_closest < threshold: + database[idx_closest] = (xi, gaussian) + else: + database.append((xi, gaussian)) + + # compute kernel inverse from the extended database + K, K_inv = self.learn_from_database(database, prior_reg=prior_reg, verbose=verbose, block=block) + self.K_inv = K_inv # shape: NOxNO + + if update_database: + self._database = database + + # return the extended database + return database + + # alias + add_via_points = modulate + + def superpose(self, databases, priorities, update_database=False, prior_reg=1., verbose=True, block=True): + r""" + Superpose different trajectories based on priorities. + + This is given by the following optimization: + + .. math:: + + \mathcal{L} = \sum_{n=1}^N \sum_{l=1}^L \gamma_{n,l} KL[p(y|x_n;\theta) || p^l_{ref}(y | x_n)] + + where :math:`L` is the total number of trajectories (i.e. databases), :math:`\gamma_{n,l} \in ]0,1[` is + the associated priority with each sample and trajectory and respects :math:`\sum_{l=1}^L \gamma_{n,l} = 1`. + + The optimal solution is given by the product of :math:`L` Gaussians whose mean and covariance are + predicted by their corresponding KMP. + + Args: + databases (list[list[(np.ndarray, Gaussian)]]): list of database where each database is a list of tuples + where each one contains an input data array and the corresponding predicted output Gaussian (by GMR). + The databases have the same size, and the same input arrays in the same order. + priorities(np.array[L,N]]): list of priorities (float) for each point in each database. + update_database (bool): If True, it will modify permanently the original database by including the new + given points. + prior_reg (float): prior regularization term + verbose (bool): if we should print details during the optimization process + block (bool): if the size of the kernel matrix is bigger than 1000, it will ask for confirmation to + continue. The kernel matrix has to be inversed, which has a time complexity of `O(N^3)` where + `N` is the size of the kernel matrix. + """ + # quick checks + if not isinstance(databases, (tuple, list, np.ndarray)): + raise TypeError("Expecting a list of databases (i.e. list[list[(np.ndarray, Gaussian)]])") + if len(databases) <= 0: + raise ValueError("Expecting a non empty list of databases") + if len(databases) != len(priorities): + raise ValueError("The number of databases does not match with the number of trajectories") + L, N = len(databases), len(databases[0]) + databases = np.array(databases) # shape: LxNx2 + priorities = np.array(priorities) # shape: LxN + if priorities.shape != (L,N): + raise ValueError("Expecting the priorities to be of shape (L,N) where L is the number of databases, " + "and N is the number of elements in these databases.") + if not np.allclose(np.sum(priorities, axis=0), np.ones(L)): + raise ValueError("The priorities should sum to one: np.sum(priorities, axis=0) == np.ones(L)") + # TODO: check if same input + + # create mixed reference database + mixed_database = [] + for i in range(N): + priority = priorities[:,i] # shape: L + database = databases[:,i,1] # shape: L + x_input = databases[0,i,0] + + # quick check if similar input for each database + for j in range(1,L): + if np.allclose(databases[j-1,i,0], databases[j,i,0]): + raise ValueError("The element {} in the database {} and {} are different input " + "arrays".format(i, j-1, j)) + + # create rescaled gaussians + gaussians = [Gaussian(mean=g.mean, covariance=g.cov / priority) + for priority, g in zip(priorities, database)] + + # take the product + gaussians = np.prod(gaussians) + + # add the result in the database + mixed_database.append((x_input, gaussians)) + + # compute kernel inverse from the extended database + K, K_inv = self.learn_from_database(mixed_database, prior_reg=prior_reg, verbose=verbose, block=block) + self.K_inv = K_inv # shape: NOxNO + + if update_database: + self._database = mixed_database + + # return the mixed reference database + return mixed_database + + # def create_local_databases(self, frames, global_database=None): + # """ + # Create local databases from the global one, and return them. If the user wishes to learn local KMPs, + # he/she can create several KMP and then for each one of them, call the method `learn_from_database()` while + # providing the local database to it. + # + # Args: + # frames: + # global_database: + # + # Returns: + # + # """ + # pass + + def multiply(self, rotation_matrix): + r""" + Multiply the prediction of a KMP by a square matrix :math:`A`. The prediction of the KMP given a new input + :math:`x` will now be given by: :math:`\mathcal{N}(A \mu(x), A \Sigma(x) A^T)`, where :math:`\mu` and + :math:`\Sigma` are the original mean and covariance predicted by KMP. + + Args: + rotation_matrix (np.array[O,O]): 2D rotation matrix of shape OxO, where O is the dimension of the output. + + Returns: + KMP: resulting KMP + """ + R = rotation_matrix + if isinstance(R, np.ndarray): + if len(R.shape) != 2: + raise ValueError("Expecting the numpy array to be a 2D array, instead got shape:" + " {}".format(R.shape)) + if R.shape != (self.output_dim, self.output_dim): + raise ValueError("Size mismatch: the dimension of the predicted output by the KMP is {}, so expecting " + " a 2D array of shape {} but got instead {}" + "".format(self.output_dim, (self.output_dim, self.output_dim), R.shape)) + + # copy KMP and rotate it + kmp = KMP(kernel_fct=self.kernel_fct, database=self.database) + kmp.rot = R.dot(self.rot) + + # return kmp + return kmp + raise TypeError("Expecting a rotation matrix") + + def add(self, bias_vector): + r""" + Add a vector :math:`b` to the prediction of a KMP. The prediction of the KMP given a new input :math:`x` will + now be given by: :math:`\mathcal{N}(\mu(x) + b, \Sigma(x))`, where :math:`\mu` and :math:`\Sigma` are + the original mean and covariance predicted by KMP. + + Args: + bias_vector (np.array[O], float, int): 1D bias vector of the size of the output dimension. If an integer + or a float number is given, it will create a vector of the size of the output dimension with the given + value. + + Returns: + KMP: resulting KMP + """ + bias = bias_vector + if isinstance(bias, (float, int)): + bias = np.array([bias]*self.output_dim, dtype=np.float) + + if isinstance(bias, np.ndarray): + if len(bias.shape) != 1: + raise ValueError("Expecting the numpy array to be a 1D array, instead got shape:" + " {}".format(bias.shape)) + if bias.shape[0] != self.output_dim: + raise ValueError("Size mismatch: the dimension of the predicted output by the KMP is {} but the " + "dimension of the given vector is {}".format(self.output_dim, bias.shape[0])) + + # copy KMP and add bias vector + kmp = KMP(kernel_fct=self.kernel_fct, database=self.database) + kmp.bias = self.bias + bias + + # return kmp + return kmp + raise TypeError("Expecting the other element to be a vector") + + def affine_transform(self, A, b=None): + r""" + Perform an affine transformation on the predicted output by the KMP. That is, given a new input :math:`x^*`, + instead of predicting: + + .. math:: + + \mu_y(x^*) &= k^* (K + \tau \Sigma)^{-1} \mu \\ + \Sigma_y(x^*) &= \frac{T}{\tau} (k(x^*, x^*) - k^* (K + \tau \Sigma)^{-1} k^*^T) + + where :math:`K(X,X) \in \mathbb{R}^{NO \times NO}` is the kernel matrix, + :math:`k^* = [k(x^*, x_1) ... k(x^*,x_N)] \in \mathbb{R}^{O \times NO}` is the kernel evaluated + on the new input, and where :math:`k(x_i, x_j) = \hat{k}(x_i, x_j) I_O` with the identity matrix + `I_O \in \mathbb{O \times O}` and :math:`\hat{k}(x_i, x_j)` the kernel function. + + it will predict: + + .. math:: + + \mu(x^*) = A \mu_y(x^*) + b + \Sigma(x^*) = A \Sigma_y(x^*) A^T + + where :math:`A` is a rotation matrix, and :math:`b` is a bias vector. + + Args: + A (np.ndarray[O,O]): square rotation matrix + b (np.ndarray[O], float, int): bias vector + + Returns: + KMP: resulting KMP + """ + kmp = self.multiply(A) + kmp = kmp.add(b) + return kmp + + ############# + # Operators # + ############# + + def __str__(self): + """Return the class name""" + return self.__class__.__name__ + + def __call__(self, x, deterministic=True): + """Predict output given input data""" + if deterministic: + return self.predict(x) + return self.predict_proba(x) + + def __len__(self): + """ + Return the length of the database. + """ + return len(self.database) + + def __iter__(self): + """ + Iterate over the database and yield the input data with the predicted Gaussian output by GMR. + """ + for x, gaussian in self.database: + yield x, gaussian + + def __getitem__(self, index): + """ + Return the specified entry from the database. + + Args: + idx (int, slice): index / indices + + Returns: + np.array: input data + Gaussian: corresponding predicted Gaussian output (by GMR) + """ + return self.database[index] + + def __add__(self, bias_vector): + """ + Add a vector :math:`b` to the prediction of a KMP. + + Args: + other (np.array[O], float, int): the other vector. + + Returns: + KMP: resulting KMP + """ + return self.add(bias_vector) + + def __radd__(self, bias_vector): + return self.add(bias_vector) + + def __mul__(self, rotation_matrix): + """ + Rotate the prediction of a KMP by the given rotation matrix. + Multiply two GMMs, or a GMM by a Gaussian, matrix, or float. See the `multiply` method for more information. + + Warnings: the multiplication of two GMMs performed here is NOT the one that multiply the components + element-wise. For this one, have a look at `multiply_element_wise` method, or the `__and__` operator. + + Args: + other (np.array[O,O]): square rotation matrix + + Returns: + KMP: resulting KMP + """ + return self.multiply(rotation_matrix) + + def __rmul__(self, rotation_matrix): + return self.multiply(rotation_matrix) + + +# TESTS +if __name__ == "__main__": + import matplotlib.pyplot as plt + + # create data + + # plot data + + # create KMP model + + # fit/train a KMP + + # predict with KMP + + # plot prediction diff --git a/pyrobolearn/models/linear.py b/pyrobolearn/models/linear.py new file mode 100644 index 0000000..41e9053 --- /dev/null +++ b/pyrobolearn/models/linear.py @@ -0,0 +1,254 @@ +#!/usr/bin/env python +"""Provide the Linear Model. + +The linear model is a discriminative deterministic model given by: :math:`y = f(x) = w^T x`. +""" + +import copy + +try: + import cPickle as pickle +except ImportError as e: + import pickle + +import numpy as np +import torch +# from model import Model + +__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 Linear(object): + r"""Linear Model + + This class describes the linear parametric model: :math:`y = W x + b` where :math:`x` and :math:`y` are + respectively the input and output vectors, :math:`W` is the weight matrix, and :math:`b` is the bias/intercept. + """ + + def __init__(self, num_inputs, num_outputs, add_bias=True): + r""" + Initialize the affine/linear model described mathematically by: + + .. math:: y = W x + b + + Args: + num_inputs (int): dimension of the input + num_outputs (int): dimension of the output + add_bias (bool): if True, it will add a bias to the prediction + """ + super(Linear, self).__init__() + self.model = torch.nn.Linear(num_inputs, num_outputs, bias=add_bias) + self._num_parameters = len(self.get_vectorized_parameters()) + + ############## + # Properties # + ############## + + @property + def input_dims(self): + """Return the input dimension of the model""" + return self.model.weight.shape[1] + + @property + def output_dims(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]) + + @property + def output_shape(self): + """Return the output shape of the model""" + return tuple([self.output_dims]) + + @property + def num_parameters(self): + """Return the total number of parameters""" + return self._num_parameters + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other, deep=True): + """Return another copy of the linear model""" + if not isinstance(other, Linear): + 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(): + """The linear model is a parametric model""" + return True + + @staticmethod + def is_linear(): + """By definition, a linear model is linear""" + return True + + @staticmethod + def is_recurrent(): + """The linear model is not recurrent; current outputs do not depend on previous inputs""" + return False + + @staticmethod + def is_deterministic(): + """The linear model is a deterministic model""" + return True + + @staticmethod + def is_probabilistic(): + """The linear model is not a probabilistic model; it is a deterministic one""" + return False + + @staticmethod + def is_discriminative(): + """The linear model is a discriminative model which predicts :math:`y = Wx + b` where :math:`x` is the input, + and :math:`y` is the output""" + return True + + @staticmethod + def is_generative(): + """The linear model is not a generative model, and thus we can not sample from it""" + return False + + @staticmethod + def load(filename): + """ + Load a model from memory. + + Args: + filename (str): file that contains the model. + """ + return pickle.load(filename) + + ########### + # Methods # + ########### + + def parameters(self): + """Returns an iterator over the model parameters.""" + return self.model.parameters() + + def named_parameters(self): + """Returns 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_vectorized_parameters(self, to_numpy=True): + """Return a vectorized form (1 dimensional array) of the parameters.""" + parameters = self.parameters() + vector = torch.cat([parameter.reshape(-1) for parameter in parameters]) # np.concatenate = torch.cat + if to_numpy: + return vector.detach().numpy() + return vector + + def set_vectorized_parameters(self, vector): + """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 + + def predict(self, x, to_numpy=True): + r""" + Predict output vector :math:`y` given input vector :math:`x`, using the formula: :math:`y = W x + b` + + Args: + x (np.ndarray, torch.Tensor): input vector + to_numpy (bool): if True, return a np.array + + Returns: + np.ndarray, torch.Tensor: output vector + """ + # convert from numpy to pytorch if necessary + if isinstance(x, np.ndarray): + x = torch.from_numpy(x).float() + + # predict + y = self.model(x) + + # return the output and convert it if necessary + if to_numpy: + if y.requires_grad: + return y.detach().numpy() + return y.numpy() + return y + + def fit(self, X, Y): + r"""Train the linear model using Linear Regression. + + This is performed by minimizing the L2 loss with respect to the parameters: + + .. math:: \min_W || Y - XW ||^2 + + where :math:`X \in \mathcal{R}^{N \times (D_x+1)}` is the augmented input data matrix, and + :math:`Y \in \mathcal{R}^{N \times (D_y)}` is the output data matrix. + + The best set of weights can be obtained by solving in closed-loop the above optimization process. + The optimal solution is given by the pseudo-inverse: :math:`W^* = (X^\top X)^{-1} X^T Y`. + + Args: + X (np.array[N,Dx], torch.Tensor[N,Dx]): input data matrix. + Y (np.array[N,Dy], torch.Tensor[N,Dy]): output data matrix. + """ + pass + + def reset(self): + pass + + def __call__(self, x, to_numpy=True): + return self.predict(x, to_numpy=to_numpy) + + # def concatenate(self, other): + # """ + # Concatenate a linear model with another one. + # + # Args: + # other (Linear): the other linear model + # """ + # if not isinstance(other, Linear): + # raise TypeError("Expecting the other model to be also linear") + # # self.model.add_module() + + +# Tests +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)) + y = model(x) + print("Linear input: {}".format(x)) + print("Linear output: {}".format(y)) + + # test with pytorch + x = torch.from_numpy(x).float() + y = model(x, to_numpy=False) + print("Linear input: {}".format(x)) + print("Linear torch output: {}".format(y)) + y = model(x, to_numpy=True) + print("Linear numpy output: {}".format(y)) diff --git a/pyrobolearn/models/model.py b/pyrobolearn/models/model.py new file mode 100644 index 0000000..7f5fb96 --- /dev/null +++ b/pyrobolearn/models/model.py @@ -0,0 +1,373 @@ +#!/usr/bin/env python +"""Define the Model abstract class from which all learning models inherit from. + +Dependencies: None +""" + + +from abc import ABCMeta, abstractmethod + +__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 Model(object): + r"""(Learning Base) 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. + + 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 + 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 + """ + __metaclass__ = ABCMeta + + def __init__(self): + """ + Initialize the learning model. + """ + self._models = [] # TODO: should be a directed graph + + self._input_shape = None + self._output_shape = None + + ############## + # Properties # + ############## + + @property + def models(self): + return self._models + + @property + def input_shape(self): + return self._input_shape + + @property + def output_shape(self): + return self._output_shape + + ################## + # Static Methods # + ################## + + @staticmethod + def is_parametric(): + """ + Return True if the model is parametric. + + Returns: + bool: True if the model is parametric. + """ + raise NotImplementedError + + @staticmethod + def is_linear(): + """ + Return True if the model is linear (wrt the parameters). This can be for instance useful for some learning + algorithms (some only works on linear models). + + Returns: + bool: True if it is a linear model + """ + raise NotImplementedError + + @staticmethod + def is_recurrent(): + """ + Return True if the model is recurrent. This can be for instance useful for some learning algorithms which + change their behavior when they deal with recurrent learning models. + + Returns: + bool: True if it is a recurrent model. + """ + raise NotImplementedError + + @staticmethod + def is_deterministic(): + """ + Return True if the model is deterministic. + + Returns: + bool: True if the model is deterministic. + """ + return not Model.is_probabilistic() + + @staticmethod + def is_probabilistic(): # is_stochastic + """ + Return True if the model is probabilistic. + + Returns: + bool: True if the model is probabilistic. + """ + raise NotImplementedError + + @staticmethod + def is_discriminative(): + """ + Return True if the model is discriminative, that is, if the model estimates the conditional probability + :math:`p(y|x)`. + + Returns: + bool: True if the model is discriminative. + """ + raise NotImplementedError + + @staticmethod + def is_generative(): + """ + Return True if the model is generative, that is, if the model estimates the joint distribution of the input + and output :math:`p(x,y)`. A generative model allows to sample from it. + + Returns: + bool: True if the model is generative. + """ + raise NotImplementedError + + # TODO: isClassifier, isRegressive, isSequential + + ########### + # Methods # + ########### + + def has_models(self): + return len(self._models) > 0 + + def add_model(self, model): + 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(): + # TODO + pass + self._models.append(model) + + @abstractmethod + def _predict(self, x=None): + """ + Given a possible input, predict the output. The input doesn't always have to be given. For instance, + generative models generate data without any inputs. + """ + raise NotImplementedError + + def predict(self, x): + 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): + """ + Save the model in memory. + + Args: + filename (str): file to save the model in. + + Returns: + None + """ + raise NotImplementedError + + @abstractmethod + def load(self, filename): + """ + Load a model from memory. + + Args: + filename (str): file that contains the model. + + Returns: + None + """ + raise NotImplementedError + + def latex(self): + """ + Returns the latex equations that describe the learning model. + This function can also be called __repr__ and/or __str__. + """ + raise NotImplementedError + + def bibtex(self): + """ + Returns the references of a learning model in the bibtex format. + """ + raise NotImplementedError + + def concatenate(self, model): + """ + Concatenate sequentially two models. + """ + raise NotImplementedError + + ############# + # Operators # + ############# + + def __repr__(self): + if self.has_models(): + lst = [self.__class__.__name__ + '('] + for model in self._models: + lst.append('\t' + model.__repr__() + ',') + lst.append(')') + return '\n'.join(lst) + else: + return self.__class__.__name__ + + def __len__(self): + if self.has_models(): + return len(self._models) + + def __call__(self, *args, **kwargs): + return self.predict(*args, **kwargs) + + def __add__(self, other): + """ + Define how to combine two models together. + There are two ways to combine a model, sequentially (one after another in time), or in parallel. + """ + pass + + def __lshift__(self, other): + """ + Define how to concatenate/sequence two models inline. + + Examples: + nn = Model() + nn1 = MLP() + nn2 = MLP() + nn << nn1 << nn2 # same as nn << (nn1 >> nn2) + """ + pass + + def __rshift__(self, other): + """ + Define how to concatenate/sequence two models. The input of the first model is given to the output + of the second model. + """ + # if same model check `rshift()` fct in the corresponding class + # if different models, it is defined here + if isinstance(self, NN): + if isinstance(self, NN): + pass # TODO: call rshift() + elif isinstance(other, CPG): + pass + elif isinstance(other, DMP): + pass + else: + raise NotImplementedError("Do not know how to concatenate {} and {}.".format(type(self).__name__, + type(other).__name__)) + elif isinstance(self, GP): + if isinstance(other, GP): + pass # TODO: call rshift + elif isinstance(other, DMP): + pass + else: + raise NotImplementedError("Do not know how to concatenate {} and {}.".format(type(self).__name__, + type(other).__name__)) + + elif isinstance(self, GMM): + if isinstance(other, GMM): + pass + elif isinstance(other, DMP): + pass + else: + raise NotImplementedError("Do not know how to concatenate {} and {}.".format(type(self).__name__, + type(other).__name__)) + + else: + raise NotImplementedError("Do not know how to concatenate {} and {}.".format(type(self).__name__, + type(other).__name__)) + + def __floordiv__(self, other): + """ + Define how to recursively sequence two models. + """ + pass + + def __mod__(self, other): + """ + Define how much to wait when producing the output. + Basically, the output of a model is put into a FIFO queue of size :math:`s` which is specified by the given + argument. Thus at any time steps :math:`t`, the output actually produced is :math:`o_{t-s}`. + """ + if not isinstance(other, int): + raise TypeError("Expecting an integer") + + def __mul__(self, other): + pass + + def __getitem__(self, key): + pass + diff --git a/pyrobolearn/models/nn/__init__.py b/pyrobolearn/models/nn/__init__.py new file mode 100644 index 0000000..ebd2eff --- /dev/null +++ b/pyrobolearn/models/nn/__init__.py @@ -0,0 +1,24 @@ + +# import general neural network model +from dnn import NN + +# import multilayer perceptron model +from mlp import * + +# import NEAT model +from neat_model import NEATModel + +# import convolutional neural network +# from cnn import * + +# import recurrent neural network +# from rnn import * + +# import auto-encoder +# from ae import * + +# import variational auto-encoder +# from vae import * + +# import generative adversarial networks +# from gan import * diff --git a/pyrobolearn/models/nn/ae.py b/pyrobolearn/models/nn/ae.py new file mode 100644 index 0000000..55fd34c --- /dev/null +++ b/pyrobolearn/models/nn/ae.py @@ -0,0 +1,111 @@ +#!/usr/bin/env python +"""Define the Auto-Encoder (AE) learning model. + +This file provides the AE model; a parametric, generally non-linear, non-recurrent, discriminative, +and deterministic model. This model is a latent variable model which projects the input data into a latent lower +dimensional space through an encoder, and re-projects it to the original data space through the use of the decoder. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class AE(NN): + r"""Auto-Encoder + + References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + """ + + def __init__(self): + pass + + +class _AETorch(torch.nn.Module): + r"""Auto-Encoder written in Pytorch (that inherits from `torch.nn.Module`) + """ + + def __init__(self, layer_sizes=[], activation_fct=None, dropout=None, encoder=None, decoder=None): + super(_AETorch, self).__init__() + + if encoder is None and decoder is None: + # nb of layers (the input layer doesn't count) + self.num_layers = len(layer_sizes) - 1 + layers = [torch.nn.Linear(layer_sizes[i], layer_sizes[i+1]) for i in range(self.num_layers)] + + # check activation function and insert it after each linear layer + if activation_fct is not None: + if isinstance(activation_fct, str): + activation_fct = getattr(torch.nn, activation_fct)() + elif activation_fct.__module__ == 'torch.nn.modules.activation': + if inspect.isclass(activation_fct): + activation_fct = activation_fct() + else: + raise ValueError("activation_fct should be a string or belong to torch.nn.modules.activation") + + # add activation layer + for i in range(self.num_layers-1, 0, -1): + layers.insert(activation_fct) + + # check dropout + if dropout is not None: + if isinstance(dropout, float): + dropout = torch.nn.Dropout(dropout) + elif dropout.__module__ == 'torch.nn.modules.dropout': + raise ValueError("Dropout should be a float or belong to torch.nn.modules.dropout") + + # add dropout layer + for i in range(self.num_layers-1, 0, -2): + layers.insert(dropout) + + # Encoder + encoder = torch.nn.Sequential(*layers[:len(layers)//2]) + # Decoder + decoder = torch.nn.Sequential(*layers[len(layers)//2:]) + + self.encoder = encoder + self.decoder = decoder + + def forward(self, x): + x = self.encoder(x) + x = self.decoder(x) + return x + + def encode(self, x): + x = self.encoder(x) + return x + + def decode(self, x): + x = self.decoder(x) + return x + + +class AETorch(NNTorch): + r"""Auto-Encoder in Pytorch + """ + pass diff --git a/pyrobolearn/models/nn/cnn.py b/pyrobolearn/models/nn/cnn.py new file mode 100644 index 0000000..e7bb442 --- /dev/null +++ b/pyrobolearn/models/nn/cnn.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python +"""Define the Convolutional Neural Network (CNN) learning model. + +This file provides the CNN model; a parametric, generally non-linear, non-recurrent, discriminative, +and deterministic model. This model is convenient for data arrays/tensors that have cells that have a spatial +relationship between them. For instance, pictures are 2D or 3D arrays where each pixel is related with its neighbors. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class CNN(NN): + r"""Convolutional Neural Network + + Feed-forward CNN. + """ + pass + + +class CNNTorch(NNTorch): + r"""Convolutional Neural Network in PyTorch + """ + pass diff --git a/pyrobolearn/models/nn/dnn.py b/pyrobolearn/models/nn/dnn.py new file mode 100644 index 0000000..e3e7c2f --- /dev/null +++ b/pyrobolearn/models/nn/dnn.py @@ -0,0 +1,257 @@ +#!/usr/bin/env python +"""Define the Deep Neural Network (DNN) learning model. + +This file provides the DNN model; a parametric, generally non-linear, possibly recurrent, discriminative/generative, +and deterministic/probabilistic model. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +# from pyrobolearn.models import Model + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class NN(object): # Model + r"""Neural Network class + + This class describes the neural network model. It is basically a wrapper around deep learning frameworks such + as pytorch, Keras, tensorflow and others. This class is inherited by any other neural network classes, such as + convolution neural networks, recurrent neural networks, and so on. + + Note that we currently mainly focus on `PyTorch`. + + * PyTorch (https://pytorch.org/) + * PyTorch is ... PyTorch allows for dynamic ... + * torch.nn.Module: it represents the base class for all the neural networks / layers + * torch.nn.modules.loss(torch.nn.Module): it contains the definition of some popular losses + * torch.optim.Optimizer: it is the base class for all the optimizers + + Examples:: + + 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=...) + + References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ + nn.Module: https://pytorch.org/docs/master/_modules/torch/nn/modules/module.html#Module +# - nn.Sequential: https://pytorch.org/docs/master/_modules/torch/nn/modules/container.html#Sequential + """ + + def __init__(self, model, input_dims, output_dims, 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 + """ + super(NN, self).__init__() + + # check if given model is valid + # if model is not None: + # if isinstance(model, torch.nn.Module): + # self.framework = 'pytorch' + # elif isinstance(model, keras.models.Model): + # self.framework = 'keras' + # else: + # raise TypeError("Model should be an instance of torch.nn.Module") + + # set model (written in the specified framework) + self.model = model + self.input_dims = input_dims + self.output_dims = output_dims + + # TODO: infer the framework based on the model + self.framework = framework + + ############## + # Properties # + ############## + + @property + def model(self): + return self._model + + @model.setter + def model(self, model): + if model is not None: + 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_shape(self): # TODO + return self.input_dims + + @property + def output_shape(self): # TODO + return self.output_dims + + ################## + # Static Methods # + ################## + + @staticmethod + def is_parametric(): + """A neural network is a parametric model""" + return True + + @staticmethod + def is_linear(): # unless all layers are linear + """A neural network is in general non-linear, where non-linear activation functions are applied on each + layer output. If all the activation layers are linear, then the NN is linear.""" + return False + + @staticmethod + def is_recurrent(): # unless RNN + """Unless the neural network is a RNN, it is not recurrent.""" + return False + + @staticmethod + def is_probabilistic(): # unless variational methods (dropouts,...) + """The neural network is not a probabilistic model per se. However, it can be simulated by using dropouts, + or using a probabilistic distribution on the output of the last layer. For instance, the network can + output the mean and covariance matrices which parametrizes a Gaussian probabilistic distribution""" + return False # if False then it is deterministic + + @staticmethod + def is_discriminative(): + """A neural network is a discriminative model which given inputs predicts some outputs""" + return True + + @staticmethod + 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 False + + ########### + # 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 self.model.parameters() + + def get_params(self): + return list(self.parameters()) + + def get_hyperparams(self): + """ + Return the number of units per layer, the number of layers, and the type of layers. + """ + raise NotImplementedError + + def hyperparameters(self): + raise NotImplementedError + + ############# + # Operators # + ############# + + def __str__(self): + """ + Return string describing the NN model. + """ + if self.framework == 'pytorch': + return str(self.model) + elif self.framework == 'keras': + summary = [] + self.model.summary(print_fn=lambda s: summary.append(s)) + return '\n'.join(summary) + else: + raise NotImplementedError + + def __getitem__(self, key): + """ + Return the corresponding layer(s). + """ + return self.model[key] + + def __rshift__(self, other): + """ + Concatenate two NN models in sequence, and return the sequenced model. + Note that It doesn't modify the given models, but return a new one. + + Args: + other (NN): other NN model + + Returns: + NN: sequenced model + """ + # copy current model + model = copy.deepcopy(self.model) + + # concatenate the other model + for idx, item in enumerate(other.model, start=len(self.model)): + model.add_module(str(idx), item) + + # return the concatenation + return NN(model) + + +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 save(self, filename): + """ + Save the neural network to the specified file. + + Args: + filename (str): filename to save the neural network + """ + torch.save(self.model, filename) + + def load(self, filename): + """ + Load the neural network from the specified file. + + Args: + filename (str): filename from which to load the neural network + """ + self.model = torch.load(filename) + # check input and output dimensions + + def __str__(self): + """ + Return string describing the NN model. + """ + return str(self.model) diff --git a/pyrobolearn/models/nn/gan.py b/pyrobolearn/models/nn/gan.py new file mode 100644 index 0000000..c3aeee6 --- /dev/null +++ b/pyrobolearn/models/nn/gan.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python +"""Define the Generative Adversarial Network (GAN) learning model. + +This file provides the GAN model; a parametric, generally non-linear, non-recurrent, generative, and stochastic model. +This is a generative model which works in a game theory setting by having a generator and discriminator compete +between each other. The goal of the generator is to generate data samples that are similar to the provided dataset +and fool the discriminator. The goal of the discriminator is to discriminate the given samples by identifying the fake +ones from the true ones. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class GAN(NN): + r"""Generative Adversarial Network + + Type: generative model + + .. seealso:: Variational Auto-Encoders + + References: + [1] "NIPS: + [2] + """ + + def __init__(self): + pass + + @staticmethod + def isDiscriminative(): + """A neural network is a discriminative model which given inputs predicts some outputs""" + return True + + @staticmethod + def isGenerative(): # 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 + + +class GANTorch(NNTorch): + r"""Generative Adversarial Network in PyTorch + """ + pass diff --git a/pyrobolearn/models/nn/mlp.py b/pyrobolearn/models/nn/mlp.py new file mode 100644 index 0000000..439e86a --- /dev/null +++ b/pyrobolearn/models/nn/mlp.py @@ -0,0 +1,179 @@ +#!/usr/bin/env python +"""Define the Multi-Layer Perceptron (MLP) learning model. + +This file provides the MLP model; a parametric, generally non-linear, non-recurrent, discriminative, +and deterministic model. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN, NNTorch + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class MLP(NN): + r"""Multi-Layer Perceptron + + Feed-forward and fully-connected neural network, where linear layers are followed by non-linear activation + functions. + + .. math:: + + h_{l} = f_{l}(W_{l} h_{l-1} + b_{l}) + + where :math:`l \in [1,...,L]` with :math:`L` is the total number of layers, + :math:`W_{l}` and :math:`b_{l}` are the weight matrix and bias vector at layer :math:`l`, :math:`f_{l}` is + the nonlinear activation function, and :math:`h_{0} = x` and :math:`y = h_{L}` are the input and output vectors. + + The parameters of the neural networks are all the weight matrices and bias vectors. + """ + + def __init__(self, num_units=(), activation_fct='Linear', last_activation_fct=None, dropout_prob=None, + framework='pytorch'): + """ + Initialize a MLP network. + + 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 + 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. + dropout_prob (None, float, or list/tuple of float/None): dropout probability. + framework (str): specifies which framework we want to use between 'pytorch' and 'keras' (default: 'pytorch') + """ + + # check framework + framework = framework.lower() + if framework == 'pytorch': + model = MLPTorch(num_units, activation_fct, last_activation_fct, dropout_prob) + elif framework == 'keras': + model = MLPKeras(num_units, activation_fct, last_activation_fct, dropout_prob) + else: + 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) + + # rewrite methods + self.save = model.save + self.load = model.load + self.__str__ = model.__str__ + + +class MLPTorch(NNTorch): + r"""Multi-Layer Perceptron in PyTorch + + Feed-forward and fully-connected neural network, where linear layers are followed by non-linear activation + functions. + + .. math:: + + h_{l} = f_{l}(W_{l} h_{l-1} + b_{l}) + + where :math:`l \in [1,...,L]` with :math:`L` is the total number of layers, + :math:`W_{l}` and :math:`b_{l}` are the weight matrix and bias vector at layer :math:`l`, :math:`f_{l}` is + the nonlinear activation function, and :math:`h_{0} = x` and :math:`y = h_{L}` are the input and output vectors. + + The parameters of the neural networks are all the weight matrices and bias vectors. + """ + + def __init__(self, num_units=(), activation_fct='Linear', last_activation_fct=None, dropout_prob=None): + """ + Initialize a MLP network. + + 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 + 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. + dropout_prob (None, float, or list/tuple of float/None): dropout probability. + """ + # check number of units + if len(num_units) < 2: + raise ValueError("The num_units list/tuple needs to have at least the input and output layers") + + # set the dimensions of the input and output + self.input_dims = num_units[0] + self.output_dims = num_units[-1] + + # check for activation fcts + activations = dir(torch.nn.modules.activation) + activations = {act: act for act in activations} + activations.update({act.lower(): act for act in activations}) + + def check_activation(activation): + if activation is None or activation.lower() == 'linear': + activation = None + else: + if activation not in activations: + raise ValueError("The given activation function is not available") + activation = getattr(torch.nn, activations[activation]) + return activation + + activation_fct = check_activation(activation_fct) + last_activation_fct = check_activation(last_activation_fct) + + # check dropout + dropout_layer = None + if dropout_prob is not None: + dropout_layer = torch.nn.Dropout(dropout_prob) + + # build pytorch network + layers = [] + for i in range(len(num_units[:-2])): + # add linear layer + layer = torch.nn.Linear(num_units[i], num_units[i + 1]) + layers.append(layer) + + # add activation layer + if activation_fct is not None: + layers.append(activation_fct()) + + # add dropout layer + if dropout_layer is not None: + layers.append(dropout_layer) + + # last output layer + layers.append(torch.nn.Linear(num_units[-2], num_units[-1])) + if last_activation_fct is not None: + layers.append(last_activation_fct) + + # create nn model + model = torch.nn.Sequential(*layers) + + super(MLPTorch, self).__init__(model, input_dims=num_units[0], output_dims=num_units[-1]) + + +# Tests +if __name__ == '__main__': + + # create MLP network + 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 new file mode 100644 index 0000000..6fd4435 --- /dev/null +++ b/pyrobolearn/models/nn/neat_model.py @@ -0,0 +1,329 @@ +#!/usr/bin/env python +"""Define the NEAT model class. + +This uses the Neuro-Evolution through Augmenting topologies (NEAT) framework. It allows the evolution of not only the +parameters/weights but also the topological structure of neural networks. Note that the model associated with +this policy (i.e. the neural network) is tightly coupled with the algorithm that modifies it. +""" + +import numpy as np +import cPickle as pickle + +try: + import neat +except ImportError as e: + raise ImportError(e.__str__() + "\n HINT: you can install NEAT directly via 'pip install neat-python'.") + +# from pyrobolearn.models import Model + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class NEATModel(object): # Model): + r"""NEAT Model + + NEAT stands for "Neuro-Evolution through Augmenting Topologies" [1] and allows the evolution of not only the + parameters/weights but also the topological structure of neural networks. The model (i.e. the neural network) + is tightly coupled with the algorithm that modifies it. By the structure of the neural network, we mean + the type (i.e. forward or recurrent) and number of connection, as well as the type (i.e. using non-linearity + activation function) and number of nodes can change. + + This model works in a Reinforcement Learning setting, where exploration is carried out in the parameter and + hyper-parameter spaces of the neural network. + + Warnings: The associated algorithm is a little bit special and currently only works with the corresponding + learning model. + + References: + [1] "Evolving Neural Networks through Augmenting Topologies", Stanley et al., 2002 + [2] NEAT-Python + - documentation: https://neat-python.readthedocs.io/en/latest/index.html + - github repo: https://github.com/CodeReclaimers/neat-python + [3] PyTorch NEAT (built upon NEAT-Python): https://github.com/uber-research/PyTorch-NEAT + """ + + def __init__(self, num_inputs, num_outputs, num_hidden=0, activation_fct='relu', network_type='feedforward', + aggregation='sum', weights_limits=(-20, 20), bias_limits=(-20, 20)): + # super(NEATModel, self).__init__() + + if network_type != 'feedforward' and network_type != 'recurrent': + raise ValueError("Expecting the 'network_type' argument to be 'feedforward' or 'recurrent'. Received " + "instead {}".format('network_type')) + self.network_type = network_type + + # set config file + # more info about genome's config file: https://neat-python.readthedocs.io/en/latest/config_file.html + # more info about activation fct: https://neat-python.readthedocs.io/en/latest/activation.html + self.config_dict = {'[NEAT]': {'fitness_criterion': 'max', + 'pop_size': 10, + 'fitness_threshold': 100, + 'no_fitness_termination': True, + 'reset_on_extinction': True}, + '[DefaultSpeciesSet]': {'compatibility_threshold': 3.0}, + '[DefaultStagnation]': {'species_fitness_func': 'max', + 'max_stagnation': 15, + 'species_elitism': 2}, + '[DefaultReproduction]': {'elitism': 2, + 'survival_threshold': 0.2, + 'min_species_size': 2}, + '[DefaultGenome]': {'activation_default': activation_fct, + 'activation_mutate_rate': 0.0, + 'activation_options': activation_fct, + # node aggregation options + 'aggregation_default': aggregation, + 'aggregation_mutate_rate': 0.0, + 'aggregation_options': aggregation, + # node bias options + 'bias_init_mean': 0.0, + 'bias_init_stdev': 1.0, + 'bias_init_type': 'gaussian', + 'bias_max_value': bias_limits[1], + 'bias_min_value': bias_limits[0], + 'bias_mutate_power': 0.5, + 'bias_mutate_rate': 0.7, + 'bias_replace_rate': 0.1, + # genome compatibility options + 'compatibility_disjoint_coefficient': 1.0, + 'compatibility_weight_coefficient': 0.5, + # connection add/remove rates + 'conn_add_prob': 0.5, + 'conn_delete_prob': 0.5, + # connection enable options + 'enabled_default': True, + 'enabled_mutate_rate': 0.01, + 'feed_forward': self.network_type == 'feedforward', + 'initial_connection': 'full_nodirect', + # node add/remove rates + 'node_add_prob': 0.1, + 'node_delete_prob': 0.1, + # network parameters + 'num_hidden': num_hidden, + 'num_inputs': num_inputs, + 'num_outputs': num_outputs, + # node response options + 'response_init_mean': 1.0, + 'response_init_stdev': 0.0, + 'response_max_value': 30.0, + 'response_min_value': -30.0, + 'response_mutate_power': 0.0, + 'response_mutate_rate': 0.0, + 'response_replace_rate': 0.0, + # connection weight options + 'weight_init_mean': 0.0, + 'weight_init_stdev': 1.0, + 'weight_max_value': weights_limits[1], + 'weight_min_value': weights_limits[0], + 'weight_mutate_power': 0.5, + 'weight_mutate_rate': 0.8, + 'weight_replace_rate': 0.1}} + + # create config + config_file = self._create_config_file() + self._config = neat.Config(neat.DefaultGenome, neat.DefaultReproduction, neat.DefaultSpeciesSet, + neat.DefaultStagnation, config_file) + + # create initial population + self.population = neat.Population(self.config) + + # set initial genome (first genome from the population) + self._genome = self.population.population[1] + + # create network + self.model = self.set_network(self.genome, self.config) + + ############## + # Properties # + ############## + + @property + def input_dims(self): + """Return the input dimension of the model""" + return len(self.model.input_nodes) + + @property + def output_dims(self): + """Return the output dimension of the model""" + return len(self.model.output_nodes) + + @property + def input_shape(self): + """Return the input shape of the model""" + return tuple([self.input_dims]) + + @property + def output_shape(self): + """Return the output shape of the model""" + return tuple([self.output_dims]) + + @property + def config(self): + """Return the config object""" + return self._config + + @config.setter + def config(self, config): + """Set the config file (str) or object.""" + if not isinstance(config, neat.config.Config): + raise TypeError("Expecting genome to be an instance of neat.config.Config.") + self._config = config + # create population + self.population = neat.Population(self._config) + + @property + def genome(self): + return self._genome + + @genome.setter + def genome(self, genome): + if not isinstance(genome, neat.genome.DefaultGenome): + raise TypeError("Expecting genome to be an instance of neat.genome.DefaultGenome type") + self._genome = genome + + # create network + self.model = self.set_network(self._genome, self.config) + + @property + def network(self): + return self.model + + ################## + # Static Methods # + ################## + + @staticmethod + def is_parametric(): + return True + + @staticmethod + def is_linear(): + return False + + @staticmethod + def is_recurrent(): + return True + + @staticmethod + def is_probabilistic(): + return False + + @staticmethod + def is_discriminative(): + return True + + @staticmethod + def is_generative(): + return False + + @staticmethod + def load(filename): + """ + Load a model from memory. + + Args: + filename (str): file that contains the model. + """ + return pickle.load(open(filename, 'rb')) + + ########### + # Methods # + ########### + + def _create_config_str(self, config_dict=None): + """Create string describing the config file from a config dictionary""" + if config_dict is None: + config_dict = self.config_dict + + # create config file + config = [] + for section, parameters in config_dict.items(): + config.append(section) + for key, value in parameters.items(): + config.append(key + ' = ' + str(value)) + config.append('') + + # return string describing the config file + return '\n'.join(config) + + def _create_config_file(self, config_dict=None): + """Create config file from a config dictionary""" + config = self._create_config_str(config_dict) + filename = 'config.txt' + + # create config file + with open(filename, 'w') as f: + f.write(config) + + # return path to the config file + return filename + + def set_network(self, genome=None, config=None): + # check arguments + if genome is None: + genome = self.genome + if config is None: + config = self.config + + # Create the neural network + if self.network_type == 'feedforward': + self.model = neat.nn.FeedForwardNetwork.create(genome, config) + elif self.network_type == 'recurrent': + self.model = neat.nn.RecurrentNetwork.create(genome, config) + else: + raise TypeError('Choose between feedforward or recurrent or implement your own type of NN.') + + return self.model + + def update_config(self, config): + # update config (dict) + if isinstance(config, dict): + self.config_dict.update(config) + config_file = self._create_config_file() + self._config = neat.Config(neat.DefaultGenome, neat.DefaultReproduction, neat.DefaultSpeciesSet, + neat.DefaultStagnation, config_file) + elif isinstance(config, neat.Config): + self._config = config + + # create new population + self.population = neat.Population(self.config) + + # set new genome (first genome from the population) + self._genome = self.population.population[1] + + # 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): + return self.model.activate(x) + + def save(self, filename): + """ + Save the model in memory. + + Args: + filename (str): file to save the model in. + """ + pickle.dump(self, open(filename, 'wb')) diff --git a/pyrobolearn/models/nn/rcnn.py b/pyrobolearn/models/nn/rcnn.py new file mode 100644 index 0000000..da54863 --- /dev/null +++ b/pyrobolearn/models/nn/rcnn.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python +"""Define the Recurrent Convolutional Neural Network (RCNN) learning model. + +This file provides the RCNN model; a parametric, generally non-linear, recurrent, discriminative, +and deterministic model. This model is convenient for sequential data arrays/tensors where at each instant, the cells +in the data arrays/tensors have a spatial relationship between them. For instance, this model can be used with videos +where there is a temporal and spatial relationship between the pixels. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RCNN(NN): + r"""Recurrent CNN + """ + pass + +class RCNN(NNTorch): + r"""Recurrent CNN in PyTorch + """ + pass diff --git a/pyrobolearn/models/nn/rnn.py b/pyrobolearn/models/nn/rnn.py new file mode 100644 index 0000000..e73e339 --- /dev/null +++ b/pyrobolearn/models/nn/rnn.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +"""Define the Recurrent Neural Network (RNN) learning model. + +This file provides the RNN model; a parametric, generally non-linear, recurrent, discriminative, +and deterministic model. This model is convenient for sequential data. For instance, they can be used with language, +where each word in a sentence is conditioned on the previous words. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class RNN(NN): + r"""Recurrent Neural Network + """ + pass + +class RNNTorch(NNTorch): + r"""Recurrent Neural Network in PyTorch + """ + pass diff --git a/pyrobolearn/models/nn/vae.py b/pyrobolearn/models/nn/vae.py new file mode 100644 index 0000000..f9bde3c --- /dev/null +++ b/pyrobolearn/models/nn/vae.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +"""Define the Variational Auto-Encoder (VAE) learning model. + +This file provides the VAE model; a parametric, generally non-linear, non-recurrent, generative, and stochastic model. +This model is a generative latent variable model which projects the input data into a latent lower +dimensional space through an encoder, and re-projects it to the original data space through the use of the decoder. + +We decided to use the `pytorch` framework because of its popularity in the research community field, flexibility, +similarity with numpy (but with automatic differentiation: autograd), GPU capabilities, and more Pythonic approach. +While we thought about using other frameworks (such as Keras, Tensorflow, and others) as well, it would have +unnecessarily complexify the whole framework, as these frameworks would not only influence the learning models, +but also the losses, optimizers, and other modules. While we could have written some interfaces that makes the bridge +between these various frameworks and ours, we came to the conclusion that this would take a considerable amount of +efforts and time that we do not have for the moment. + +References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] PyTorch: https://pytorch.org/ +""" + +import copy +import inspect +import numpy as np +import torch + +from dnn import NN + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2018, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "(c) Brian Delhaisse" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class VAE(NN): + r"""Variational AutoEncoder + + Type: generative model + + .. seealso:: Generative Adversarial Networks + + References: + [1] "Deep Learning" (http://www.deeplearningbook.org/), Goodfellow et al., 2016 + [2] "Tutorial on Variational Autoencoder" + """ + def __init__(self, layer_sizes, activation_fct=None, dropout=None): + self.encoder = None + self.decoder = None + + @staticmethod + def isDiscriminative(): + """A neural network is a discriminative model which given inputs predicts some outputs""" + return True + + @staticmethod + def isGenerative(): # 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 + + ########### + # Methods # + ########### + + def sample(self, size=None, seed=None): + """Sample from the VAE""" + pass + + +class VAETorch(NNTorch): + r"""Variational AutoEncoder in PyTorch + """ + pass diff --git a/pyrobolearn/models/pca.py b/pyrobolearn/models/pca.py new file mode 100644 index 0000000..286e2e9 --- /dev/null +++ b/pyrobolearn/models/pca.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python +"""Define the PCA model. +""" + +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 PCA(object): + r"""Principal Component Analysis (PCA) + + This class describes PCA; a non-parametric, linear model which possesses the 3 following properties [1]: + * linearity + * orthogonality + * high signal-noise ratio + + PCA can be achieved by performing Singular Value Decomposition (SVD) on the data, or performing an + eigendecomposition on the covariance data matrix. + + Assuming the mean-centered data matrix is given by :math:`X \in \mathbb{R}^{N \times M}`, applying SVD on it + gives us: + .. math:: X = USV^T, + where :math:`U \in \mathbb{R}^{N \times N}` is an orthogonal matrix where its columns represent the eigenvectors + of :math:`XX^T` also known as the left-singular vectors of :math:`X`, :math:`S \in \mathbb{R}^{N \times M}` + contains the singular values ordered by descending order, and :math:`V \in \mathbb{R}^{M \times M}` is an + orthogonal matrix in which its columns represent the eigenvectors of :math:`X^TX` also known as the right-singular + vectors of :math:`X`. The columns of :math:`V` form a basis spanning :math:`\mathbb{R}^M`. + + Applying eigendecomposition on the covariance matrix :math:`C_X \sim X^TX` gives us: + .. math:: X^TX = QLQ^T. + + While applying SVD on :math:`X` and then computing the covariance gives us: + .. math:: X^TX = (USV^T)^T (USV^T) = VSSV^T = VS^2V^T + + Thus, :math:`Q=V` (i.e. same orthogonal matrix containing the evecs) and :math:`L=S^2` (that is, the eigenvalues + are the square of the singular values). Note that the covariance :math:`C_X` is formally defined as + :math:`\frac{1}{(N-1)} X^TX` where :math:`N` is the number of data points, and not :math:`X^TX`, then + :math:`L=\frac{S^2}{(N-1)}`. + + PCA can be formulated as an optimization process, which consists to find the dimensions that maximize + the projected variance. Specifically, this can be written as: + + .. math:: + max_{u_i} ||Xu_i||^2 & \mbox{ subj. to } u_i^Tu_i = 1; u_j^Tu_i = 0 \\ + max_{u_i} (Xu_i)^T(Xu_i) & \mbox{ subj. to } u_i^Tu_i = 1; u_j^Tu_i = 0 \\ + max_{u_i} u_i^TX^TXu_i & \mbox{ subj. to } u_i^Tu_i = 1; u_j^Tu_i = 0 \\ + + :math:`\forall i, \forall j < i`. The solution is given by the column vectors of the matrix :math:`V`, that is, + the eigenvectors, while the obtained values during the maximization process are given by the eigenvalues. + This is why PCA can be performed by applying SVD (on the data matrix) or Eigendecomposition (on the covariance + matrix). + + Complexity: + * Spatial complexity: O(NM) + * Time complexity: O(min(NM^2, MN^2)) + + References: + [1] "A Tutorial on Principal Component Analysis", Shlens, 2014 + """ + def __init__(self, X=None, normalize_data=False): + if X is not None: + self.train(X, normalize=normalize_data) + + self.evals, self.evecs = None, None + + ############## + # Properties # + ############## + + @property + def eigenvalues(self): # alias to evals + return self.evals + + @property + def eigenvectors(self): # alias to evecs + return self.evecs + + ################## + # Static Methods # + ################## + + # TODO: think if PCA can be considered as a model + + @staticmethod + def isParametric(): + """PCA is a non-parametric approach""" + return False + + @staticmethod + def isLinear(): + """PCA does not have parameters, but it is a linear dimensionality reduction algo""" + return True + + @staticmethod + def isRecurrent(): + """PCA is not recurrent""" + return False + + @staticmethod + def isLatent(): + """PCA gives a latent model""" + return True + + @staticmethod + def isProbabilistic(): + """PCA is not a probabilistic approach but a deterministic one""" + return False + + @staticmethod + def isDiscriminative(): + """PCA is a discriminative model, which projects the given data into a lower space""" + return True + + @staticmethod + def isGenerative(): + """PCA is not a generative model from which you can sample from it""" + return False + + ########### + # Methods # + ########### + + + def parameters(self): + raise RuntimeError("PCA doesn't have any parameters.") + + def getParams(self): + raise RuntimeError("PCA doesn't have any parameters.") + + def getHyperparams(self): + pass + + def train(self, X, normalize=False, copy=True): + """ + Compute PCA on the given data. This method will center the data. + + Args: + X (float[N,D]): the matrix to apply PCA on (with shape(X)=NxD, where `N` is the nb of samples, and + `D` is the dimensionality of a sample). + normalize (bool): if True, it will normalize the data using the std dev. PCA will then be applied on + the correlation matrix instead of the covariance matrix. + copy (bool): if True, it will first copy the data before centering it, and possibly normalizing it. + + Return: + float[D]: the sorted eigenvalues + float[D]: the sorted eigenvectors + """ + if copy: + X = np.copy(X) + + # 1. Center the data + mean = X.mean(axis=0) + X -= mean + N = X.shape[0] + + # Normalize using the std dev + if normalize: + X /= X.std(axis=0) + + # 2. Compute the covariance/correlation matrix + CovX = 1./(N-1) * X.T.dot(X) # TxT (same as np.cov(X, rowvar=False))) + + # 3. Compute the eigenvectors of this covariance matrix + # np.linalg.eigh is more efficient than np.linalg.eig for symmetric matrix + evals, evecs = np.linalg.eigh(CovX) + + # 4. Sort the eigenvalues (in decreasing order) and eigenvectors + idx = np.argsort(evals)[::-1] + evals, evecs = evals[idx], evecs[:,idx] + + # save values + self.evals = evals + self.evecs = evecs + + return evals, evecs + + def predict(self, x): + # TODO + pass + + +class RecursivePCA(PCA): + r"""Recursive PCA + + Given a new data point, the + + Assume the covariance matrix is given by :math:`C`, and the mean by :math:`m`, then the new covariance matrix + accounting for the new data point is given by: + + .. math:: C' = C + \frac{N}{N+1} (m m^T - m x'^T - x' m^T + x'x'^T) + + where :math:`C` can be reconstructed from the eigenvalues and eigenvectors using the eigendecomposition: + + .. math:: C = V \Lambda V^T + + and the new mean is given by: + + .. math:: + + Finally, the total number of data points N is updated. This reduces the the spatial and time complexities to + O(T^2) and O(T^3), respectively. + + Time complexity: + * O(min(NT^2, TN^2)) + """ + + def __init__(self, X=None, normalize_data=False): + super(RecursivePCA, self).__init__(X, normalize_data) + + def train_recursive(self, X): + if self.X is None: + # apply std PCA + self.X = self.train(X) + self.mean = np.mean(X, axis=0).reshape(-1, 1) + self.cov = np.cov(X, rowvar=False) + self.N = len(X) + + else: # recursive + for x in X: + x = x.reshape(-1, 1) + Y = self.mean.dot(x.T) + self.cov = self.cov + self.N / (self.N + 1.) * (self.mean.dot(self.mean.T) - Y - Y.T + + self.mean.dot(self.mean.T)) + self.mean = self.N / (self.N + 1.) * self.mean + 1 / (N+1) * x + + self.N += 1 + + +class HierarchicalPCA(PCA): + r"""Hierarchical PCA + + Assume :math:`X = [X_1, X_2] \in \mathbb{R}^{N \times 2D}`, where :math:`X_1, X_2 \in \mathbb{R \times D}`. + + We first decompose the covariance matrix :math:`C` of :math:`X` in terms of :math:`X_1` and :math:`X_2`, and + apply SVD on each term as follows: + + .. math:: + + C = X^T X + = \left[ \begin{array}{cc} + X_1^T X_1 & X_1^T X_2 \\ + X_2^T X_1 & X_2^T X_2 + \end{array} \right] + = \left[ \begin{array}{cc} + V_1 \Lambda_1 V_1^T & V_1 \Sigma_1 U_1^T U_2 \Sigma_2 V_2^T \\ + V_2 \Sigma_2 U_2^T U_1 \Sigma_1 V_1^T & V_2 \Lambda_2 V_2^T + \end{array} \right] + + Similarly, we apply the eigendecomposition on C which results in: + + .. math:: + + C = X^T X = V \Lambda V^T + = \left[ \begin{array}{cc} V_{11} & V_{12} \\ V_{21} & V_{22} \end{array} \right] + \left[ \begin{array}{cc} \Lambda_{11} & 0 \\ 0 & \Lambda_{22} \end{array} \right] + \left[ \begin{array}{cc} V_{11}^T & V_{21}^T \\ V_{12}^T & V_{22}^T \end{array} \right] + = \left[ \begin{array}{cc} + V_{11} \Lambda_{11} V_{11}^T + V_{12} \Lambda_{22} V_{12}^T + & V_{11} \Lambda_{11} V_{21}^T + V_{12} \Lambda_{22} V_{22}^T \\ + V_{21} \Lambda_{11} V_{11}^T + V_{22} \Lambda_{22} V_{12}^T + & V_{21} \Lambda_{11} V_{21}^T + V_{22} \Lambda_{22} V_{22}^T + \end{array} \right] + + """ + pass \ No newline at end of file diff --git a/pyrobolearn/models/polynomial.py b/pyrobolearn/models/polynomial.py new file mode 100644 index 0000000..7bcda69 --- /dev/null +++ b/pyrobolearn/models/polynomial.py @@ -0,0 +1,292 @@ +#!/usr/bin/env python +"""Define the polynomial learning model. + +The polynomial model is a discriminative deterministic model given by: :math:`y = f(x) = W \phi(x)`, where +:math:`\phi` is a function that returns a transformed input vector (possibly of higher dimension). +""" + +import copy +# import inspect +import collections +import numpy as np +import torch + +__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 Polynomial(object): + r"""Polynomial model + + The polynomial model is a discriminative deterministic model expressed mathematically as + :math:`y = f(x) = W \phi(x)`, where :math:`x` is the input vector, :math:`y` is the output vector, :math:`W` + is the weight matrix, and :math:`\phi` is the polynomial function which returns the transformed input vector. + This transformed input vector is often of higher dimension, based on the idea that if it is not linear with + respect to the parameters in the current space, it might be in a higher dimensional space. + """ + + def __init__(self, num_inputs, num_outputs, polynomial_fct): + """ + Initialize the polynomial model: :math:`y = W \phi(x)` + + 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. + """ + self.phi = polynomial_fct + num_inputs = num_inputs * self.phi.size + self.model = torch.nn.Linear(num_inputs, num_outputs, bias=False) + self._num_parameters = len(self.get_vectorized_parameters()) + + ############## + # Properties # + ############## + + @property + def phi(self): + """Return the polynomial function""" + return self._phi + + @phi.setter + def phi(self, fct): + """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 + + @property + def polynomial_function(self): + """Return the polynomial function""" + return self._phi + + @polynomial_function.setter + def polynomial_function(self, fct): + """Set the polynomial function""" + self.phi = fct + + @property + def input_dims(self): + """Return the input dimension of the model""" + return self.model.weight.shape[1] + + @property + def output_dims(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]) + + @property + def output_shape(self): + """Return the output shape of the model""" + return tuple([self.output_dims]) + + @property + def num_parameters(self): + """Return the total number of parameters""" + return self._num_parameters + + ################## + # Static methods # + ################## + + @staticmethod + def copy(other): + """Return another copy of the polynomial model""" + if not isinstance(other, Polynomial): + raise TypeError("Trying to copy an object which is not a Polynomial model") + return copy.copy(other) + + @staticmethod + def is_parametric(): + """The polynomial model is a parametric model""" + return True + + @staticmethod + def is_linear(): + """The polynomial model is linear with respect to its parameters""" + return True + + @staticmethod + def is_recurrent(): + """The polynomial model is not recurrent; current outputs do not depend on previous inputs""" + return False + + @staticmethod + def is_deterministic(): + """The polynomial model is a deterministic model""" + return True + + @staticmethod + def is_probabilistic(): # same as is_stochastic() + """The polynomial model is not a probabilistic model; it is a deterministic one""" + return False + + @staticmethod + def is_discriminative(): + """The polynomial model is a discriminative model.""" + return True + + @staticmethod + def is_generative(): + """The polynomial model is not a generative model, and thus we can not sample from it""" + return False + + ########### + # Methods # + ########### + + def parameters(self): + """Returns an iterator over the model parameters.""" + return self.model.parameters() + + def named_parameters(self): + """Returns 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_vectorized_parameters(self, to_numpy=True): + """Return a vectorized form (1 dimensional array) of the parameters.""" + parameters = self.parameters() + vector = torch.cat([parameter.reshape(-1) for parameter in parameters]) # np.concatenate = torch.cat + if to_numpy: + return vector.detach().numpy() + return vector + + def set_vectorized_parameters(self, vector): + """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 + + def predict(self, x, to_numpy=True): + """ + Predict output vector :math:`y` given input vector :math:`x`, using the formula: :math:`y = W \phi(x)`. + + Args: + x (np.ndarray, torch.Tensor): input vector + to_numpy (bool): if True, return a np.array + + Returns: + np.ndarray, torch.Tensor: output vector + """ + # convert from numpy to pytorch if necessary + if isinstance(x, np.ndarray): + x = torch.from_numpy(x).float() + + # predict the output + y = self.model(self.phi(x)) + + # return the output and convert it if necessary + if to_numpy: + if y.requires_grad: + return y.detach().numpy() + return y.numpy() + return y + + def __call__(self, x, to_numpy=True): + 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)) + y = model(x) + print("Polynomial input: {}".format(x)) + print("Polynomial output: {}".format(y)) + + # test with pytorch + x = torch.from_numpy(x).float() + y = model(x, to_numpy=False) + print("Polynomial input: {}".format(x)) + print("Polynomial torch output: {}".format(y)) + y = model(x, to_numpy=True) + print("Polynomial numpy output: {}".format(y)) diff --git a/pyrobolearn/models/promp.py b/pyrobolearn/models/promp.py new file mode 100755 index 0000000..73d6981 --- /dev/null +++ b/pyrobolearn/models/promp.py @@ -0,0 +1,2206 @@ +#!/usr/bin/env python +"""Define the Probabilistic Movement Primitive class + +This file defines the Probabilistic Movement Primitive (ProMP) model, and use the Gaussian distribution defined +in `gaussian.py` +""" + + +from abc import ABCMeta, abstractmethod +import numpy as np +from scipy.linalg import block_diag +import scipy.interpolate + +from model import Model +from gaussian import Gaussian + + +__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 + """ + pass + + +class LinearCS(CS): + r"""Linear Canonical System. + + A canonical system (CS) allows to modulate temporarily the ProMP, that is, it provides the phase that drives + the ProMP [1]. + The phase variable was introduced to avoid an explicit dependency with time in the ProMP 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] "Probabilistic Movement Primitives", Paraschos et al., 2013 + [2] "Using Probabilistic Movement Primitives in Robotics", Paraschos et al., 2018 + """ + + __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. + T (float): total time for the movement (period) + """ + # 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.) + + # initial time + self.t0 = 0. + + # slope + if self.T <= 0: + raise ValueError("Expecting the period T to be bigger than 0") + self.slope = 1. / self.T + + # reset the phase variable + self.reset() + + ############## + # Properties # + ############## + + @property + def initial_phase(self): + """Return the initial phase""" + return self.t0 + + @property + def final_phase(self): + """Return the final phase""" + return self.T + + @property + def num_timesteps(self): + """Return the number of timesteps""" + return self.timesteps + + ########### + # Methods # + ########### + + def reset(self): + """ + Reset the phase variable to its initial phase. + """ + self.s = self.t0 + + def step(self, tau=1.0, error_coupling=1.0): + """ + Perform a step using Euler's method; increment the phase by a small amount. + + Args: + tau (float): speed. Increase tau to make the system faster, and decrease it to make it slower. + + Returns: + float: current phase + """ + s = self.s + self.s += tau * self.slope * self.dt + + # return previous 's' such that it starts from t0 + return s + + # aliases + predict = step + forward = step + + def grad(self, t=None): + """ + Compute the gradient of the phase variable with respect to time, i.e. :math:`ds/dt(t)`. + + Args: + t (float, None): time variable + + Returns: + float: gradient evaluated at the given time + """ + return self.slope + + def rollout(self, tau=1.0, error_coupling=1.0): + """ + Generate phase variable in an open loop fashion from the initial to the final phase. + + Args: + tau (float): Increase tau to make the system faster, and decrease it to make it slower. + error_coupling (float): slow down if the error is > 1 + + Returns: + np.array[T]: value of phase variable at each time step + """ + 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 + + + +#################### Basis Functions ########################## + +class BasisFunction(object): + r"""Basis Function + + The choice of basis function depends on the type of movement the user which to model; a discrete (aka stroke-based) + or rhythmic movement. + """ + __metaclass__ = ABCMeta + + def __init__(self): + pass + + ########### + # Methods # + ########### + + def compute(self, s): + """ + Predict the value of the basis function given the phase variable :math:`s` + + Args: + s (float): phase value + + Returns: + float: value of the basis function evaluated at the given phase + """ + raise NotImplementedError + + # # aliases + # predict = compute + # forward = compute + + @abstractmethod + def grad(self, s): + """ + Compute the gradient of the basis function with respect to the phase variable :math:`s`, evaluated at + the given phase. + + Args: + s (float): phase value + + Returns: + float: gradient evaluated at the given phase + """ + raise NotImplementedError + + # @abstractmethod + # def grad_t(self, s): # TODO: use automatic differentiation + # """ + # Compute the gradient of the basis function with respect to the time variable :math:`t`, evaluated at + # the given phase :math:`s(t)`. + # + # Args: + # s (float): phase value s(t) + # + # Returns: + # float: gradient evaluated at the given phase s(t) + # """ + # raise NotImplementedError + + ############# + # Operators # + ############# + + def __call__(self, s): + """Predict value of basis function given phase""" + return self.compute(s) + +# alias +BF = BasisFunction + +class GaussianBF(BF): + r"""Gaussian Basis Function + + This basis function is given by the formula: + + .. math:: b(s) = \exp \left( - \frac{1}{2 h} (s - c)^2 \right) + + where :math:`c` is the center, and :math:`h` is the width of the basis. + + This is often used for discrete movement primitives. + """ + + def __init__(self, center=0., width=1.): + """Initialize basis function + + Args: + center (float, np.ndarray): center of the distribution + width (float, np.ndarray): width of the distribution + """ + super(GaussianBF, self).__init__() + + if isinstance(center, np.ndarray): pass + + self.c = center + if width <= 0: + raise ValueError("Invalid `width` argument: the width of the basis has to be strictly positive") + self.h = width + + def compute(self, s): + """ + Predict the value of the basis function given the phase variable :math:`s`, given by: + + .. math:: b(s) = \exp \left( - \frac{1}{2 h} (s - c)^2 \right) + + where :math:`c` is the center, and :math:`h` is the width of the basis. + + Args: + s (float): phase value + + Returns: + float: value of the basis function evaluated at the given phase + """ + if isinstance(s, np.ndarray): + s = s[:, None] + return np.exp( - 0.5 / self.h * (s - self.c)**2 ) + + def grad(self, s): + """ + Return the gradient of the basis function :math:`b(s)` with respect to the phase variable :math:`s`, + evaluated at the given phase. + + For the Gaussian basis function, this results in: + + .. math:: + + \frac{d b(s)}{ds} = - b(s) \frac{(s - c)}{h} + + Args: + s (float): phase value + + Returns: + float: gradient evaluated at the given phase + """ + s1 = s[:, None] if isinstance(s, np.ndarray) else s + return - self(s) * (s1 - self.c) / self.h + + +# aliases +GBF = GaussianBF + + +class VonMisesBF(BF): + r"""Von-Mises Basis Function + + This basis function is given by the formula: + + .. math:: b(s) = \exp \left( \frac{ \cos( 2\pi (s - c)) }{h} \right) + + where :math:`c` is the center, and :math:`h` is the width of the basis. + + This is often used for rhythmic movement primitives. + """ + + def __init__(self, center=0, width=1.): + """Initialize basis function + + Args: + center (float, np.ndarray): center of the basis fct + width (float, np.ndarray): width of the distribution + """ + super(VonMisesBF, self).__init__() + self.c = center + if width <= 0: + raise ValueError("Invalid `width` argument: the width of the basis has to be strictly positive") + self.h = width + + def compute(self, s): + """ + Predict the value of the basis function given the phase variable :math:`s`, given by: + + .. math:: b(s) = \exp \left( \frac{ \cos( 2\pi (s - c)) }{h} \right) + + where :math:`c` is the center, and :math:`h` is the width of the basis. + + Args: + s (float): phase value + + Returns: + float: value of the basis function evaluated at the given phase + """ + if isinstance(s, np.ndarray): + s = s[:, None] + return np.exp(np.cos(2*np.pi * (s - self.c)) / self.h) + + def grad(self, s): + """ + Return the gradient of the basis function :math:`b(s)` with respect to the phase variable :math:`s`, + evaluated at the given phase. + + For the Von-Mises basis function, this results in: + + .. math:: + + \frac{d b(s)}{ds} = - b(s) 2\pi \frac{ \sin(2 \pi (s - c)) }{ h } + + Args: + s (float): phase value + + Returns: + float: gradient evaluated at the given phase + """ + s1 = s[:, None] if isinstance(s, np.ndarray) else s + return - 2 * np.pi * self(s) * np.sin(2 * np.pi * (s1 - self.c)) / self.h + + +# aliases +CBF = VonMisesBF # Circular Basis Function + + +class Matrix(object): + """callable matrix""" + def __call__(self, s): + raise NotImplementedError + + +class BasisMatrix(Matrix): + r"""Basis matrix + + The basis matrix contains the basis functions, and the derivative of the basis functions. + + This is given by: + + .. math:: \Phi(s) = [\phi(s) \dot{\phi}(s)] \in \mathcal{R}^{M \times 2} + + where :math:`s` is the phase variable, and :math:`M` is the total number of components. + """ + + def __init__(self, matrix): + """ + Initialize the basis matrix. + + Args: + matrix (np.array[M,D]): 2D matrix containing callable functions + """ + self.matrix = matrix + + # get shape + self._shape = self(0.).shape + + @property + def shape(self): + """Return the shape of the matrix""" + return self._shape + + @property + def num_basis(self): + """return the number of basis function""" + return self._shape[0] + + def evaluate(self, s): + """ + Return matrix evaluated at the given phase. + + Args: + s (float, np.array[T]): phase value(s) + + Returns: + np.array: array of shape Mx2, or MxTx2 + """ + # matrix = np.array([[fct(s) for fct in row] + # for row in self.matrix]) + matrix = np.array([fct(s) for fct in self.matrix]).T + return matrix + + def __call__(self, s): + """ + Return matrix evaluated at the given phase. + + Args: + s (float): phase value + + Returns: + np.array: array of shape 2xM, or Tx2xM + """ + return self.evaluate(s) + + +class GaussianBM(BasisMatrix): + r"""Gaussian Basis Matrix + + Matrix containing Gaussian basis functions. + """ + + def __init__(self, cs, num_basis, basis_width=1.): + """ + Initialize the Gaussian basis matrix. + + Args: + cs (CS): canonical system + num_basis (int): number of basis functions + basis_width (float): width of the basis functions + """ + + # create derivative of basis function wrt to time + def dphi_t(cs, phi): + def step(s): + return phi.grad(s) * cs.grad() + return step + + # distribute centers for the Gaussian basis functions + # the centers are placed uniformly between [-2*width, 1+2*width] + if num_basis == 1: + centers = (1.+4*basis_width)/2. + else: + centers = np.linspace(-2*basis_width, 1+2*basis_width, num_basis) + + # create basis function and its derivative + phi = GaussianBF(centers, basis_width) + dphi = dphi_t(cs, phi) + + # create basis matrix (shape: Mx2) + matrix = np.array([phi, dphi]) + + # call superclass constructor + super(GaussianBM, self).__init__(matrix) + + +class VonMisesBM(BasisMatrix): + r"""Von-Mises Basis Matrix + + Matrix containing Von-Mises basis functions. + """ + + def __init__(self, cs, num_basis, basis_width=1.): + """ + Initialize the Von-Mises basis matrix. + + Args: + cs (CS): canonical system + num_basis (int): number of basis functions + basis_width (float): width of the basis functions + """ + # create derivative of basis function wrt to time + def dphi_t(cs, phi): + def step(s): + return phi.grad(s) * cs.grad() + return step + + # distribute centers for the Gaussian basis functions + # the centers are placed uniformly between [-2*width, 1+2*width] + if num_basis == 1: + centers = (1. + 4 * basis_width) / 2. + else: + centers = np.linspace(-2 * basis_width, 1 + 2 * basis_width, num_basis) + + # create basis function and its derivative + phi = VonMisesBF(centers, basis_width) + dphi = dphi_t(cs, phi) + + # create basis matrix + matrix = np.array([phi, dphi]) + super(VonMisesBM, self).__init__(matrix) + + +class BlockDiagonalMatrix(Matrix): + r"""Callable Block Diagonal matrix + """ + + def __init__(self, matrices): + """ + Initialize the block diagonal matrix which contains callable matrices in its diagonal. + + Args: + matrices (list[BasisMatrix]): list of callable matrices + """ + self.matrices = matrices + + @property + def shape(self): + """Return the shape of the block diagonal matrix""" + shape = 0 + for matrix in self.matrices: + shape += np.array(matrix.shape) + return tuple(shape) + + @property + def num_basis(self): + """Return the number of basis per dimensions""" + return [matrix.num_basis for matrix in self.matrices] + + def evaluate(self, s): + """ + Evaluate the block diagonal matrix on the given input. + + Args: + s (float, np.array): input value + + Returns: + np.array: block diagonal matrix + """ + return block_diag(*[matrix(s) for matrix in self.matrices]) + + def __call__(self, s): + """ + Evaluate the block diagonal matrix on the given input. + + Args: + s (float, np.array): input value + + Returns: + np.array: block diagonal matrix + """ + return self.evaluate(s) + + def __getitem__(self, idx): + """ + Return a desired chunk of the block diagonal matrix. + + Args: + idx (int, slice): index of the basis matrix(ces) we wish to keep + + Returns: + BlockDiagonalMatrix: return the desired chunk of the diagonal matrix + """ + return BlockDiagonalMatrix(matrices=self.matrices[idx]) + + + +######################## ProMP ############################## + +class ProMP(object): + r"""Probabilistic Movement Primitives + + This class implements the ProMP framework proposed in [1]. This works by putting a prior over the weight + parameters. + + .. math:: y_t = [q_t, \dot{q}_t]^T = \Phi_t^T w + \epsilon_y + + where :math:`y_t \in \mathbb{R}^{2 \times 1}` is the joint state vector at time step :math:`t`, + :math:`\Phi_t = [\phi_t, \dot{\phi}_t] \in \mathbb{R}^{M \times 2}` is the matrix containing the basis functions + defined by the user and where `M` is the number of these basis functions, :math:`w \in \mathbb{R}^{Mx1}` is the + weight vector on which we put a Gaussian prior distribution given by :math:`w \sim \mathcal{N}(\mu_w, \Sigma_w)`, + and :math:`epsilon_y \sim \mathcal{N}(0, \Sigma_y)` is the zero-mean Gaussian noise. + + The probability of a specific trajectory :math:`\tau` is given by taking the joint probability distribution, and + assuming independence between each time step: + + .. math:: + + p(\tau | w) = p(y_0, ..., y_T | w) &= \prod_{t=0}^T p(y_t | w) \\ + &= \prod_{t=0}^T \mathcal{N}(y_t | \Phi_t^T w, \Sigma_y) + + Note that because we are modeling trajectories with a probability distribution, we need several demonstrations + in order to capture its variance. + + By marginalizing :math:`p(\tau | w)` such that the specific weights :math:`w` are integrated out, gives us: + + .. math:: p(\tau ; \theta) = \int p(\tau | w) p(w ; \theta) dw + + where :math:`p(w; \theta)` is the Gaussian prior distribution over the weights :math:`w`, and + :math:`\theta = {\mu_w, \Sigma_w}` are the parameters that describe this distribution. This is referred as the + marginal likelihood, which is more robust to overfitting as we average over the different models. + + + Coupling between Movement Primitives: + ------------------------------------- + We can generalize the above method and encode the coupling between multiple movement primitives. + + + Learning from demonstrations: + ----------------------------- + In the imitation learning case, when learning from demonstrations, instead of maximizing the likelihood we maximize + the marginal likelihood (type-II MLE), that is: + + .. math:: \theta^* = argmax_{\theta} p(\tau ; \theta) = argmax_{\theta} \int p(\tau | w) p(w ; \theta) dw + + + Modulation of via-points, final position and velocities by conditioning: + ------------------------------------------------------------------------ + We can condition the probability on the weights given a desired observation + :math:`\hat{x}_t = [\hat{y}_t, \hat{\Sigma}_y]`. + + + Combination and blending of movement primitives: + ------------------------------------------------ + * combination + * blending / sequencing + + References: + [1] "Probabilistic Movement Primitives", Paraschos et al., 2013 + [2] "Using Probabilistic Movement Primitives in Robotics", Paraschos et al., 2018 + """ + + __metaclass__ = ABCMeta + + def __init__(self, num_dofs=None, weight_size=None, weights=None, weights_covariance=None, + canonical_system=None, noise_covariance=1., Phi=None, promps=None): + """ + Initialize the probabilistic movement primitive + + Args: + num_dofs (int, None): number of degrees of freedom. + weight_size (int, None): size of the weight mean vector. This has to be specified if one of the following + arguments are not given: `Phi`, `weights`, `weights_covariance`. + weights (None, np.array[DM], Gaussian): mean of the weigths, or Gaussian weight distribution + weights_covariance (None): covariance on the weights + canonical_system (None, CS): canonical system + noise_covariance (float, np.array[2Dx2D]): covariance on the noise + Phi (None, Matrix[DM,2D]): callable basis matrix + promps (list[ProMP], None): list of ProMPs (useful when combining different ProMPs) + """ + super(ProMP, self).__init__() + + # check if multiple promps are given + if promps: + # Notes: the ProMPs can be of different type, and/or have different nb of basis fcts + + # check that we are given a list of promps + if not isinstance(promps, (list, tuple)): + raise TypeError("Expecting a list of ProMPs for `promps`") + for i, promp in enumerate(promps): + if not isinstance(promp, ProMP): + raise TypeError("The item {} is not an instance of ProMP".format(i)) + + # check other attributes + for i in range(0, len(promps) - 1): + promp, next_promp = promps[i], promps[i+1] + + # check if the basis matrix has been defined + if not promp.Phi: + raise ValueError("The ProMP {} doesn't have a basis matrix defined".format(i)) + + # check if each ProMP has the same dimensionality (i.e. the same number of DoF) + if promp.dim != next_promp.dim: + raise ValueError("The ProMPs {} and {} have different dimensions".format(i, i+1)) + + # check if each ProMP has the same number of time steps + if promp.cs.num_timesteps != next_promp.cs.num_timesteps: + raise ValueError("The ProMPs {} and {} have different number of time steps".format(i, i+1)) + + num_dofs = promps[0].dim + + # else, define one promp + else: + if Phi: + # basis matrix (shape should be: DMx2D where D=nb of DoFs, and M=nb of basis fcts) + num_dofs = Phi.shape[1] / 2 + else: + if num_dofs is None: + raise ValueError("The number of degrees of freedom should be specified") + + # quick checks + if num_dofs < 1: + raise ValueError("Expecting at least one degree freedom") + + # create Gaussian weight distribution (with shape(mean) = DM, shape(cov) = DMxDM) + if isinstance(weights, Gaussian): + # check that it matches with the dimension of Phi if specified + if Phi: + if Phi.shape[0] != weights.mean.size: + raise ValueError("Mismatch between the dimensions of Phi and the weights; got Phi.shape[0]={} " + "and weights.size={}".format(Phi.shape[0], weights.mean.size)) + else: + # infer the size of the weights + if Phi: + size = Phi.shape[0] + if weights: + if weights.size != size: + raise ValueError( + "Mismatch between the dimensions of Phi and the weights; got Phi.shape[0]={} " + "and weights.size={}".format(size, weights.size)) + elif weights: + size = weights.size + elif weights_covariance: + size = weights_covariance.shape[0] + else: + size = weight_size + + # check that the size is valid + if not isinstance(size, int): + raise TypeError("Expecting the weight_size to be given and to be an integer") + if size <= 0: + raise ValueError("Expecting the size to be positive") + + # create mean vector and covariance matrix if no weigths specified + if weights is None: + weights = np.random.rand(size) + if weights_covariance is None: + weights_covariance = np.identity(size) + + if weights_covariance.shape != (size, size): + raise ValueError("Expecting a square matrix for the covariance matrix of shape SxS, where S is " + "the size of the weight vector") + + # create weight distribution + weights = Gaussian(weights, weights_covariance) + + # create Gaussian noise distribution (with shape(mean) = 2D, shape(cov) = 2Dx2D) + if isinstance(noise_covariance, (float, int)): + noise_covariance = noise_covariance * np.identity(2 * num_dofs) + self._noise = Gaussian(np.zeros(2 * num_dofs), noise_covariance) + + + # set the variables + self.D = num_dofs + self._weights = weights + self.promps = promps + self.Phi = Phi # shape: DMx2D + + # priority exponent (only play a role if combining different ProMPs) + self.priority = 1. + + # create canonical system + self.cs = canonical_system if canonical_system is not None else LinearCS() + + + ############## + # Properties # + ############## + + @property + def canonical_system(self): + """Return the canonical system""" + return self.cs + + @property + def num_mps(self): + """Return the number of ProMPs.""" + if self.promps: + return len(self.promps) + return 1 + + # alias + num_promps = num_mps + + @property + def num_dofs(self): + """Return the number of degrees of freedom""" + return self.D + + @property + def dim(self): + """Return the dimensionality which is 2 * the number of degrees of freedom (2 because we have position and + velocity info)""" + return 2 * self.D + + # TODO: generalize it with different number of basis functions + @property + def num_basis_per_dof(self): + """Return the number of basis function per degree of freedom""" + return self.Phi.num_basis + + @property + def total_num_basis(self): + """Return the total number of basis functions""" + return self.Phi.shape[0] / self.num_dofs + + @property + def basis_matrix(self): + """Return the basis matrix""" + return self.Phi + + @property + def weights(self): + """Return the weight distribution""" + return self._weights + + @property + def noise(self): + """Return the noise distribution""" + return self._noise + + @property + def input_dims(self): + """Return the input dimension of the model, which is one as it only accepts the phase variable""" + return 1 + + @property + def output_dims(self): + """Return the output dimension of the model""" + return 2 * self.D + + @property + def priority(self): + """Return the priority exponent""" + return self._alpha + + @property + def priority_fct(self): + """Return the priority exponent function""" + return self._alpha_fct + + @priority.setter + def priority(self, priority): + """Set the priority exponent which is a function""" + if priority is None: + self._alpha = 1 + elif isinstance(priority, (float, int)): + if priority < 0 or priority > 1: + raise ValueError("Priority exponents can only be between 0 and 1") + self._alpha = priority + self._alpha_fct = lambda s: priority + # elif isinstance(priority, np.ndarray): + # if len(priority) != self.cs.num_timesteps: + # raise ValueError("Mismatch between the number of priorities and the number of phases. Here, we " + # "assume the user wants to give a priority for each phase.") + # if np.all(priority < 0) or np.all(priority > 1): + # raise ValueError("Some priority exponents are not between 0 and 1") + # self._alpha = priority + elif callable(priority): + # check if the callable priority accepts the phase value(s) + try: + # check if the callable priority accepts the phase values one by one + self.cs.reset() + for _ in range(self.cs.num_timesteps): # for one period + s = self.cs.step() + p = priority(s) + if not isinstance(p, (float, int)): + raise TypeError("The callable function/class doesn't return a float/int number given " + "the phase value") + if p < 0 or p > 1: + raise ValueError("The callable function returned a priority which is not between 0 and 1; " + "given the phase value s={}, it returned the priority p={}".format(s, p)) + + # check if the callable priority accepts the phase values all at once + self.cs.reset() + s = self.cs.rollout() + p = priority(s) + if not isinstance(p, np.array): + raise TypeError("The callable function/class doesn't return an np.array given the phase values") + if np.all(p < 0) or np.all(p > 1): + raise ValueError("Some priority exponents returned by the callable function/class are not between " + "0 and 1") + + # reset the canonical system + self.cs.reset() + except: + raise ValueError("The callable function/class doesn't accept float numbers or ") + self._alpha = priority + self._alpha_fct = priority + else: + raise TypeError("Priority exponent can only be None, a float, or an array of floats") + + ################## + # Static Methods # + ################## + + @staticmethod + def copy(other): + """Return a copy of a ProMP""" + if not isinstance(other, ProMP): + raise TypeError("Trying to copy an object which is not a ProMP") + pass + + @staticmethod + def isParametric(): + """The ProMP is a parametric model""" + return True + + @staticmethod + def isLinear(): + """The ProMP has linear parameters""" + return True + + @staticmethod + def isRecurrent(): + """The ProMP is not a recurrent model where outputs depen ds on previous inputs. Its sequential nature + is due to the fact that the time is given as an input to the ProMP model""" + return False + + @staticmethod + def isProbabilistic(): + """The ProMP is a probabilistic model which parametrizes a normal distribution (by specifying its mean + and covariance)""" + return True + + @staticmethod + def isDiscriminative(): + """The ProMP is a discriminative model which predicts :math:`p(y|x)` where :math:`x` is the (time) input, + and :math:`y` is the output""" + return True + + @staticmethod + def isGenerative(): + """The ProMP is not a generative model, and thus we can not sample from it""" + return False + + + ########### + # Methods # + ########### + + def parameters(self): + """Returns an iterator over the model parameters.""" + #yield self.weights + yield self.weights.mean + yield self.weights.cov + + def named_parameters(self): + """Returns an iterator over the model parameters, yielding both the name and the parameter itself""" + #yield "Gaussian", self.weights + yield "mean", self.weights.mean + yield "covariance", self.weights.cov + + def reset(self): + """Reset the canonical systems""" + self.cs.reset() + + def basis(self, s): + """ + Return the basis matrix evaluated at the given phase(s). + + Args: + s (np.array[T], float): phase value(s) + + Returns: + np.array[DM,2D], np.array[DM,T,2D]: basis matrix evaluated at the given phase(s). + """ + return self.Phi(s) + + def weighted_basis(self, s): + """ + Return the weighted basis evaluated at the given phase(s), that is, `Phi(s) * w`, where `w` is the weight + mean vector, and the product is a broadcast product (and not a dot/scalar product). + + Args: + s (np.array[T], float): phase value(s) + + Returns: + np.array[DM,T,2D]: weighted basis matrix (this has the same shape as the basis matrix Phi(s)) + """ + return (self.Phi(s).T * self.weights.mean).T + + def predict_conditional(self, s, weights=None): + """ + Predict conditional output distribution given the weights :math:`p(y_t|w)`. + + This is given by: + + .. math:: p(y_t|w) = \mathcal{N}(\Phi(s_t)^\top \mu_w , \Sigma_y) + + Args: + s (float, np.array[T]): phase value(s) + weights (None, np.array[DM]): weights (array). If None, it will take the mean/mode of the weight + distribution. + + Returns: + Gaussian or list[Gaussian]: conditional output distribution for each phase value + """ + # if multiple promps that need to be combined + if self.promps: + gaussians = [promp.predict_conditional(s) for promp in self.promps] + priorities = [promp.priority_fct(s) for promp in self.promps] + + # if one phase value + if isinstance(gaussians[0], Gaussian): + covs, means = [], [] + for g, p in zip(gaussians, priorities): + # compute covariance and mean + prec = g.prec + cov = p * prec + mean = p * prec.dot(g.mean) + covs.append(cov) + means.append(mean) + + cov = np.linalg.inv( np.sum(covs, axis=0) ) + mean = cov.dot( np.sum(means, axis=0) ) + + return Gaussian(mean=mean, covariance=cov) + + # if multiple phases + else: + gaussian_results = [] + for gaussian, priority in zip(gaussians, priorities): + covs, means = [], [] + for g, p in zip(gaussians, priorities): + # compute covariance and mean + prec = g.prec + cov = p * prec + mean = p * prec.dot(g.mean) + covs.append(cov) + means.append(mean) + + cov = np.linalg.inv(np.sum(covs, axis=0)) + mean = cov.dot(np.sum(means, axis=0)) + g = Gaussian(mean=mean, covariance=cov) + gaussian_results.append(g) + + return gaussian_results + + # if one promp + if weights is None: + weights = self.weights.mean + if isinstance(s, (float, int)): + return Gaussian(mean=self.Phi(s).T.dot(weights), covariance=self.noise.cov) + return [Gaussian(mean=self.Phi(phase).T.dot(weights), covariance=self.noise.cov) for phase in s] + + def predict_marginal(self, s): + """ + Predict marginal output distribution :math:`p(y_t; \theta)`. + + .. math:: + + p(y_t; \theta) &= \int p(y_t | w) p(w; \theta) dw \\ + &= \int \mathcal{N}(y_t | \Phi(s_t)^\top w, \Sigma_y) \mathcal{N}(w | \mu_w, \Sigma_w) dw + &= \mathcal{N}(\Phi(s_t)^\top \mu_w, \Phi(s_t)^\top \Sigma_w \Phi(s_t) + \Sigma_y) + + where :math:`\theta = \{\mu_w, \Sigma_w\}` are the parameters. + + Args: + s (float, np.array[T]): phase value(s) + + Returns: + Gaussian or list[Gaussian]: marginal output distribution for each phase value + """ + # if multiple promps that need to be combined + if self.promps: + gaussians = [promp.predict_marginal(s) for promp in self.promps] + priorities = [promp.priority_fct(s) for promp in self.promps] + + # if one phase value + if isinstance(gaussians[0], Gaussian): + covs, means = [], [] + for g, p in zip(gaussians, priorities): + # compute covariance and mean + prec = g.prec + cov = p * prec + mean = p * prec.dot(g.mean) + covs.append(cov) + means.append(mean) + + cov = np.linalg.inv(np.sum(covs, axis=0)) + mean = cov.dot(np.sum(means, axis=0)) + + return Gaussian(mean=mean, covariance=cov) + + # if multiple phases + else: + gaussian_results = [] + for gaussian, priority in zip(gaussians, priorities): + covs, means = [], [] + for g, p in zip(gaussians, priorities): + # compute covariance and mean + prec = g.prec + cov = p * prec + mean = p * prec.dot(g.mean) + covs.append(cov) + means.append(mean) + + cov = np.linalg.inv(np.sum(covs, axis=0)) + mean = cov.dot(np.sum(means, axis=0)) + g = Gaussian(mean=mean, covariance=cov) + gaussian_results.append(g) + + return gaussian_results + + # if one promp + if isinstance(s, (float, int)): + return self.Phi(s).T * self.weights + self.noise + return [(self.Phi(phase).T * self.weights + self.noise) for phase in s] + + def predict(self, s, sample=False, method='conditional'): + """ + Predict output mean :math:`\mu_y` given input data :math:`s`. + + .. math:: \mu_y = \Phi(s)^T \mu_w + \epsilon_y + + where :math:`\Phi(s)` is the basis matrix, :math:`w` is the weight vector, and :math:`\epsilon_y` is the + Gaussian noise such that :math:`\epsilon_y \sim \mathcal{N}(0, \Sigma_y)` + + Args: + s (float, np.array[T]): phase value(s) + method (str): choice between 'conditional' and 'marginal' prediction + sample (bool): if False, it will return the mode/mean of the Gaussian, otherwise it will sample from it + for each phase value + + Returns: + np.array[2D], np.array[T,2D]: output mean(s) + """ + # Prediction + if method == 'conditional': + gaussians = self.predict_conditional(s) + elif method == 'marginal': + gaussians = self.predict_marginal(s) + else: + raise NotImplementedError("The given 'method' argument has not been implemented. Please choose " + "between 'conditional' and 'marginal'.") + + # if one phase value, return predicted output vector (shape: 2D) + if isinstance(gaussians, Gaussian): + if sample: + return gaussians.sample() + return gaussians.mode + + # if multiple phase values, return output vectors (shape: [T,2D]) + else: + if sample: + return np.array([g.sample() for g in gaussians]) + return np.array([g.mode for g in gaussians]) + + def predict_proba(self, s, method='marginal', return_gaussian=True): + """ + Predict the probability of output :math:`\mathcal{N}(\mu_y, \Sigma_y)` given input data :math:`s`. + + .. math:: + + p(y; \theta) &= \int p(y | w) p(w; \theta) dw \\ + &= \int \mathcal{N}(y | \Phi(s)^\top w, \Sigma_y) \mathcal{N}(w | \mu_w, \Sigma_w) dw + &= \mathcal{N}(\Phi(s)^\top \mu_w, \Phi(s)^\top \Sigma_w \Phi(s) + \Sigma_y) + + where :math:`\theta = \{\mu_w, \Sigma_w\}` are the parameters. + + Args: + s (float, np.array[T]): phase value(s) + method (str): choice between 'conditional' and 'marginal' prediction + return_gaussian (bool): If True, it will return a Gaussian for each input phase value. If False, + it will return the mean and covariance for each phase value. + + Returns: + if return_gaussian: + Gaussian, or list[Gaussian]: gaussian(s) for each input phase value + else: + np.array[2D], np.array[T,2D]: output mean(s) + np.array[2D,2D], np.array[T,2D,2D]: output covariance(s) + """ + # Predict + if method == 'conditional': + y = self.predict_conditional(s) + elif method == 'marginal': + y = self.predict_marginal(s) + else: + raise NotImplementedError("The given 'method' argument has not been implemented. Please choose " + "between 'conditional' and 'marginal'.") + + # return Gaussian in the desired form + if return_gaussian: + return y + + # if one phase value + if isinstance(y, Gaussian): + return y.mean, y.cov + + # if multiple phase values + return np.array([gaussian.mean for gaussian in y]), np.array([gaussian.cov for gaussian in y]) + + def step(self, tau=1., sample=False, method='marginal'): + """ + Perform one step forward with the canonical system, and return the deterministic predicted output, given by: + + .. math:: y = \Phi(s)^T \mu_w + \epsilon_y + + where :math:`\Phi(s)` is the basis matrix, :math:`w` is the weight vector, and :math:`\epsilon_y` is the + noise. + + Args: + tau (float): speed + sample (bool): if False, it will return the mode/mean of the Gaussian, otherwise it will sample from it + for each phase value. + method (str): choice between 'conditional' and 'marginal' prediction + + Returns: + np.array[2D]: predicted output + """ + s = self.cs.step(tau=tau) + return self.predict(s, sample=sample, method=method) + + def step_proba(self, tau=1., method='marginal', return_gaussian=True): + """ + Perform one step forward with the canonical system, and return the predicted output distribution + :math:`p(y_t | w)` and :math:`p(y_t ; \theta)` based on the specified `method`. + + Args: + tau (float): speed + method (str): choice between 'conditional' and 'marginal' prediction + return_gaussian (bool): If True, it will return a Gaussian for the current phase value. If False, + it will return the mean and covariance of the current phase value. + + Returns: + if return_gaussian: + Gaussian: gaussian for the phase value + else: + np.array[2D]: output mean + np.array[2D,2D]: output covariance + """ + s = self.cs.step(tau=tau) + return self.predict_proba(s, method=method, return_gaussian=return_gaussian) + + def rollout(self, tau=1., sample=False, method='marginal'): + """ + Perform a complete rollout; predict a whole trajectory from the initial phase to final one. + + Args: + tau (float): speed + sample (bool): if False, it will return the mode/mean of the Gaussian, otherwise it will sample from it + for each phase value. + method (str): choice between 'conditional' and 'marginal' prediction + + Returns: + np.array[T,2D]: predicted outputs + """ + # reset system + self.reset() + + # rollout with the canonical system + s = self.cs.rollout(tau=tau) + + # return predictions + return self.predict(s, sample=sample, method=method) + + def rollout_proba(self, tau=1., method='marginal', return_gaussian=True): + """ + Perform a complete probabilistic rollout; predict a whole probabilistic trajectory from the initial phase to + final one. + + Args: + tau (float): speed + method (str): choice between 'conditional' and 'marginal' prediction + return_gaussian (bool): If True, it will return a Gaussian for each phase value. If False, + it will return the mean and covariance for each phase value. + + Returns: + if return_gaussian: + list[Gaussian]: gaussians for each phase value + else: + np.array[T,2D]: output means + np.array[T,2D,2D]: output covariances + """ + # reset system + self.reset() + + # rollout with the canonical system + s = self.cs.rollout(tau=tau) + + # return predictions + return self.predict_proba(s, method=method, return_gaussian=return_gaussian) + + def sample_weights(self, size=None, seed=None): + """ + Sample weight vector from the distribution. + + Args: + size (int, None): number of samples + seed (int, None): seed for the random number generator + + Returns: + np.array[DM], np.array[N,DM]: samples + """ + return self.weights.sample(size=size, seed=seed) + + def sample_trajectory(self, size=None): + """ + Sample one complete trajectory. + + .. math:: \tau \sim p(\tau; \theta) = \prod_{t=1}^T p(y_t; \theta) + + Warnings: we assume complete independence between the :math:`y_t` instead of a conditional independence + of the :math:`y_t` given the weights :math:`w`. + + Args: + size (int, None): number of samples + + Returns: + np.array[T,2D], np.array[N,T,2D]: trajectory/trajectories + """ + if size is None or size < 2: + return self.rollout(tau=1., sample=True, method='marginal') + return np.array([self.rollout(tau=1., sample=True, method='marginal') for _ in range(size)]) + + def sample_from_prediction(self, s, size=None, seed=None, method='conditional'): + """ + Sample from predicted output distribution :math:`y_t \sim p(y_t | w)` or :math:`y_t \sim p(y_t; \theta)`, + based on the specified `method`. + + Args: + s (float): phase value + size (int, None): number of samples + seed (int, None): seed for the random number generator + method (str): choice between 'conditional' and 'marginal' prediction + + Returns: + np.array[2D], np.array[N,2D]: sample(s) + """ + if not isinstance(s, (float, int)): + raise TypeError("Expecting only one phase value (float)") + gaussian = self.predict_proba(s, method=method, return_gaussian=True) + return gaussian.sample(size=size, seed=seed) + + def sample_from_conditional(self, s, size=None, seed=None): + """ + Sample output from the likelihood (conditional probability). + + .. math:: y_t \sim p(y_t | w) + + where :math:`w` are the weights. Because :math:`y_t = \Phi(s_t)^\top w + \epsilon_y` with :math:`\epsilon_y` + being a zero-mean Gaussian noise (i.e. :math:`\epsilon_y \sim \mathcal{N}(0, \Sigma_y)`), the outputs are + sampled from the following distribution: + + .. math:: y_t \sim \mathcal{N}(\Phi(s)^\top w, \Sigma_y) + + Args: + s (float): phase value + size (int, None): number of samples + seed (int, None): seed for the random number generator + + Returns: + np.array[2D], np.array[N,2D]: sample(s) + """ + if not isinstance(s, (float, int)): + raise TypeError("Expecting only one phase value (float)") + gaussian = self.predict_conditional(s) + return gaussian.sample(size=size, seed=seed) + + def sample_from_marginal_likelihood(self, s, size=None, seed=None): + """ + Sample output from the marginal likelihood. + + .. math:: y_t \sim p(y_t; \theta) + + where :math:`\theta = {\mu_w, \Sigma_w}` are the parameters of the Gaussian distribution put on the weights, + and :math:`p(y_t; \theta) = \int p(y_t | w) p(w; \theta) dw`. The outputs are thus sampled from: + + .. math:: y_t \sim \mathcal{N}(\Phi(s)^\top \mu_w, \Phi(s)^\top \Sigma_w \Phi(s) + \Sigma_y) + + Args: + s (float): phase value + size (int, None): number of samples + seed (int, None): seed for the random number generator + + Returns: + np.array[2D], np.array[N,2D]: sample(s) + """ + if not isinstance(s, (float, int)): + raise TypeError("Expecting only one phase value (float)") + gaussian = self.predict_marginal(s) + return gaussian.sample(size=size, seed=seed) + + def likelihood(self, y, s=None): + """ + Compute the likelihood of the output data given the input data. + + .. math:: p(y_{1:T} | w) = \prod_{t=1}^T \mathcal{N}(y_t | \Phi(s_t)^\top \mu_w, \Sigma_y) + + Args: + y (np.array[2D], np.array[T,2D]): output vector(s) to evaluate the likelihood. It can be an output + vector at one particular time step, or an output vector for each time step + s (float, np.array[T], None): the corresponding time step(s). If None, it will generate the same number + of phase values than the number of output vectors, and uniformly place them between [0,1]. + + Returns: + float: likelihood + """ + if len(y.shape) == 1: + if not isinstance(s, (float, int)): + raise TypeError("One sample is given but not its associated phase value (float number)") + gaussian = self.predict_conditional(s) + return gaussian.pdf(y) + if len(y.shape) != 2: + raise ValueError("Expecting a 2D matrix containing the concatenated output vectors") + T = y.shape[0] + if s is None: + s = np.linspace(0.,1.,T) + if isinstance(s, (float, int)): + s = s * np.ones(T) + if len(s) != T: + raise ValueError("Expecting the number of phase values to be the same as the number of output vectors") + gaussians = self.predict_conditional(s) + return np.prod([gaussian.pdf(yt) for yt, gaussian in zip(y, gaussians)]) + + # alias + pdf = likelihood + + def log_likelihood(self, y, s=None): + """ + Compute the log-likelihood. + + .. math:: \log p(y_{1:T} | w) = \sum_{t=1}^T \log \mathcal{N}(y_t | \Phi(s_t)^\top \mu_w, \Sigma_y) + + Args: + y (np.array[2D], np.array[T,2D]): output vector(s) to evaluate the likelihood. It can be an output + vector at one particular time step, or an output vector for each time step + s (float, np.array[T], None): the corresponding time step(s). If None, it will generate the same number + of phase values than the number of output vectors, and uniformly place them between [0,1]. + + Returns: + float: log-likelihood + """ + return np.log(self.likelihood(y, s)) + + # alias + log_pdf = log_likelihood + + def marginal_likelihood(self, y, s=None): + """ + Compute the marginal likelihood (which is the loss being optimized in ProMPs) by assuming that we have + independence between the output vectors :math:`y_t`. + + If we assume independence between each sample :math:`y_t` then: + + .. math:: + + p(y_{1:T}; \theta) = \prod_{t=1}^T p(y_t; \theta) + + where + + .. math:: + + p(y_t; \theta) &= \int p(y_t | w) p(w; \theta) dw \\ + &= \int \mathcal{N}(y_t | \Phi(s_t)^\top w, \Sigma_y) \mathcal{N}(w | \mu_w, \Sigma_w) dw + &= \mathcal{N}(y_t | \Phi(s_t)^\top \mu_w, \Phi(s_t)^\top \Sigma_w \Phi(s_t) + \Sigma_y) + + where :math:`\theta = \{\mu_w, \Sigma_w\}` are the parameters. + + Note that if instead of independence, we assume conditional independence given the weights :math:`w`, + then we have: + + .. math:: + + p(y_{1:T}; \theta) &= \int p(y_{1:T} | w) p(w; \theta) dw \\ + &= \int \prod_{t=1}^T p(y_t | w) p(w; \theta) dw + + Note that in this expression the product is inside the integral instead of outside. + + Args: + y (np.array[2D], np.array[T,2D]): output vector(s) to evaluate the likelihood. It can be an output + vector at one particular time step, or an output vector for each time step + s (float, np.array[T], None): the corresponding time step(s). If None, it will generate the same number + of phase values than the number of output vectors, and uniformly place them between [0,1]. + + Returns: + float: marginal likelihood + """ + if len(y.shape) == 1: + if not isinstance(s, (float, int)): + raise TypeError("One sample is given but not its associated phase value (float number)") + gaussian = self.predict_marginal(s) + return gaussian.pdf(y) + if len(y.shape) != 2: + raise ValueError("Expecting a 2D matrix containing the concatenated output vectors") + T = y.shape[0] + if s is None: + s = np.linspace(0.,1.,T) + if isinstance(s, (float, int)): + s = s * np.ones(T) + if len(s) != T: + raise ValueError("Expecting the number of phase values to be the same as the number of output vectors") + gaussians = self.predict_marginal(s) + return np.prod([gaussian.pdf(yt) for yt, gaussian in zip(y, gaussians)]) + + def log_marginal_likelihood(self, y, s): + """ + Compute the marginal log-likelihood (which is the loss being optimized in ProMPs). + + If we assume independence between each sample :math:`y_t` then: + + .. math:: + + \log p(y_{1:T}; \theta) = \sum_{t=1}^T \log p(y_t; \theta) + + See also the `marginal_likelihood` method for more information. + + Args: + y (np.array[2D], np.array[T,2D]): output vector(s) to evaluate the likelihood. It can be an output + vector at one particular time step, or an output vector for each time step + s (float, np.array[T], None): the corresponding time step(s). If None, it will generate the same number + of phase values than the number of output vectors, and uniformly place them between [0,1]. + + Returns: + float: log marginal likelihood + """ + return np.log(self.marginal_likelihood(y,s)) + + def joint_distribution(self, y, Phi_or_s, y_cov=None): + """ + Compute and return the joint distribution between the weights and the output vector :math:`p(w, y)`. This will + be useful when computing the posterior conditional probability of the weights given the output vector + :math:`p(w|y)`. + + .. math:: p(w, y) = \mathcal{N}(\mu, \Sigma) + + where :math:`\mu = [\mu_w^\top, (\Phi^\top \mu_w)^\top]^\top` and :math:`\Sigma = \left[ \begin{array}{cc} + \Sigma_w & \Sigma_w \Phi \\ \Phi^\top \Sigma_w & \Phi^\top \Sigma_w \Phi + \Sigma_y \end{array} \right]`, with + :math:`\Sigma_y` is the specified covariance for the output vector. If it is not provided, it will be set to 0. + + Args: + y (np.array[2D]): output vector + Phi_or_s (np.array[DM,2D], float): basis matrix evaluated at a particular phase, or phase value. + If the phase is given, the basis matrix will computed internally. + y_cov (np.array[2D,2D]): desired covariance. + + Returns: + Gaussian: joint Gaussian distribution between the output and weights + """ + # Quick checks + y_cov_shape = (y.shape[-1], y.shape[-1]) + if y_cov is None: + y_cov = np.zeros(y_cov_shape) + if y_cov.shape != y_cov_shape: + raise ValueError("Expecting a 2D array of shape {} for the output covariance".format(y_cov_shape)) + + # check if phase value or basis matrix given + if isinstance(Phi_or_s, (float, int)): # phase value + Phi = self.Phi(Phi_or_s) + else: + Phi = Phi_or_s + + # compute joint distribution between weights and output vector + w_mean, w_cov = self.weights.mean, self.weights.cov # shape: DM and DMxDM + joint_mean = np.concatenate((w_mean, Phi.T.dot(w_mean))) + joint_cov = np.vstack((np.hstack((w_cov, w_cov.dot(Phi))), + np.hstack((Phi.T.dot(w_cov), Phi.T.dot(w_cov).dot(Phi) + y_cov)))) + + # return joint distribution + return Gaussian(mean=joint_mean, covariance=joint_cov) + + def posterior_weights(self, y, Phi_or_s, y_cov=None): + """ + Compute the posterior distribution on the weights given the output vector :math:`p(w|y)`. + + .. math:: p(w|y) = \mathcal{\mu_w', \Sigma_w'} + + where :math:`\mathcal{\mu_w', \Sigma_w'}` is the resulting conditional Gaussian distribution, with: + + .. math:: + + \mu_w' &= \mu_w + \Sigma_w \Phi \Sigma_{yy}^{-1} (y - \Phi^\top \mu_w) \\ + \Sigma_w' &= \Sigma_w - \Sigma_w \Phi \Sigma_{yy}^{-1} \Phi^\top \Sigma_w + + with :math:`\Sigma_{yy} = \Phi^\top \Sigma_w \Phi + \Sigma_y` where :math:`\Sigma_y` is the specified + covariance for the output vector. If it is not provided, it will be set to 0. + + Args: + y (np.array[2D]): output vector + Phi_or_s (np.array[DM,2D], float): basis matrix evaluated at a particular phase, or phase value. + If the phase is given, the basis matrix will computed internally. + y_cov (np.array[2D,2D]): desired covariance. + + Returns: + Gaussian: posterior distribution on the weights given the output vector (along with its possible + desired covariance) + """ + # check if phase value or basis matrix given + if isinstance(Phi_or_s, (float, int)): # phase value + Phi = self.Phi(Phi_or_s) + else: + Phi = Phi_or_s + + # compute joint distribution between weight and given output vector + joint = self.joint_distribution(y, Phi, y_cov) + + # compute the posterior weight by conditioning the joint Gaussian + weight_idx_in_joint = range(self.weights.mean.size) + weight = joint.condition(y, weight_idx_in_joint) + + return weight + + def compute_loss(self, Y, kind='marginal_log_likelihood'): + """ + Compute the loss on the whole given data. + + Args: + Y (np.array[N,T,2D], list[np.array[T,2D]]): state trajectories + kind (str): specifies the kind/type of loss we wish to compute. Select between 'likelihood', + 'log_likelihood', 'marginal_likelihood', 'marginal_log_likelihood' + + Returns: + float: loss + """ + # compute phases + phases = [np.linspace(0., 1., len(y)) for y in Y] + + if kind == 'marginal_log_likelihood': + return np.sum([self.log_marginal_likelihood(y, s) for y, s in zip(Y, phases)]) + elif kind == 'log_likelihood': + return np.sum([self.log_likelihood(y, s) for y, s in zip(Y, phases)]) + elif kind == 'likelihood': + return np.sum([self.likelihood(y, s) for y, s in zip(Y, phases)]) + elif kind == 'marginal_likelihood': + return np.sum([self.marginal_likelihood(y, s) for y, s in zip(Y, phases)]) + else: + raise ValueError("The specified kind of loss has not been implemented") + + def expectation_maximization(self, Y, num_iters=1000, threshold=1e-4, verbose=False): + """ + Learn the parameters :math:`\theta = \{\mu_w, \Sigma_w\}` of the Gaussian distribution put on the weights. + + Args: + Y (np.array[N,T,2D], list[np.array[T,2D]]): state trajectories + num_iters (int): number of iterations for the EM algo + threshold (float): convergence threshold for the EM algo + verbose (bool): if we should print details during the optimization process + + Returns: + dict: dictionary containing info collected during the optimization process, such as the history of losses, + the number of iterations it took to converge, if it succeeded, etc. + """ + # TODO: quick checks + + # compute dictionary results + results = {'losses': [], 'success': False, 'num_iters': 0} + + # compute initial loss + loss = self.compute_loss(Y, kind='marginal_log_likelihood') + prev_loss = loss + results['losses'].append(loss) + + for it in range(num_iters): + # E-step: posterior distribution on weights + means, covs = [], [] + for y in Y: + # compute phase + T = len(y) + phases = np.linspace(0., 1., T) + + # compute weight vector + y = y.reshape(-1) # shape: 2DT + Phi = np.hstack([self.Phi(s) for s in phases]) # DMx2DT + weight = self.posterior_weights(y, Phi) # Gaussian with mean of shape DM + + # append mean and cov + means.append(weight.mean) + covs.append(weight.cov) + + means = np.array(means) # shape: NxDM + covs = np.array(covs) # shape: NxDMxDM + + # M-step: result from optimizing the complete-data log-likelihood + mean = Gaussian.compute_mean(means) # shape: DM + cov = Gaussian.compute_covariance(means - mean, bessels_correction=False) # shape: DMxDM + cov += Gaussian.compute_mean(covs, axis=0) # shape: DMxDM + # set new weights for ProMP + self._weights = Gaussian(mean=mean, covariance=cov) + + # 4. check convergence + self.compute_loss(Y, kind='marginal_log_likelihood') + results['losses'].append(loss) + if np.abs(loss - prev_loss) <= threshold: + if verbose: + print("Convergence achieved at iteration {} with associated loss: {}".format(it + 1, loss)) + results['num_iters'] = it + 1 + results['success'] = True + + # update previous loss + prev_loss = loss + + return results + + # TODO: implement following method + def maximum_marginal_likelihood(self, Y, prior_reg=1., num_iters=1000, threshold=1e-4, verbose=False): + """ + Compute the closed-form solutions for the mean and covariance that maximizes the marginal likelihood. + + Assuming independence in time between the predicted output, we have + :math:`p(y_{t=1:T} ; \theta) = \prod_{t=1}^T p(y_t ; \theta)`. Also, by assuming iid sampled trajectories, + the log marginal loss to be optimized is given by: + + .. math:: \mathcal{L}(\theta) = \log p(Y; \theta) = \sum_{n=1}^N \sum_{t=1}^T \log p(y_t^{(n)}; \theta) + + Args: + Y (np.array[N,T,2D], list[np.array[T,2D]]): state trajectories + prior_reg (float): prior regularization + num_iters (int): number of iterations for the EM algo + threshold (float): convergence threshold for the EM algo + verbose (bool): if we should print details during the optimization process + + Returns: + dict: dictionary containing info collected during the optimization process, such as the history of losses, + the number of iterations it took to converge, if it succeeded, etc. + + References: + [1] "The Matrix Cookbook", Petersen and Pedersen, 2012 + [2] "Second Order Adjoint Matrix Equation", Crone, 1981 + """ + results = {'losses': [], 'success': False, 'num_iters': 1} + raise NotImplementedError + # return results + + def linear_ridge_regression(self, Y, prior_reg=1.): + """ + Learn the weights for the ProMP using linear ridge regression. + + Args: + Y (np.array[N,T,2D], list[np.array[T,2D]]): state trajectories + prior_reg (float): prior regularization + + Returns: + None + """ + # create variables + results = {'losses': [], 'success': False, 'num_iters': 1} + I = np.identity(self.num_dofs * self.total_num_basis) # shape: DMxDM + + # compute initial loss + prev_loss = self.compute_loss(Y, kind='marginal_log_likelihood') + results['losses'].append(prev_loss) + + weights = [] + # for each trajectory in Y + for y in Y: + # compute phase + T = len(y) + phases = np.linspace(0, 1, T) + + # compute weight vector (shape: DM) + y = y.reshape(-1) # shape: 2DT + Phi = np.hstack([self.Phi(s) for s in phases]) # shape: DMx2DT + + weight = (np.linalg.inv(Phi.dot(Phi.T) + prior_reg * I)).dot(Phi.dot(y)) # shape: DM + + weights.append(weight) + + weights = np.array(weights) # shape: NxDM + + # fit a gaussian and set weight distribution + mean = Gaussian.compute_mean(weights) # shape: DM + cov = Gaussian.compute_covariance((weights - mean), bessels_correction=False) # shape: DMxDM + + self._weights = Gaussian(mean=mean, covariance=cov) + + # compute loss + loss = self.compute_loss(Y, kind='marginal_log_likelihood') + results['losses'].append(loss) + results['success'] = loss < prev_loss + + return results + + def imitate(self, Y, prior_reg=1., method='lrr', num_iters=1000, threshold=1e-4, verbose=False): + """ + Imitate given trajectories, i.e. learn the parameters :math:`\theta = \{\mu_w, \Sigma_w\}` of the Gaussian + distribution put on the weights. + + Note that because we are trying to model the distribution, multiple demonstrations need to be given. + + Args: + Y (np.array[N,T,2D], list[np.array[T,2D]]): state trajectories + method (str): method to use when learning ('lrr' for linear ridge regression, 'em' for expectation- + maximization, 'mml' for maximum_marginal_likelihood) + prior_reg (float): prior regularization + num_iters (int): number of iterations for the EM algo + threshold (float): convergence threshold for the EM algo + verbose (bool): if we should print details during the optimization process + + Returns: + None + """ + # linear ridge regression + if method == 'lrr': + return self.linear_ridge_regression(Y, prior_reg=prior_reg) + + # expectation-maximization + elif method == 'em': + return self.expectation_maximization(Y, num_iters=num_iters, threshold=threshold, verbose=verbose) + + # maximum marginal likelihood + elif method == 'mml': + return self.maximum_marginal_likelihood(Y, prior_reg=prior_reg, num_iters=num_iters, threshold=threshold, + verbose=verbose) + else: + raise NotImplementedError("The specified method has not been implemented") + + # alias + fit = imitate + + def condition(self, s, y_desired_mean, y_desired_covariance): + """ + Modulate the trajectory distribution by conditioning. + + .. math:: p(w | y^*) + + Args: + s (float): phase value + y_desired_mean (np.array[2D]): desired output state + y_desired_covariance (np.array[2D]): desired output covariance. If it's low (thus the precision is high) + it means, that we want to reach that point + + Returns: + + """ + # # construct joint distribution between new point and weight + # w_mean, w_cov = self.weights.mean, self.weights.cov # shape: DM and DMxDM + # Phi_s = self.Phi(s) # shape: DMx2D + # joint_mean = np.concatenate((w_mean, Phi_s.T.dot(w_mean))) + # joint_cov = np.vstack((np.hstack((w_cov, w_cov.dot(Phi_s))), + # np.hstack((Phi_s.T.dot(w_cov), Phi_s.T.dot(w_cov).dot(Phi_s) + y_desired_covariance)))) + # gaussian = Gaussian(mean=joint_mean, covariance=joint_cov) + # # compute posterior mean by conditioning the joint Gaussian + # weight = gaussian.condition(y_desired_mean, range(w_mean.size)) + + # construct the joint distribution between the new point (i.e. mean) and weight, and then compute the + # posterior on the weight by conditioning the joint distribution given the new desired mean and covariance + return self.posterior_weights(y_desired_mean, s, y_cov=y_desired_covariance) + + # alias + #modulate = condition + + def combine(self, mps, priorities): + """ + Modulate the trajectory distribution by combining/co-activating different movement primitives. This returns + a ProMP which represents the combination of the given ProMPs. + + .. math:: p(\tau) = \prod_{i=1}^P p_i(\tau)^{\alpha_i} + + where :math:`\alpha_i \in [0,1]` are the priorities. + + Notes: this is a specific case of the `blend()` method, where the same priority is set for all the phase + values. + + Args: + mps (list[ProMP]): list of ProMPs + priorities (list[float]): list of priorities with the same length as the number of given `mps` + + Returns: + ProMP: resulting ProMP + """ + return self.blend(mps=mps, priorities=priorities) + + def blend(self, mps, priorities): + """ + Modulate the trajectory distribution by blending different movement primitives together. This returns + a ProMP which represents the combination of the given ProMPs. + + .. math:: + + p(\tau) = \prod{t=1}^T p(y_t) + p(y_t) = \prod_{i=1}^P p_i(y_t)^{\alpha_{i,t}} + p_i(y_t) = \int p_i(y_t | w_i) p_i(w_i) dw_i + + Warnings: note that the above formula assumes independence instead of conditional independence. Indeed, + :math:`p(\tau | w) = \prod_{t=1}^T p(y_t | w)`, and :math:`p(\tau; \theta) = \int p(\tau | w) p(w; \theta) + dw = \int \prod_{t=1}^T p(y_t | w) p(w) dw` which is different from :math:`p(\tau; \theta) = \prod_{t=1}^T + p(y_t; \theta) = \prod_{t=1}^T \int p(y_t| w) p(w; \theta) dw`. Note the product symbol which has switched + with the integral symbol. The former expression assumes conditional independence between the :math:`y_t` + given :math:`w`, while the latter assumes that the :math:`y_t` are completely independent between each + other, which is a stronger assumption. + + Args: + mps (list[ProMP]): list of ProMPs + priorities (np.array[K,T]): list of priorities, where each priority is an array of float numbers + representing the priority / activation factor for each phase value. + + Returns: + ProMP: resulting ProMP + """ + # check that the number of priorities and the number of ProMP coincide + if len(mps) != len(priorities): + raise ValueError("The number of priorities is different from the number of ProMPs") + + # set for each ProMP its priority + for mp, priority in zip(mps, priorities): + mp.priority = priority + + # return the combination + return ProMP(promps=mps) + + # alias + # TODO def sequence + #sequence = blend + + def power(self, priority): + """ + Set the priority (aka activation function) which represents the degree of activation of this ProMP. + + Args: + priority (float, int, callable): priority function + """ + self.priority = priority + + def multiply(self, other): + """ + Multiply a ProMP with another ProMP. + + The resulting prediction (in the case for the marginal prediction) will be given by: + + .. math:: p^*(y_t) = p_1(y_t; \theta_1)^{\alpha_1} p_2(y_t; \theta_2)^{\alpha_2} + + where the :math:`\alpha` are the priorities set beforehand. + + Notes: multiplying a ProMP by a square matrix (to rotate or scale the prediction) can be done by first + calling `predict_proba()` or `step_proba()` which return a Gaussian distribution, and then multiply this + last one by the matrix. The same can be carried out to add a vector or to perform an affine transformation. + + Args: + other (ProMP): other ProMP + + Returns: + ProMP: resulting ProMP + """ + # if other == ProMP + if isinstance(other, ProMP): + if self.promps and other.promps: + return ProMP(promps=self.promps + other.promps) + elif self.promps: + return ProMP(promps=self.promps + [other]) + elif other.promps: + return ProMP(promps=[self] + other.promps) + else: + return ProMP(promps=[self, other]) + else: + raise TypeError("Trying to multiply a ProMP with {}, which has not be defined".format(type(other))) + + + + ############# + # Operators # + ############# + + def __str__(self): + """Return description of this class""" + return self.__class__.__name__ + + def __call__(self, s, probabilistic=True, method='marginal', return_gaussian=True, sample=False): + """Predict output given the phase""" + if probabilistic: + return self.predict_proba(s, method=method, return_gaussian=return_gaussian) + return self.predict(s, method=method, sample=sample) + + def __len__(self): + """Return the number of degree of freedoms if one ProMP. Else, return the number of ProMPs""" + if self.promps: + return len(self.promps) + return self.num_dofs + + def __getitem__(self, idx): + """ + Return the specified probabilistic movement primitive(s) + + Args: + idx (int, slice): index + + Returns: + ProMP: the interested ProMPs + """ + # if multiple ProMPs, return the one specified + if self.promps: + return self.promps[idx] + + # check number of movement primitives + if isinstance(idx, int): + num_dofs = 1 + elif isinstance(idx, slice): # slice + num_dofs = abs(idx.stop - idx.start) / abs(idx.step) + else: + raise TypeError("Expecting the given index to be an integer or a slice") + + # create probabilistic movement primitive of the same type + if isinstance(self, DiscreteProMP): + promp = DiscreteProMP(num_dofs=num_dofs, weights=self.weights[idx]) + elif isinstance(self, RhythmicProMP): + promp = RhythmicProMP(num_dofs=num_dofs, weights=self.weights[idx]) + else: # ProMP + promp = ProMP(num_dofs=num_dofs, weights=self.weights[idx]) + + # set block diagonal basis matrix + promp.Phi = self.Phi[idx] + + # return ProMP + return promp + + def __iter__(self): + """Iterate over the probabilistic movement primitives""" + for i in range(len(self)): + yield self[i] + + def __pow__(self, priority): + """Set the priority""" + self.power(priority=priority) + + def __mul__(self, other): + """Multiply this ProMP with another one""" + return self.multiply(other) + + def __rmul__(self, other): + """Multiply this ProMP with another one""" + return self.multiply(other) + + + +class DiscreteProMP(ProMP): + r"""Discrete ProMP + + ProMP to be used for discrete / stroke-based movements. + """ + def __init__(self, num_dofs, num_basis, weights=None, canonical_system=None, noise_covariance=1., + basis_width=None): + """ + Initialize the Discrete ProMP. + + Args: + num_dofs (int): number of degrees of freedom (denoted by `D`) + num_basis (int): number of basis functions (denoted by `M`) + weights (np.array[DM], Gaussian, None): the weights that can be optimized. If None, it will create a + custom weight array. + canonical_system (CS, None): canonical system. If None, it will create a Linear canonical system that goes + from `t0=0` to `tf=1`. + noise_covariance (np.array[2D,2D]): covariance noise matrix + basis_width (None, float): width of the basis. By default, it will be 1./(2*num_basis) such that the + basis_width represents the standard deviation, and such that 2*std_dev = 1./num_basis. + """ + super(DiscreteProMP, self).__init__(num_dofs=num_dofs, weight_size=num_dofs*num_basis, weights=weights, + canonical_system=canonical_system, noise_covariance=noise_covariance) + + # define the basis width if not defined + if basis_width is None: + basis_width = 1./(2*num_basis) + + # create Gaussian basis matrix with shape: DMx2D + if num_dofs == 1: + self.Phi = GaussianBM(self.cs, num_basis, basis_width=basis_width) + else: + self.Phi = BlockDiagonalMatrix([GaussianBM(self.cs, num_basis, basis_width=basis_width) + for _ in range(num_dofs)]) + + + +class RhythmicProMP(ProMP): + r"""Rhythmic ProMP + + ProMP to be used for rhythmic movements. + """ + + def __init__(self, num_dofs, num_basis, weights=None, canonical_system=None, noise_covariance=1., + basis_width=None): + """ + Initialize the Rhythmic ProMP. + + Args: + num_dofs (int): number of degrees of freedom (denoted by `D`) + num_basis (int): number of basis functions (denoted by `M`) + weights (np.array[DM], Gaussian, None): the weights that can be optimized. If None, it will create a + custom weight array. + canonical_system (CS, None): canonical system. If None, it will create a Linear canonical system that goes + from `t0=0` to `tf=1`. + noise_covariance (np.array[2D,2D]): covariance noise matrix + basis_width (None, float): width of the basis. By default, it will be 1./(2*num_basis) such that the + basis_width represents the standard deviation, and such that 2*std_dev = 1./num_basis. + """ + super(RhythmicProMP, self).__init__(num_dofs=num_dofs, weight_size=num_dofs * num_basis, weights=weights, + canonical_system=canonical_system, noise_covariance=noise_covariance) + + # define the basis width if not defined + if basis_width is None: + basis_width = 1. / (2 * num_basis) + + # create Von-Mises basis matrix with shape: DMx2D + if num_dofs == 1: + self.Phi = VonMisesBM(self.cs, num_basis, basis_width=basis_width) + else: + self.Phi = BlockDiagonalMatrix([VonMisesBM(self.cs, num_basis, basis_width=basis_width) + for _ in range(num_dofs)]) + + + + +# TESTS +if __name__ == "__main__": + import matplotlib.pyplot as plt + + # num_basis, width = 10, 1. + # centers = np.linspace(-2 * width, 1 + 2 * width, num_basis) + # cs = LinearCS() + # phi = GaussianBF(centers, width) + # s = 0.5 + # #s = np.array([0.5, 0.6, 0.7]) + # print(phi(s).shape) # shape: TxM + # print(phi(s)) + # + # # create dphi function + # def dphi_t(cs, phi): + # def step(s): + # return phi.grad(s) * cs.grad() + # return step + # + # dphi = dphi_t(cs, phi) + # print(dphi(s).shape) + # print(dphi(s)) + # + # bm = GaussianBM(cs, num_basis, width) + # bm = VonMisesBM(cs, num_basis, width) + # print(bm(s).shape) + # print(bm(s)) + + def plot_state(Y, title=None, linewidth=1.): + y, dy = Y.T + plt.figure() + if title is not None: + plt.suptitle(title) + + # plot position y(t) + plt.subplot(1, 2, 1) + plt.title('y(t)') + plt.plot(y, linewidth=linewidth) # TxN + + # plot velocity dy(t) + plt.subplot(1, 2, 2) + plt.title('dy(t)') + plt.plot(dy, linewidth=linewidth) # TxN + + def plot_weigthed_basis(promp): + phi_track = promp.weighted_basis(t) # shape: DM,T,2D + + plt.subplot(1, 2, 1) + plt.plot(phi_track[:,:,0].T, linewidth=0.5) + + plt.subplot(1, 2, 2) + plt.plot(phi_track[:,:,1].T, linewidth=0.5) + + # create data and plot it + N = 8 + t = np.linspace(0., 1., 100) + eps = 0.1 + y = np.array([np.sin(2*np.pi*t) + eps * np.random.rand(len(t)) for _ in range(N)]) # shape: NxT + dy = np.array([2*np.pi*np.cos(2*np.pi*t) + eps * np.random.rand(len(t)) for _ in range(N)]) # shape: NxT + Y = np.dstack((y, dy)) # N,T,2D --> why not N,2D,T + plot_state(Y, title='Training data') + plt.show() + + # create discrete and rhythmic ProMP + promp = DiscreteProMP(num_dofs=1, num_basis=10, basis_width=1./20) + + # plot the basis function activations + plt.plot(promp.Phi(t)[:,:,0].T) + plt.title('basis functions') + plt.show() + + # plot ProMPs + y_pred = promp.rollout() + plot_state(y_pred[None], title='ProMP prediction before learning', linewidth=2.) # shape: N,T,2D + plot_weigthed_basis(promp) + plt.show() + + # learn from demonstrations + promp.imitate(Y) + y_pred = promp.rollout() + plot_state(y_pred[None], title='ProMP prediction after learning', linewidth=2.) # N,T,2D + plot_weigthed_basis(promp) + plt.show() + + # modulation: final positions (goals) + + # modulation: final velocities + + # modulation: via-points + + # combination/co-activation/superposition + + # blending diff --git a/pyrobolearn/optim/__init__.py b/pyrobolearn/optim/__init__.py new file mode 100644 index 0000000..9168742 --- /dev/null +++ b/pyrobolearn/optim/__init__.py @@ -0,0 +1,3 @@ + +# import optimizers +from optimizer import * diff --git a/pyrobolearn/optim/cio.py b/pyrobolearn/optim/cio.py new file mode 100644 index 0000000..10fdf24 --- /dev/null +++ b/pyrobolearn/optim/cio.py @@ -0,0 +1,65 @@ +# This file implements the 'Contact Invariant Optimization' framework developed by Igor Mordatch. +# Ref: "Automated Discovery and Learning of Complex Movement Behaviors" (PhD thesis), Mordatch, 2015 +# See also: presentation given CS294 + +import numpy as np +from scipy.interpolate as interp1d + + +class CIO(object): + r""" + The Contact Invariant Optimization (CIO) algorithm [1] consists to minimize the following cost: + + .. math:: + + s* = argmin_s L(s) + = argmin_s L_{CI}(s) + L_{physics}(s) + L_{task}(s) + L_{hint}(s) + + where :math:`s` is the state which contains :math:`x_k`, :math:`\dot{x}_k`, and :math:`c_k` for each phase/interval + :math:`k`. The vector :math:`x_k` contains the torso and end-effector position and orientation, while + the :math:`c_k` vector represents the auxiliary contact variables. + + Here is what each term represents in the total cost: + - :math:`L_{CI}` is the contact invariant cost + - :math:`L_{physics}` penalizes physics violation + - :math:`L_{task}` describes the task objectives (i.e. high-level goals of the movement) + - :math:`L_{hint}` provides hints to accelerate the optimization. This term is optional. + + The CIO consists of 3 phases: + 1. only :math:`L_{task}` is enabled + 2. All 4 terms (:math:`L_{task}`, :math:`L_{physics}`, :math:`L_{CI}`, :math:`L_{hint}`) are enabled but with + :math:`L_{physics}` down-weighted by 0.1 + 3. :math:`L_{task}`, :math:`L_{physics}`, and :math:`L_{CI}` are fully enabled + + Note that the solution obtained at the end of each phase is perturbed with small zero-mean Gaussian noise to + break any symmetries, and used to initialize the next phase. + + From the optimized state :math:`s^*`, the optimal joints :math:`q^*` at each time step can be computed (using IK). + Then, a PD controller can be used to move the joints to their desired configuration. + + Note that the framework do not take into account any sensory feedbacks. + + References: + [1] "Automated Discovery and Learning of Complex Movement Behaviors" (PhD thesis), Mordatch, 2015 + """ + + def __init__(self, robot, T, num_interval=20): + # TODO: think about optimizing multiple actors + self.robot = robot + self.K = num_interval + + x = np.linspace(0, T, self.K) + y = np.array(range(1, self.K+1)) + self.phase = scipy.interpolate.interp1d(x, y, kind='zero') + + def get_phase_index(self, t): + return self.phase(t) + + def compute_state(self): + base_pos = self.robot.getBasePosition() + base_quat = self.robot.getBaseOrientation() + end_effector_pos = self.robot.getEndEffectorPositions() + end_effector_quat = self.robot.getEndEffectorOrientations() + + def optimize(self): + pass \ No newline at end of file diff --git a/pyrobolearn/optim/optimizer.py b/pyrobolearn/optim/optimizer.py new file mode 100644 index 0000000..3ce00e5 --- /dev/null +++ b/pyrobolearn/optim/optimizer.py @@ -0,0 +1,786 @@ +#!/usr/bin/env python +"""Provide various optimizers. + +Optimizers allows to optimize (i.e. minimize or maximize) a utility function (also known as objective function, +fitness function, loss, etc.) with or without constraints and bounds. Notably, it assumes the functions have some +parameters that the optimizer can update. + +Mathematically, this is described as: + +.. math:: \min_{x \in R^n} f(x) + +subject to + +.. math:: + + g_i(x) \geq 0, \quad i = 1,...,m + h_j(x) = 0, \quad j = 1,...,p + x_l \leq x \leq x_u + +For trajectory optimization, check "An Introduction to Trajectory Optimization: How to do your own Direct Collocation". +""" + +# TODO: trajectory optimization + +from abc import ABCMeta, abstractmethod + +# Numpy with autograd +import autograd.numpy as np # Thinly-wrapped numpy +from autograd import grad # The only autograd function you may ever need + +# Scipy optimizer +import scipy + +# Pytorch optimizers +import torch.nn as nn +import torch.optim as optim + +# NLopt optimizers +try: + import nlopt +except ImportError as e: + raise ImportError(e.__str__() + "\n HINT: you can install nlopt via `pip install nlopt`.") + +# IPopt optimizer +try: + import ipopt +except ImportError as e: + raise ImportError(e.__str__() + "\n HINT: you can install ipopt using the `pyrobolearn/scripts/install_ipopt.sh`." + "If ipopt is already installed, you can install the python wrapper via " + "`pip install ipopt`.") + +# CVXOPT +# import cvxopt +# CVXPY: nice wrapper around cvxopt +# import cvxpy +# Quadprog +# import quadprog + +# QPsolvers optimizers: unified Python interface for multiple QP solvers (cvxopt, cvxpy, quadprog,...) +try: + import qpsolvers +except ImportError as e: + raise ImportError(e.__str__() + "\n HINT: you can install qpsolvers directly via 'pip install qpsolvers'.") + +# Bayesian optimization +try: + import GPy + import GPyOpt +except ImportError as e: + raise ImportError(e.__str__() + "\n HINT: you can install GPy/GPyOpt directly via 'pip install GPy' and " + "'pip install GPyOpt'.") + +# CMA-ES +try: + import cma +except ImportError as e: + raise ImportError(e.__str__() + "\n HINT: you can install CMA-ES or `pycma` directly via 'pip install cma'.") + + +__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" + + +################################################################## +# OPTIMIZER # +################################################################## + + +class Optimizer(object): + r"""Optimizer abstract class + + This is an abstract class from which all optimizers inherit from. Most of the child optimizer classes are wrappers + around the original optimizer. This is to provide the same common interface to all the optimizers, and convert + seamlessly to the correct data types. + + Optimizers are often given as a parameter to the learning algorithms, but can also be used of out of the box + directly on models. Several original optimizers can be given to the learning algorithm which will automatically + wrap the optimizer with the corresponding wrapper to provide a common interface. + + In their most natural form, optimizers are used to ... optimization process which can be described mathematically + by: + + .. math:: + + \min_{\theta} J(\theta) \mbox{ subj. to constraints} + + \max_{\theta} J(\theta) + + Several optimizers provides + + The list of optimizers available are from the following libraries: + * nlopt + * ipopt + * torch.optim + * GPy / GPyOpt + * scipy.optimize + * qpsolvers (which includes cvxpy) + * cmaes + * pso + + Each one of them expect a certain kind of type of the parameters. + + Optimizers can be divided into: + * global vs local + * derivative-free vs gradient-based + * without constraints vs with (equality and/or inequality) constraints + + Many implemented optimizers that can be found online are specific to a certain type of learning model. + If necessary, a conversion or wrapping process is carried out to make the optimizer work with the given learning + model. + """ + __metaclass__ = ABCMeta + + def __init__(self, model, losses, hyperparameters): + """ + + :param model: a certain type of model. If the original model is given instead of an instance of `Model`, then + the model will be wrapped appropriately. + :param losses: + :param hyperparameters: + """ + pass + + +################################################################## +# SCIPY # +################################################################## + +class Scipy(object): + r"""Scipy optimizer + + This uses the `scipy.optimize.minimize` to optimize a given objective function under various bounds and + constraints. Specifically, it consists of the minimization of a scalar function of one or more variables. + In general, the optimization problems are of the form: + + .. math:: + + \min_{x \in R^n} f(x) + + subject to + + .. math:: + + g_i(x) \geq 0, \quad i = 1,...,m + h_j(x) = 0, \quad j = 1,...,p + + where :math:`x` is a vector of one or more variables, :math:`g_i(x)` are the inequality constraints, and + :math:`h_j(x)` are the equality constrains. + + Optionally, the lower and upper bounds for each element in :math:`x` can also be specified using the `bounds` + argument. + + Several methods/optimizers are available: + - + + Note that only 'COBYLA' and 'SLSQP' support constraints, where the former only supports inequality constraints. + + References: + [1] scipy.optimize.minimize: https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.minimize.html + """ + + def __init__(self, method='SLSQP'): + """ + Initialize the scipy method + + Args: + method (str, callable): + - 'Nelder-Mead' :ref:`(see here) ` + - 'Powell' :ref:`(see here) ` + - 'CG' :ref:`(see here) ` + - 'BFGS' :ref:`(see here) ` + - 'Newton-CG' :ref:`(see here) ` + - 'L-BFGS-B' :ref:`(see here) ` + - 'TNC' :ref:`(see here) ` + - 'COBYLA' :ref:`(see here) ` + - 'SLSQP' :ref:`(see here) ` + - 'dogleg' :ref:`(see here) ` + - 'trust-ncg' :ref:`(see here) ` + - custom - a callable object (added in version 0.14.0), + + """ + # define optimization method + # By default, it will be 'BFGS', 'L-BFGS-B', or 'SLSQP' depending on the constraints and bounds + # If constraints, it can only be 'COBYLA' or 'SLSQP'. COBYLA only supports inequality constraints. + self.method = method + + def optimize(self, maxiter=1e6, verbose=True): + # define objective function to MINIMIZE + # f = lambda x: -(x.T.dot(C)).dot(x) + def f(x): + return -(x.T.dot(C)).dot(x) + + # define initial guess + x0 = np.ones((M,)) # np.zeros((M,)) + + # define 1st constraints: norm of 1 + constraints = [{'type': 'eq', 'fun': lambda x: x.T.dot(x) - 1, 'jac': None, 'args': ()}] + + # define bounds: each vector u have a norm of 1 thus each parameter is between -1 and 1 + bounds = [(-1., 1.)] * M + + # optimize recursively + evals, evecs = [], [] + messages = {} + options = {'maxiter': maxiter, 'disp': verbose} + for i in range(M): + if i != 0: + # add orthogonality constraint + constraints.append({'type': 'eq', 'fun': lambda u: u1.T.dot(u)}) + + # minimize --> it returns an instance of OptimizeResult + result = scipy.optimize.minimize(f, x0, args=(), method=self.method, jac=None, hess=None, bounds=bounds, + constraints=constraints, tol=None, callback=None, options=options) + + print(result.success) + print(result.message) + print(result.fun) + print(result.x) + + +################################################################## +# Quadratic Programming # +################################################################## + +# class CVXOPT(Optimizer): +# r"""Convex Optimizer +# +# Note: cvxpy module is a nice wrapper around cvxopt that follows paradigm of a disciplined convex programming. +# +# References: +# [1] Python Software for Convex Optimization: https://cvxopt.org/ +# [2] Github repo: https://github.com/cvxopt/cvxopt +# """ +# pass +# +# +# class CVXPY(Optimizer): +# r"""Convex Optimizer +# +# References: +# [1] CVXPY: http://www.cvxpy.org/ +# [2] Github repo: https://github.com/cvxgrp/cvxpy +# """ +# pass +# +# +# class QuadProg(object): +# r"""Quadprog +# +# References: +# [1] Github repo: https://github.com/rmcgibbo/quadprog +# """ +# pass + +class QP(object): + r"""Quadratic Programming solvers + + This class uses the `qpsolvers` which is a unified Python interface for multiple QP solvers [1,2]. + + .. math:: + + \min_{x \in R^n} \frac{1}{2} x^T P x + q^T x + + subject to + + .. math:: + + Gx \leq h + Ax = b + + where :math:`x` is the vector of optimization variables, the matrix :math:`P` and vector :math:`q` are used to + define any quadratic objective function on these variables, while the matrix-vector couples :math:`(G,h)` and + :math:`(A,b)` respectively define inequality and equality constraints. Vector inequalities apply coordinate by + coordinate [1]. + + - Dense solvers: + - CVXOPT + - CVXPY + - qpOASES + - quadprog + - Sparse solvers: + - ECOS as wrapped by CVXPY + - Gurobi + - MOSEK + - OSQP + + Check the available solvers by calling `print(qpsolvers.available_solvers)`. + + Notes: Many solvers (including CVXOPT, OSQP and quadprog) assume that `P` is a symmetric matrix, and may return + erroneous results when that is not the case. You can set ``sym_proj=True`` to project `P` on its symmetric part, + at the cost of some computation time. + + References: + [1] QP in Python: https://scaron.info/blog/quadratic-programming-in-python.html + [2] Github repo: https://github.com/stephane-caron/qpsolvers + """ + + def __init__(self, method='quadprog'): + """ + Initialize the QP solver. + + Args: + method (str): ['cvxopt', 'cvxpy', 'ecos', 'gurobi', 'mosek', 'osqp', 'qpoases', 'quadprog'] + """ + solvers = set(qpsolvers.available_solvers) + if len(solvers) == 0: + raise ValueError("No QP solvers have been found on this computer. Please install one of the QP modules") + if method not in solvers: + method = 'quadprog' + self.method = method + + # check methods that require a symmetric matrix for P + methods = ['cvxopt', 'osqp', 'quadprog'] + self.sym_proj = True if self.method in set(methods) else False + + def is_symmetric(self, X, tol=1e-8): + return np.allclose(X, X.T, atol=tol) + + def optimize(self, P, q, x0=None, G=None, h=None, A=None, b=None): + return qpsolvers.solve_qp(P, q, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj) + + +################################################################## +# NLOPT # +################################################################## + +class NLopt(object): + r"""Non-Linear Optimizer + + Non-linear optimizers based on the `nlopt` libraries. + + Here is a brief of lists of the current algorithms implemented: + * + + Nonlinear optimization algos that can handle nonlinear inequality and EQUALITY constraints are: + - ISRES (Improved Stochastic Ranking Evolution Strategy) --> global derivative-free + - COBYLA (Constrained Optimization BY Linear Approximations) --> local derivative-free + - SLSQP (Sequential Least-SQuares Programming) --> local gradient-based + - AUGLAG (AUGmented LAGrangian) --> global/local derivative-free/gradient based (determined based on the + subsidiary algo) + + More information about: + - algorithms: https://nlopt.readthedocs.io/en/latest/NLopt_Algorithms/ + + References: + [1] NLopt: https://nlopt.readthedocs.io/en/latest/ + [2] NLopt with Python: with Python: https://nlopt.readthedocs.io/en/latest/NLopt_Python_Reference/ + [3] Github repo: https://github.com/stevengj/nlopt + """ + + # def __init__(self, model, losses, hyperparameters, seed): + # super(NLopt, self).__init__(model, losses, hyperparameters) + + def __init__(self, method, submethod=None, seed=None): + + # define useful variables + self.results = {1: 'success', 2: 'stop_val reached', 3: 'ftol reached', 4: 'xtol reached', + 5: 'maxeval reached', 6: 'maxtime reached', -1: 'failure', -2: 'invalid args', + -3: 'out of memory', -4: 'roundoff limited', -5: 'forced stop'} + + # define random seed + nlopt.srand(seed) + + # define which solver to use + def get_opt(method): + if method == 'ISRES': + return nlopt.opt(nlopt.GN_ISRES, M) + elif method == 'COBYLA': + return nlopt.opt(nlopt.LN_COBYLA, M) + elif method == 'SLSQP': + return nlopt.opt(nlopt.LD_SLSQP, M) + elif method == 'AUGLAG': + return nlopt.opt(nlopt.AUGLAG, M) + else: + raise NotImplementedError("The given method has not been implemented") + + if method is None: + method = 'SLSQP' + self.opt = get_opt(method) + + # define subsolver to use (if we use the AUGLAG method) + if method == 'AUGLAG': + if submethod is None: + submethod = 'SLSQP' + elif submethod == 'AUGLAG': + raise ValueError("Submethod should be different from AUGLAG") + subopt = get_opt(submethod) + subopt.set_lower_bounds(-1) + subopt.set_upper_bounds(1) + # subopt.set_ftol_rel(1e-2) + # subopt.set_maxeval(100) + self.opt.set_local_optimizer(subopt) + + def optimize(self): + # define objective function and its gradient + def f(x, grad): + if grad.size > 0: + grad[:] = 2 * x.T.dot(C) + return x.T.dot(C).dot(x) + + # define objective function to maximize + self.opt.set_max_objective(f) + + # if nlopt.GN_ISRES, we can define the population size + self.opt.set_population(0) # by default for ISRES: pop=20*(M+1) + + # define bound constraints (should be between -1 and 1 because the norm should be 1) + self.opt.set_lower_bounds(-1.) + self.opt.set_upper_bounds(1.) + + # define norm constraint and its gradient + def c1(x, grad): + if grad.size > 0: + grad[:] = 2 * x + return (x.T.dot(x) - 1) + + # define orthogonal constraint + class OrthogonalConstraint(object): + + def __init__(self, v): + self.v = np.copy(v) + + def constraint(self, x, grad): + if grad.size > 0: + grad[:] = self.v + return (x.T.dot(self.v)) + + # define equality constraints + opt.add_equality_constraint(c1, 0) + # opt.add_equality_mconstraint(constraints, tol) + + # define stopping criteria + # opt.set_stopval(stopval) + opt.set_ftol_rel(1e-8) + # opt.set_xtol_rel(1e-4) + opt.set_maxeval(100000) # nb of iteration + opt.set_maxtime(2) # time in secs + + # define initial value + x0 = np.array([0.1] * M) # important that the initial value != 0 for the computation of the grad! + + evals, evecs, msgs = [], [], {} + for i in range(M): + # add constraint + if i > 0: + c = OrthogonalConstraint(x) + opt.add_equality_constraint(c.constraint, 0) + + # optimize + try: + x = opt.optimize(x0) + except nlopt.RoundoffLimited as e: + pass + + # save values + evecs.append(x) # param vector + evals.append(opt.last_optimum_value()) # max value + msgs[i] = nlopt_results[opt.last_optimize_result()] + + +################################################################## +# IPOPT # +################################################################## + +class NormConstraint(object): + + def __init__(self): + pass + + def constraint(self, x): + return x.T.dot(x) + + def jacobian(self, x): + return 2 * x + + +class OrthogonalConstraint(object): + + def __init__(self, v): + self.v = np.copy(v) + + def constraint(self, x): + return x.T.dot(self.v) + + def jacobian(self, x): + return self.v + + +class _IPopt(object): + + def __init__(self, verbose=True): + self.verbose = verbose + self.iter_count = 0 + self.constraints = [] + + def add_constraint(self, constraint): + self.constraints.append(constraint) + + def objective(self, x): + # objective fct to minimize + return -x.T.dot(C).dot(x) + + def gradient(self, x): + # grad of the objective fct + return -2 * x.T.dot(C) + + def constraints(self, x): + return np.array([c.constraint(x) for c in self.constraints]) + + def jacobian(self, x): + return np.array([c.jacobian(x) for c in self.constraints]) + + # def hessian(self, x): + # pass + + def intermediate(self, alg_mod, iter_count, obj_value, inf_pr, inf_du, mu, d_norm, + regularization_size, alpha_du, alpha_pr, ls_trials): + if self.verbose: + print("Objective value at iteration #%d: %g" % (iter_count, obj_value)) + self.iter_count = iter_count + + +class IPopt(Optimizer): + r"""Interior-Point optimizer + + This is a wrapper around the `ipopt` library. It can be used to solve general nonlinear programming problems of + the form: + + .. math:: + + \min_{x \in R^n} f(x) + + subject to + + .. math:: + + g_L \leq g(x) \leq g_U + + x_L \leq x \leq x_U + + where :math:`x` are the optimization variables (possibly with upper an lower bounds, :math:`x_U` and :math:`x_L` + respectively), :math:`f(x)` is the objective function and :math:`g(x)` are the general nonlinear constraints. + The constraints, :math:`g(x)`, have lower and upper bounds. Note that equality constraints can be specified + by setting :math:`g^i_L = g^i_U`. + + More info: + - Check the documentation of the `ipopt.problem` method + + References: + [1] "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear + programming", Wachter and Biegler, 2004 + [2] Ipopt: https://projects.coin-or.org/Ipopt + [3] Ipopt in Python: https://pythonhosted.org/ipopt/ + [4] Repos: https://github.com/coin-or/Ipopt and https://pypi.org/project/ipopt/ + """ + + def __init__(self, model, losses, hyperparameters): + super(IPopt, self).__init__(model, losses, hyperparameters) + + def optimize(self): + # define initial value + x0 = np.array([0.1] * N) # important that the initial value != 0 for the computation of the grad! + + # define (lower and upper) bound constraints + lb = [-1] * N + ub = [1] * N + + # define constraints; if upper and lower constraints (resp. cu and cl) are equal then equality constraint + cl = [1] + [0] * (N - 1) + cu = [1] + [0] * (N - 1) + + # create ipopt (which contains the objective function, its gradients, and constraints) + opt = _IPopt(verbose=False) + opt.add_constraint(NormConstraint()) + opt.add_constraint(OrthogonalConstraint(x)) + + # define the nonlinear optimization problem + nlp = ipopt.problem(n=N, m=len(cl[:i]), problem_obj=opt, lb=lb, ub=ub, cl=cl[:i], cu=cu[:i]) + + # solve problem + x, info = nlp.solve(x0) + + return x + + +################################################################## +# PYTORCH OPTIMIZERS # +################################################################## + + +class PyTorchOpt(Optimizer): + r"""PyTorch Optimizers + + This is a wrapper around the optimizers from pytorch. + """ + + def __init__(self, model, losses, hyperparameters): + super(PyTorchOpt, self).__init__(model, losses, hyperparameters) + + def add_constraint(self): + # it will add a constraint as the augmented lagrangian + pass + + +class Adam(object): + r"""Adam Optimizer + + References: + [1] "Adam: A Method for Stochastic Optimization", Kingma et al., 2014 + """ + + def __init__(self, learning_rate=1e-3, betas=(0.9, 0.999), eps=1e-08, weight_decay=0, amsgrad=False, + max_grad_norm=None): # 0.5 + self.optimizer = None + self.learning_rate = learning_rate + self.betas = betas + self.eps = eps + self.weight_decay = weight_decay + self.amsgrad = amsgrad + self.max_grad_norm = max_grad_norm + + def reset(self): + self.optimizer = None + + def optimize(self, params, loss): + # create optimizer if necessary + if self.optimizer is None: + self.optimizer = optim.Adam(params, lr=self.learning_rate, betas=self.betas, eps=self.eps, + weight_decay=self.weight_decay, amsgrad=self.amsgrad) + + # optimize + self.optimizer.zero_grad() + loss.backward() + if self.max_grad_norm is not None: + nn.utils.clip_grad_norm_(params, self.max_grad_norm) + self.optimizer.step() + + +class Adadelta(object): + r"""Adadelta Optimizer + + References: + [1] "ADADELTA: An Adaptive Learning Rate Method", Zeiler, 2012 + """ + + def __init__(self, learning_rate=1., rho=0.9, eps=1e-6, weight_decay=0, max_grad_norm=None): #0.5 + self.optimizer = None + self.learning_rate = learning_rate + self.rho = rho + self.eps = eps + self.weight_decay = weight_decay + self.max_grad_norm = max_grad_norm + + def optimize(self, params, loss): + if self.optimizer is None: + self.optimizer = optim.Adadelta(params, lr=self.learning_rate, rho=self.rho, eps=self.eps, + weight_decay=self.weight_decay) + + # optimize + self.optimizer.zero_grad() + loss.backward() + if self.max_grad_norm is not None: + nn.utils.clip_grad_norm_(params, self.max_grad_norm) + self.optimizer.step() + + +class Adagrad(object): + r"""Adagrad Optimizer + + References: + [1] "Adaptive Subgradient Methods for Online Learning and Stochastic Optimization", Duchi et al., 2011 + """ + + def __init__(self, learning_rate=0.01, learning_rate_decay=0, weight_decay=0, initial_accumumaltor_value=0, + max_grad_norm=None): # 0.5 + self.optimizer = None + self.learning_rate = learning_rate + self.learning_rate_decay = learning_rate_decay + self.weight_decay = weight_decay + self.initial_accumulator_value = initial_accumumaltor_value + self.max_grad_norm = max_grad_norm + + def optimize(self, params, loss): + if self.optimizer is None: + self.optimizer = optim.Adagrad(params, lr=self.learning_rate, lr_decay=self.learning_rate_decay, + weight_decay=self.weight_decay, + initial_accumulator_value=self.initial_accumulator_value) + + # optimize + self.optimizer.zero_grad() + loss.backward() + if self.max_grad_norm is not None: + nn.utils.clip_grad_norm_(params, self.max_grad_norm) + self.optimizer.step() + + +class RMSprop(object): + r"""RMSprop + + References: + [1] "RMSprop: Divide the gradient by a running average of its recent magnitude" (lecture 6.5), Tieleman and + Hinton, 2012 + [2] "Generating Sequences With Recurrent Neural Networks", Graves, 2014 + """ + + def __init__(self, learning_rate=1e-2, alpha=0.99, eps=1e-8, weight_decay=0, momentum=0, centered=False, + max_grad_norm=None): # 0.5 + self.optimizer = None + self.learning_rate = learning_rate + self.alpha = alpha + self.eps = eps + self.weight_decay = weight_decay + self.momentum = momentum + self.centered = centered + self.max_grad_norm = max_grad_norm + + def optimize(self, params, loss): + if self.optimizer is None: + self.optimizer = optim.RMSprop(params, lr=self.learning_rate, alpha=self.alpha, eps=self.eps, + weight_decay=self.weight_decay, momentum=self.momentum, + centered=self.centered) + + # optimize + self.optimizer.zero_grad() + loss.backward() + if self.max_grad_norm is not None: + nn.utils.clip_grad_norm_(params, self.max_grad_norm) + self.optimizer.step() + + +class SGD(object): + r"""Stochastic Gradient Descent + + References: + [1] "A Stochastic Approximation Method", Robbins and Monro, 1951 + [2] "On the importance of initialization and momentum in deep learning", Sutskever et al., 2013 + """ + + def __init__(self, learning_rate=1e-3, momentum=0, dampening=0, weight_decay=0, nesterov=False, + max_grad_norm=None): #0.5 + self.optimizer = None + self.learning_rate = learning_rate + self.momentum = momentum + self.dampening = dampening + self.weight_decay = weight_decay + self.nesterov = nesterov + self.max_grad_norm = max_grad_norm + + def optimize(self, params, loss): + # create optimizer if necessary + if self.optimizer is None: + self.optimizer = optim.SGD(params, lr=self.learning_rate, momentum=self.momentum, dampening=self.dampening, + weight_decay=self.weight_decay, nesterov=self.nesterov) + + # optimize + self.optimizer.zero_grad() + loss.backward() + if self.max_grad_norm is not None: + nn.utils.clip_grad_norm_(params, self.max_grad_norm) + self.optimizer.step() diff --git a/pyrobolearn/rewards/reward.py b/pyrobolearn/rewards/reward.py index 606c239..dee0dbe 100644 --- a/pyrobolearn/rewards/reward.py +++ b/pyrobolearn/rewards/reward.py @@ -27,7 +27,7 @@ from pyrobolearn.actions import * __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/basic_states.py b/pyrobolearn/states/basic_states.py index c9d912c..650859a 100644 --- a/pyrobolearn/states/basic_states.py +++ b/pyrobolearn/states/basic_states.py @@ -12,7 +12,7 @@ from pyrobolearn.actions import Action __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/gym_states.py b/pyrobolearn/states/gym_states.py index 7fb4861..536f1bc 100644 --- a/pyrobolearn/states/gym_states.py +++ b/pyrobolearn/states/gym_states.py @@ -13,7 +13,7 @@ from state import State __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/object_states.py b/pyrobolearn/states/object_states.py index 5a38844..766b1ce 100644 --- a/pyrobolearn/states/object_states.py +++ b/pyrobolearn/states/object_states.py @@ -13,7 +13,7 @@ from pyrobolearn.robots import Object __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/robot_states/joint_states.py b/pyrobolearn/states/robot_states/joint_states.py index e5b0b63..60963fd 100644 --- a/pyrobolearn/states/robot_states/joint_states.py +++ b/pyrobolearn/states/robot_states/joint_states.py @@ -11,7 +11,7 @@ from robot_states import RobotState __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/robot_states/link_states.py b/pyrobolearn/states/robot_states/link_states.py index df2aea7..ac2182b 100644 --- a/pyrobolearn/states/robot_states/link_states.py +++ b/pyrobolearn/states/robot_states/link_states.py @@ -11,7 +11,7 @@ from robot_states import RobotState __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/robot_states/robot_states.py b/pyrobolearn/states/robot_states/robot_states.py index db621a1..dae8159 100644 --- a/pyrobolearn/states/robot_states/robot_states.py +++ b/pyrobolearn/states/robot_states/robot_states.py @@ -17,7 +17,7 @@ from pyrobolearn.robots import Robot __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/robot_states/sensor_states.py b/pyrobolearn/states/robot_states/sensor_states.py index d64dcae..344831f 100644 --- a/pyrobolearn/states/robot_states/sensor_states.py +++ b/pyrobolearn/states/robot_states/sensor_states.py @@ -11,7 +11,7 @@ from robot_states import RobotState __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/state.py b/pyrobolearn/states/state.py index 68d3197..5c80697 100644 --- a/pyrobolearn/states/state.py +++ b/pyrobolearn/states/state.py @@ -16,7 +16,7 @@ from pyrobolearn.utils.data_structures.orderedset import OrderedSet __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com" diff --git a/pyrobolearn/states/time_states.py b/pyrobolearn/states/time_states.py index dc45a38..ce8b55d 100644 --- a/pyrobolearn/states/time_states.py +++ b/pyrobolearn/states/time_states.py @@ -13,7 +13,7 @@ from state import State __author__ = "Brian Delhaisse" __copyright__ = "Copyright 2018, PyRoboLearn" __credits__ = ["Brian Delhaisse"] -__license__ = "(c) Brian Delhaisse" +__license__ = "MIT" __version__ = "1.0.0" __maintainer__ = "Brian Delhaisse" __email__ = "briandelhaisse@gmail.com"