next up previous
Next: Task Space Planning Up: MLB_Exercises_2012 Previous: Learning overhypotheses [3* P]

Planning with Approximate Inference [3 P]

In this example we consider a 2-link planar arm. In this task you have to plan an optimal path from an initial joint-position to a given endeffector-position 9 by Gibbs sampling.

The length of the links $ l_1$ and $ l_2$ is given by $ 0.5$ meters. We will restrict the first joint position of our arm model to be in the range of $ [0; \pi / 2]$ and the second to be in the range of $ [-\pi / 2; \pi / 2]$ .

We will first discuss how to construct a Bayesian Network where we can sample from. We want to reach our target within $ T=10$ time steps, i.e. we get a dynamic Bayesian Network with one node per time step (11 nodes), see Figure 9.

Figure 9: Dynamic Bayesian Network for task-space planning. The task-space is defined as cartesian coordinates of the endeffector position.
Image planning_no_control_onlyFinalZ

Each node $ t$ represents the joint positions $ \mathbf{q}_t$ at time $ t$ . For simplicity, we will use a discrete representation of the joint positions. Therefore we use a uniform $ 11 \times 11$ grid to discretize the joint space. We will use a Gaussian motion prior in order to define the transition probabilities of $ P(q_{t}^{(j)}\vert q_{t-1}^{(i)})$ from the $ i$ th discrete joint position at time $ t-1$ to the $ j$ th joint position at time $ t$ . Let $ \mathbf{q}^(j)$ be the $ j$ joint position vector (in radians), then $ P(\mathbf{q}_{t}^{(j)}\vert\mathbf{q}_{t-1}^{(i)}) \propto \mathcal{N}(\mathbf{q}_{t}^{(j)}\vert \mathbf{q}_{t-1}^{(i)}, \mathbf{W})$ , where $ W$ equals $ \textrm{diag}([0.0125, 0.05])$ . The motion prior encodes our laziness, meaning that, if not necessary, we do not want to move away from $ \mathbf{q}_{t-1}$ .

In order to plan a trajectory to a certain end-effector position we still need to define our kinematic task space mapping. We will also use a discrete representation for task space (Cartesian coordinates of the hand). Here, we use again a $ 11 \times 11$ uniform grid over the range $ [0;1]$ for $ x$ and $ y$ . The probability of reaching the $ j$ th discrete task space position when being in the $ i$ th discrete joint space position is given by $ P(x_t^{(j)}\vert q_t^{(i)}) \propto \mathcal{N}(x_t^{(j)}\vert \Phi(q_t^{(i)}), \mathbf{C})$ , where $ \Phi$ is the non-linear mapping from the joint positions to the endeffector coordinates. The covariance matrix $ \mathbf{C}$ is set to $ \textrm{diag}([0.004, 0.004])$ .

next up previous
Next: Task Space Planning Up: MLB_Exercises_2012 Previous: Learning overhypotheses [3* P]
Haeusler Stefan 2013-01-16