mirror of
https://github.com/wassname/pyrobolearn.git
synced 2026-09-09 11:31:38 +08:00
clean models
This commit is contained in:
Brian Delhaisse
@@ -122,12 +122,12 @@ class GMM(object):
|
||||
|
||||
|
||||
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
|
||||
- [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
|
||||
@@ -557,7 +557,7 @@ class GMM(object):
|
||||
float: posterior
|
||||
|
||||
References:
|
||||
[1] "Pattern Recognition and Machine Learning" (eq. 9.75), Bishop, 2006
|
||||
- [1] "Pattern Recognition and Machine Learning" (eq. 9.75), Bishop, 2006
|
||||
"""
|
||||
# if hidden variable is an index
|
||||
if isinstance(z, int):
|
||||
@@ -605,7 +605,7 @@ class GMM(object):
|
||||
float: log posterior
|
||||
|
||||
References:
|
||||
[1] "Pattern Recognition and Machine Learning" (eq. 9.75), Bishop, 2006
|
||||
- [1] "Pattern Recognition and Machine Learning" (eq. 9.75), Bishop, 2006
|
||||
"""
|
||||
return np.log(self.posterior_pdf(x, z))
|
||||
|
||||
@@ -642,12 +642,12 @@ class GMM(object):
|
||||
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)
|
||||
|
||||
References:
|
||||
- [1] "Pattern Recognition and Machine Learning" (chap 9.4), Bishop, 2006
|
||||
"""
|
||||
# get useful variables
|
||||
responsibilities = self.responsibilities(x) # shape: K if one data point, otherwise NxK
|
||||
@@ -801,7 +801,7 @@ class GMM(object):
|
||||
data:
|
||||
|
||||
References:
|
||||
[1] "Robot Programming by Demonstration: A Probabilistic Approach", Calinon, 2009, Chap 2.9.3
|
||||
- [1] "Robot Programming by Demonstration: A Probabilistic Approach", Calinon, 2009, Chap 2.9.3
|
||||
"""
|
||||
pass
|
||||
|
||||
@@ -1040,9 +1040,9 @@ class GMM(object):
|
||||
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
|
||||
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"""
|
||||
@@ -1126,8 +1126,8 @@ class GMM(object):
|
||||
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
|
||||
- [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]
|
||||
@@ -1384,7 +1384,7 @@ class GMM(object):
|
||||
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
|
||||
- [1] "The Matrix Cookbook" (math.uwaterloo.ca/~hwolkowi/matrixcookbook.pdf), Petersen and Pedersen, 2012
|
||||
"""
|
||||
# TODO: check when x is a matrix
|
||||
|
||||
@@ -1614,7 +1614,7 @@ class VBGMM(GMM):
|
||||
as much so as to render usage unpractical." from [1]
|
||||
|
||||
References:
|
||||
[1] sklearn
|
||||
- [1] sklearn
|
||||
"""
|
||||
|
||||
def __init__(self, num_components):
|
||||
@@ -1625,7 +1625,7 @@ class TPGMM(GMM):
|
||||
r"""Task-Parametrized Gaussian Mixtured Model
|
||||
|
||||
References:
|
||||
[1] "A Tutorial on Task-Parameterized Movement Learning and Retrieval", Calinon, 2015
|
||||
- [1] "A Tutorial on Task-Parameterized Movement Learning and Retrieval", Calinon, 2015
|
||||
"""
|
||||
|
||||
def __init__(self, num_frames, num_components):
|
||||
@@ -1734,7 +1734,6 @@ if __name__ == "__main__":
|
||||
|
||||
# samples from the GMM and plot
|
||||
|
||||
|
||||
# GMR: condition on the input variable and plot
|
||||
|
||||
# GMR: condition on the output variable and plot
|
||||
|
||||
@@ -3,6 +3,10 @@
|
||||
|
||||
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`.
|
||||
|
||||
References:
|
||||
- [1] "Kernelized Movement Primitives", Huang et al., 2017
|
||||
- [2] https://github.com/yanlongtu/robInfLib
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
@@ -15,7 +19,7 @@ from pyrobolearn.models.gmm import GMM, Gaussian
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse"]
|
||||
__credits__ = ["Yanlong Huang (paper + Matlab)", "Brian Delhaisse (Python)"]
|
||||
__license__ = "GNU GPLv3"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
@@ -24,13 +28,13 @@ __status__ = "Development"
|
||||
|
||||
|
||||
class RBF(object):
|
||||
"""
|
||||
r"""
|
||||
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.
|
||||
the length scale.
|
||||
"""
|
||||
|
||||
def __init__(self, variance=1., lengthscale=1.):
|
||||
@@ -45,11 +49,11 @@ class RBF(object):
|
||||
self.l = lengthscale
|
||||
|
||||
def k(self, x1, x2=None):
|
||||
"""
|
||||
r"""
|
||||
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.
|
||||
the length scale.
|
||||
|
||||
Args:
|
||||
x1 (float, np.array): 1st value
|
||||
@@ -78,7 +82,7 @@ class KMP(object):
|
||||
model.
|
||||
|
||||
References:
|
||||
[1] "Kernelized Movement Primitives", Huang et al., 2017
|
||||
- [1] "Kernelized Movement Primitives", Huang et al., 2017
|
||||
"""
|
||||
|
||||
def __init__(self, kernel_fct=None, database=None):
|
||||
@@ -86,8 +90,9 @@ class KMP(object):
|
||||
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.
|
||||
kernel_fct (None, callable): kernel function. If None, it will use the `RBF` kernel with a variance
|
||||
of 1, and a length scale of 2.
|
||||
database (None, list): initial database.
|
||||
"""
|
||||
super(KMP, self).__init__()
|
||||
|
||||
@@ -204,14 +209,14 @@ class KMP(object):
|
||||
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
|
||||
distance (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
|
||||
database_threshold (float): threshold associated with the `distance` 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.
|
||||
database. If True, it doesn't use the `distance` 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)
|
||||
@@ -406,12 +411,12 @@ class KMP(object):
|
||||
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
|
||||
database_threshold (float): threshold associated with the `distance` 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.
|
||||
database. If True, it doesn't use the `distance` 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)
|
||||
@@ -423,7 +428,7 @@ class KMP(object):
|
||||
`N` is the size of the kernel matrix.
|
||||
|
||||
References:
|
||||
[1] "Kernelized Movement Primitives", Huang et al., 2017
|
||||
- [1] "Kernelized Movement Primitives", Huang et al., 2017
|
||||
"""
|
||||
# TODO: replace gmm by joint generative model
|
||||
|
||||
@@ -545,7 +550,7 @@ class KMP(object):
|
||||
# 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
|
||||
k = k.reshape(-1, 1).T # shape: OxNO
|
||||
return k
|
||||
|
||||
def predict(self, x):
|
||||
@@ -667,7 +672,7 @@ class KMP(object):
|
||||
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
|
||||
threshold (float): threshold associated with the `distance` 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
|
||||
@@ -1019,3 +1024,4 @@ if __name__ == "__main__":
|
||||
# predict with KMP
|
||||
|
||||
# plot prediction
|
||||
pass
|
||||
|
||||
@@ -1,3 +1,16 @@
|
||||
|
||||
# import canonical system
|
||||
from .canonical_systems import LinearCS
|
||||
|
||||
# import basis functions and matrices
|
||||
from .basis_functions import BasisFunction, GaussianBF, VonMisesBF, BasisMatrix, GaussianBM, VonMisesBM, \
|
||||
BlockDiagonalMatrix
|
||||
|
||||
# import promp
|
||||
from .promp import *
|
||||
from .promp import ProMP
|
||||
|
||||
# import discrete promp
|
||||
from .promp import DiscreteProMP
|
||||
|
||||
# import rhythmic promp
|
||||
from .promp import RhythmicProMP
|
||||
|
||||
@@ -0,0 +1,496 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provides the various basis functions and matrices used in ProMPs.
|
||||
|
||||
A basis matrix contains the basis functions, and the derivative of the basis functions (with respect to the phase),
|
||||
and is callable (it accepts the phase as input).
|
||||
|
||||
References
|
||||
- [1] "Probabilistic Movement Primitives", Paraschos et al., 2013
|
||||
- [2] "Using Probabilistic Movement Primitives in Robotics", Paraschos et al., 2018
|
||||
"""
|
||||
|
||||
from abc import ABCMeta, abstractmethod
|
||||
import numpy as np
|
||||
from scipy.linalg import block_diag
|
||||
|
||||
from pyrobolearn.models.promp.canonical_systems import CS
|
||||
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse"]
|
||||
__license__ = "GNU GPLv3"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
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):
|
||||
r"""
|
||||
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):
|
||||
r"""
|
||||
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):
|
||||
r"""
|
||||
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):
|
||||
r"""
|
||||
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
|
||||
"""
|
||||
if not isinstance(cs, CS):
|
||||
raise TypeError("Expecting the ")
|
||||
|
||||
# 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
|
||||
|
||||
##############
|
||||
# Properties #
|
||||
##############
|
||||
|
||||
@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]
|
||||
|
||||
###########
|
||||
# Methods #
|
||||
###########
|
||||
|
||||
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])
|
||||
|
||||
#############
|
||||
# Operators #
|
||||
#############
|
||||
|
||||
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])
|
||||
|
||||
|
||||
# TESTS
|
||||
if __name__ == "__main__":
|
||||
from pyrobolearn.models.promp.canonical_systems import LinearCS
|
||||
|
||||
num_basis, width = 10, 1.
|
||||
centers = np.linspace(-2 * width, 1 + 2 * width, num_basis)
|
||||
cs = LinearCS()
|
||||
|
||||
# create basis functions
|
||||
|
||||
phi = GaussianBF(centers, width)
|
||||
s = 0.5
|
||||
# s = np.array([0.5, 0.6, 0.7])
|
||||
print("Gaussian basis function - phi(s) shape: {}".format(phi(s).shape)) # shape: (T,M)
|
||||
print("Gaussian basis function - phi(s) = {}".format(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: {}".format(dphi(s).shape))
|
||||
print("dphi(s) = {}".format(dphi(s)))
|
||||
|
||||
# create basis matrices
|
||||
|
||||
bm = GaussianBM(cs, num_basis, width)
|
||||
print("Gaussian basis matrix Phi(s) shape: {}".format(bm(s).shape)) # shape: (M,2)
|
||||
print("Gaussian basis matrix Phi(s) = {}".format(bm(s)))
|
||||
|
||||
bm = VonMisesBM(cs, num_basis, width)
|
||||
print("Von-Mises basis matrix Phi(s) shape: {}".format(bm(s).shape)) # shape: (M,2)
|
||||
print("Von-Mises basis matrix Phi(s) = {}".format(bm(s)))
|
||||
@@ -0,0 +1,167 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provides the canonical systems used in ProMPs.
|
||||
|
||||
This file defines the canonical systems used in ProMPs. The canonical system (CS) allows to modulate temporarily the
|
||||
ProMP, that is, it provides the phase that drives the ProMP [1].
|
||||
|
||||
References
|
||||
- [1] "Probabilistic Movement Primitives", Paraschos et al., 2013
|
||||
- [2] "Using Probabilistic Movement Primitives in Robotics", Paraschos et al., 2018
|
||||
"""
|
||||
|
||||
from abc import ABCMeta
|
||||
import numpy as np
|
||||
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse"]
|
||||
__license__ = "GNU GPLv3"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
class CS(object):
|
||||
r"""Canonical System
|
||||
"""
|
||||
|
||||
def step(self, tau=1., **kwargs):
|
||||
pass
|
||||
|
||||
def grad(self, t=None):
|
||||
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.
|
||||
self.s = self.t0
|
||||
|
||||
# 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):
|
||||
r"""
|
||||
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
|
||||
@@ -0,0 +1,132 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provides the discrete ProMP.
|
||||
|
||||
References
|
||||
- [1] "Probabilistic Movement Primitives", Paraschos et al., 2013
|
||||
- [2] "Using Probabilistic Movement Primitives in Robotics", Paraschos et al., 2018
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from pyrobolearn.models.promp.basis_functions import GaussianBM, BlockDiagonalMatrix
|
||||
from pyrobolearn.models.promp.promp import ProMP
|
||||
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse"]
|
||||
__license__ = "GNU GPLv3"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
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)])
|
||||
|
||||
|
||||
# TESTS
|
||||
if __name__ == "__main__":
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
|
||||
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_weighted_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_weighted_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_weighted_basis(promp)
|
||||
plt.show()
|
||||
|
||||
# modulation: final positions (goals)
|
||||
|
||||
# modulation: final velocities
|
||||
|
||||
# modulation: via-points
|
||||
|
||||
# combination/co-activation/superposition
|
||||
|
||||
# blending
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,133 @@
|
||||
#!/usr/bin/env python
|
||||
"""Provides the discrete ProMP.
|
||||
|
||||
References
|
||||
- [1] "Probabilistic Movement Primitives", Paraschos et al., 2013
|
||||
- [2] "Using Probabilistic Movement Primitives in Robotics", Paraschos et al., 2018
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
from pyrobolearn.models.promp.basis_functions import VonMisesBM, BlockDiagonalMatrix
|
||||
from pyrobolearn.models.promp.promp import ProMP
|
||||
|
||||
|
||||
__author__ = "Brian Delhaisse"
|
||||
__copyright__ = "Copyright 2018, PyRoboLearn"
|
||||
__credits__ = ["Brian Delhaisse"]
|
||||
__license__ = "GNU GPLv3"
|
||||
__version__ = "1.0.0"
|
||||
__maintainer__ = "Brian Delhaisse"
|
||||
__email__ = "briandelhaisse@gmail.com"
|
||||
__status__ = "Development"
|
||||
|
||||
|
||||
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
|
||||
|
||||
|
||||
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_weighted_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 = RhythmicProMP(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_weighted_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_weighted_basis(promp)
|
||||
plt.show()
|
||||
|
||||
# modulation: final positions (goals)
|
||||
|
||||
# modulation: final velocities
|
||||
|
||||
# modulation: via-points
|
||||
|
||||
# combination/co-activation/superposition
|
||||
|
||||
# blending
|
||||
Reference in New Issue
Block a user