Probabilistic Movement Primitives Part 3: Supervised Learning

In this post, we describe how a Probabilistic Movement Primitive can be learnt from demonstrations using supervised learning.

Learning from Demonstrations

To simplify the learning of the parameters $\theta$, a Gaussian is assumed for $p(w;\theta )=\mathcal{N}(w|{\mu }_w,{\mathrm{\Sigma }}_w)$ over $w$. The distribution $p(y_t|\theta )$ for time step $t$ is written as,

$$\begin{array}{}\text{(1)}& \begin{array}{rl}p(y_t|\theta )& =\int p(y_t|w)p(w;\theta )dw,\\ & =\int \mathcal{N}(y_t|{\mathrm{\Phi }}_{t}w,{\mathrm{\Sigma }}_y)\mathcal{N}(w|{\mu }_w,{\mathrm{\Sigma }}_w),\\ & =\mathcal{N}(y_t|{\mathrm{\Phi }}_t{\mu }_w,{\mathrm{\Psi }}_t{\mathrm{\Sigma }}_w{\mathrm{\Phi }}_t^T+{\mathrm{\Sigma }}_y).\end{array}\end{array}$$

It can be observed from the above equation, that the learnt ProMP distribution is Gaussian with, $$\begin{array}{}\text{(2)}& {\mu }_t={\mathrm{\Phi }}_t{\mu }_w,{\mathrm{\Sigma }}_t={\mathrm{\Phi }}_t{\mathrm{\Sigma }}_w{\mathrm{\Phi }}_t^T+{\mathrm{\Sigma }}_y\end{array}$$

Learning Stroke-based Movements

For stroke-based movements, the parameter $\theta =\{{\mu }_w,{\mathrm{\Sigma }}_w\}$ which specifies the mean and variance of $w$, can be learnt by maximum likelihood estimation using multiple demonstrations. The weights of each trajectory are estimated individually with linear ridge regression,

$$\begin{array}{}\text{(3)}& w_i=({\mathrm{\Phi }}^T\mathrm{\Phi }+\lambda I{)}^{-1}{\mathrm{\Phi }}^T{Y}_i\end{array}$$

where $Y_d$ represents the positions of all joints from the demonstration $d$ for all time steps, $\mathrm{\Phi }$ represents the basis function matrix and $I$ is the identity matrix. The demonstrations are aligned by varying the phase variable. For each demonstration, it assumed that $z_{begin}$ = 0 and $z_{end}$ = 1. The ridge factor $\lambda$ is generally set to a very small value ($\lambda ={10}^{-12}$), as larger value debases the estimation of the trajectory distribution.

The mean ${\mu }_w$ and covariance ${\mathrm{\Sigma }}_w$ are computed from $w_i$ as,

$$\begin{array}{}\text{(4)}& {\mu }_w=\frac{1}{N}\sum _{i=1}^N{w}_i,{\mathrm{\Sigma }}_w=\frac{1}{N-1}\sum _{i=1}^N(i_d-{\mu }_w)(i_d-{\mu }_w{)}^T\end{array}$$

where $N$ is the number of demonstrations. Since one trajectory is generated from one demonstration, $N$ also denoted number of trajectories.

Let's code

W = []  # list that stores all the weights
mean_W  = None
sigma_W = None

mid_term = np.dot(Phi, Phi.T) + np.dot(Phi_dot, Phi_dot.T)

for demo_traj in pos_list:
    interpolate = interp1d(z, demo_traj, kind='cubic')
    stretched_demo = interpolate(z)[None,:] # strech the trajectory to fit 0 to 1

    w_d = np.dot(np.linalg.inv(mid_term + 1e-12*np.eye(n_bfs)),
    np.dot(Phi, stretched_demo.T)).T  # weights for each trajectory
    W.append(w_d.copy()) # append the weights to the list

W =  np.asarray(W).squeeze()

mean_W = np.mean(W, axis=0)
sigma_W = np.cov(W.T)

# Computing the mean and sigma of the sampled trajectory
mean_of_sample_traj = np.dot(Phi.T, mean_W)
sigma_of_sample_traj = np.dot(np.dot(Phi.T, sigma_W), Phi) + 1e-10  # Sigma_y

The learnt stroke-based ProMP distribution is shown in blue over the demonstration distribution in red.

References: 

1. Paraschos, Alexandros, et al. "Using probabilistic movement primitives in robotics." Autonomous Robots 42.3 (2018): 529-551.