Dynamical Movement Primitives (DMPs) are trajectory representations that
DMPs consist of superimposed movements:
Superimposed Movements
The forcing term can be learned from demonstrations (e.g. from human demonstrations) by linear regression.
Imitation Learning
# Enable inline plots, import NumPy and Matplotlib
%pylab inline
# Fix seed for random number generator to make the results repeatable
random.seed(0)
# Pretty matrix outputs
numpy.set_printoptions(precision=3, threshold=1000, edgeitems=5, linewidth=80, suppress=True)
Populating the interactive namespace from numpy and matplotlib
Dataset: line + noise
# Training set:
N = 5 # Size of the data set
X = linspace(0.0, 1.0, N).reshape((N, 1)) # Inputs, shape: N x 1
y = linspace(7.0, -5.0, N) + 2*random.randn(N) # Outputs, shape: N
print("X =")
print(X)
print("y =")
print(y)
# Test set:
N_test = 100
X_test = linspace(0.0, 1.0, N_test).reshape((N_test, 1))
y_test = linspace(7.0, -5.0, N_test) + 2*random.randn(N_test)
X = [[ 0. ] [ 0.25] [ 0.5 ] [ 0.75] [ 1. ]] y = [ 10.528 4.8 2.957 2.482 -1.265]
plot(X[:, 0], y, "o")
#plot(X_test[:, 0], y_test, "o")
xlabel("x")
ylabel("y")
<matplotlib.text.Text at 0x27bf310>
There are two ways to adjust the weights of a linear model (which includes linear models with nonlinear features):
In addition, we can add a constraint to the objective function to penalize large weights:
Solving the equation \(\boldsymbol{X} \cdot \boldsymbol{w} = \boldsymbol{y}\) directly for \(\boldsymbol{w}\) by inversion is not possible, because \(\boldsymbol{X}\) is not necessarily a square matrix, i.e. \(\boldsymbol{w} = \boldsymbol{X}^{-1} \boldsymbol{y}\) is usually not possible. In addition, it is usually not possible to find an exact solution. Instead, we find the least squares solution, which is the solution of
\[\boldsymbol{X}^T\boldsymbol{X} \hat{\boldsymbol{w}} = \boldsymbol{X}^T\boldsymbol{y}\]
for \(\hat{\boldsymbol{w}}\), i.e.
\[\hat{\boldsymbol{w}} = (\boldsymbol{X}^T\boldsymbol{X})^{-1}\boldsymbol{X}^T\boldsymbol{y}\]
Hint: The expression \((\boldsymbol{X}^T\boldsymbol{X})^{-1}\boldsymbol{X}^T\) is equivalent to the Moore-Penrose pseudoinverse \(\boldsymbol{X}^+\) of \(\boldsymbol{X}\). It is a generalization of the inverse for non-square matrices. You can use this to implement normal equations if your library provides the function. You could also use a least squares solver (e.g. numpy.linalg.lstsq or dgelsd/zgelsd from LAPACK).
def normal_equations(X, y):
#w = linalg.inv(X.T.dot(X)).dot(X.T).dot(y)
# ... or we can use the Moore-Penrose pseudoinverse (is usually more likely to be numerically stable)
#w = linalg.pinv(X).dot(y)
# ... or we use the solver
w = linalg.lstsq(X, y)[0]
return w
# Add bias to each row/instance:
# x11 x12 ... x11 x12 ... 1
# x21 x22 ... ---> x21 x22 ... 1
# . .
# . .
# . .
X_bias = hstack((X, ones((N, 1))))
X_test_bias = hstack((X_test, ones((N_test, 1))))
w = normal_equations(X_bias, y)
plot(X_bias[:, 0], y, "o")
plot(X_bias[:, 0], X_bias.dot(w), "-", linewidth=3)
xlabel("x")
ylabel("y")
title("Model: $y = %.2f x + %.2f$" % tuple(w))
<matplotlib.text.Text at 0x29c1a50>
n_pixels = 256 * 256
memory_size_xtx = n_pixels * n_pixels * 64/8
print("%d GiB" % (memory_size_xtx / 2**30))
32 GiB
Inversion is an operation that requires \(O(n^{2.373})\) operations (Source).
Update \(\boldsymbol{w}_t\) incrementally:
\[\boldsymbol{w}_{t+1} = \boldsymbol{w}_t - \alpha \nabla \boldsymbol{w}_t,\]
where
Gradient descent actually is a more general rule that can be applied to any minimization problem if the gradient can be computed (e.g. for artificial neural networks, support vector machines, k-means, ...).
Error function: sum of squared errors (SSE) \[E(\boldsymbol{X}, \boldsymbol{y};\boldsymbol{w}) = \frac{1}{2}||\boldsymbol{X} \cdot \boldsymbol{w} - \boldsymbol{y}||^2_2\]
Gradient \[\nabla \boldsymbol{w} = \nabla_{\boldsymbol{w}} E(\boldsymbol{X}, \boldsymbol{y};\boldsymbol{w}) = \boldsymbol{X}^T \cdot (\boldsymbol{X} \cdot \boldsymbol{w} - \boldsymbol{y})\]
To approximate nonlinear funtions with a linear model, we have to generate nonlinear features. In this example, we generate sinusoidal features. You could also try radial basis functions, polynomials, ...
We expand each feature \(x\) to the nonlinear feature vector \((\cos(\frac{0}{d} \pi x), \cos(\frac{1}{d} \pi x), \ldots, \cos(\frac{d}{d} \pi x))^T\), where \(d\) is the number of basis functions. Note that \(\cos \left(\frac{0}{D} \pi x \right)=1\) is the bias we added manually in the previous example.
def sinusoidalize(X, n_degree):
X_sinusoidal = numpy.ndarray((len(X), n_degree+1))
for d in range(n_degree+1):
X_sinusoidal[:, d] = numpy.cos(X[:, 0] * numpy.pi * d / n_degree)
return X_sinusoidal
# Utility function
def build_sinusoidal(n_degree, w):
from itertools import chain
# That does not look readable but it works :)
return ((("%+.2f \sin(%d /" + str(n_degree) + " \pi x)") * (n_degree+1)) % tuple(chain(*zip(w, range(n_degree+1)))))
n_degree = 4
X_sinusoidal = sinusoidalize(X, n_degree=n_degree)
X_test_sinusoidal = sinusoidalize(X_test, n_degree=n_degree)
w_sin = normal_equations(X_sinusoidal, y)
plot(X_bias[:, 0], y, "o")
#plot(X_bias[:, 0], X_bias.dot(w), "-", linewidth=3) # Uncomment to compare with linear approximation
plot(X_test_bias[:, 0], X_test_sinusoidal.dot(w_sin), "-", linewidth=3, color="r")
xlabel("x")
ylabel("y")
title("Model: $y = " + build_sinusoidal(n_degree, w_sin) + "$")
<matplotlib.text.Text at 0x2df5e10>