Task Space Planning with Obstacle Avoidance

Now we want to add an obstacle to our environment. The obstacle is located at $[0.5, 0.5]$ in Cartesian space. For simplicity, we assume that only the end-effector can collide with the obstacle. The collision probability $P(c_t = 1\vert q_t^{(j)})$ for joint position $\mathbf{q}^{(j)}$ is given by $\exp(-1/2 \vert\vert\Phi(\mathbf{q}^{(j)}) - [0.5, 0.5]^T\vert\vert^2 / 0.15^2)$ . We want our robot to avoid the obstacle for the whole trajectory, therefore we add the observation of not colliding with the obstacle $P(c_t = 0\vert q_t^{(j)})$ for each time step $t$ to our Bayesian network (see Figure 11, $c_t$ notes).

Figure 11: Dynamic Bayesian Network for planning with obstacles.

• Generate the collision probability $P_{qc}$
• Use Gibbs sampling the same ways as before, visualize the marginals $P(\mathbf{q}_t = q^{(i)})$ . How has the estimated solution changed?