Solving a query

Once the roadmap is obtained, it is straightforward to solve a motion planning query, $({q_{I}},{q_{G}})$. Let $C_0$ and $C_k$ denote the cells that contain ${q_{I}}$ and ${q_{G}}$, respectively. In the graph ${\cal G}$, search for a path that connects the sample point of $C_0$ to the sample point of $C_k$. If no such path exists, then the planning algorithm correctly declares that no solution exists. If one does exist, then let $C_1$, $C_2$, $\ldots$, $C_{k-1}$ denote the sequence of 1-cells and 2-cells visited along the computed path in ${\cal G}$ from $C_0$ to $C_k$.

A solution path can be formed by simply ``connecting the dots.'' Let $q_0$, $q_1$, $q_2$, $\ldots$, $q_{k-1}$, $q_k$, denote the sample points along the path in ${\cal G}$. There is one sample point for every cell that is crossed. The solution path, $\tau: [0,1] \rightarrow {\cal C}_{free}$, is formed by setting $\tau(0) = {q_{I}}$, $\tau(1) = {q_{G}}$, and visiting each of the points in the sequence from $q_0$ to $q_k$ by traveling along the shortest path. For the example, this leads to the solution shown in Figure 6.5. In selecting the sample points, it was important to ensure that each path segment from the sample point of one cell to the sample point of its neighboring cell is collision-free.^6.4