YA's Blog

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),...