Task Space Planning

We will first start with task space planning. Therefore we add an 'mental observation' of reaching the $j$ th discrete position in task space $x_T = x^{(j)}$ at time $T=10$ to our Bayesian network (see Figure 10). We will set our desired target position to be $[0.2, 0.2]$ , the index $j$ denotes the discrete index of this position. In addition, we also observe our current state $\mathbf{q}_0$ which is $[\pi/4, 0]^T$ . Our task is to estimate a trajectory $\mathbf{q}_{1:T}$ using Gibbs-sampling.

• Your main task is to generate the state transition probabilities $P_{qq}$ as well as the kinematic task mapping $P_{qx}$ as described in the text.
• Initialize your trajectory $\mathbf{q}_{1:T}$ with random discrete indices. Use 5000 Gibbs sampling steps to estimate the true distribution. Plot 5 independent samples of $\mathbf{q}_{1:T}$ from the sampling process. We assume that two samples are independent at least after 100 Gibbs Sampling steps.
• Now we want to calculate the marginals $P(\mathbf{q}_t = q^{(i)})$ for all $i$ and $t$ . Therefore count the number of of times in which the variable $\mathbf{q}_t$ equals $q^{(i)}$ during the last 3000 Gibbs sampling steps. Plot the marginals for each time step, use a visualization which you find appropriate. How does the estimated solution look like?
• Repeat the experiment at least 10 times using different initial positions for the Gibbs sampler. Are the marginals always similar (up to a certain noise level...)? If not, why not?