Probabilistic Movement Primitives Part 4: Conditioning

The modulation of via-points, final positions, and velocities is carried out using conditioning so that MP can adapt to new situations. In order to condition the MP to reach a certain state $y^{\ast }$ at any time point $t$, a desired observation $x_t^{\ast }=[y_t^{\ast },{\mathrm{\Sigma }}_y^{\ast }]$ is added to the model. Applying Bayes theorem,

$$\begin{array}{}\text{(1)}& p(w|x_t^{\ast })\propto \mathcal{N}(y_t^{\ast }|{\mathrm{\Psi }}_{t}w,{\mathrm{\Sigma }}_y^{\ast })p(w)\end{array}$$

where state vector $y_t^{\ast }$ defines the desired position and velocity at a time $t$ and ${\mathrm{\Sigma }}_y^{\ast }$ defines the accuracy of the desired observation. The conditional distribution $p(w|x_t^{\ast })$ is Gaussian for a Gaussian trajectory distribution, whose mean and variance are given by,

$$\begin{array}{}\text{(2)}& {\mu }_w^{[new]}={\mu }_w+L(y_t^{\ast }-{\mathrm{\Psi }}_t^T{\mu }_w),{\mathrm{\Sigma }}_w^{[new]}={\mathrm{\Sigma }}_w-L{\mathrm{\Psi }}_t^T{\mathrm{\Sigma }}_w,\end{array}$$

where $L={\mathrm{\Sigma }}_w{\mathrm{\Psi }}_t{({\mathrm{\Sigma }}_y^{\ast }+{\mathrm{\Psi }}_t^T{\mathrm{\Sigma }}_w{\mathrm{\Psi }}_t)}^{-1}$.

Let's code

via_points = [(0.2, .02),(0.6, .06)] # (time t, state y*)
new_mean_W   = mean_W
new_sigma_W  = sigma_W

traj_point_sigma=1e-10  # observation variance for the via points

for viapoint in via_points:
    plt.scatter(viapoint[0], viapoint[1], marker='o', s=100)
    
    # basis functions at observed time points
    PhiT, _= generate_basis_function(phase_z=get_phase_from_time(viapoint[0])
    , phase_zd=phase_speed)
    
    aux = traj_point_sigma + np.dot(np.dot(PhiT.T, new_sigma_W), PhiT)
    
    new_mean_W  = new_mean_W  + np.dot(np.dot(new_sigma_W, PhiT) * 1 / aux,
    (viapoint[1] - np.dot(PhiT.T, new_mean_W)))  
    new_sigma_W = new_sigma_W - np.dot(np.dot(new_sigma_W, PhiT) * 1 / aux,
    np.dot(PhiT.T, new_sigma_W))
    
# get mean and variance for the new trajectories
mean_of_sample_traj = np.dot(Phi.T, new_mean_W)
sigma_of_sample_traj = np.dot(np.dot(Phi.T, new_sigma_W), Phi)

The sampled distribution is shown in blue. The distribution in green is sampled after adding two via-points. The red one is generated after modulating the final position.

References: 

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