In this video we'll plan motions on a graph derived from a grid on the C-space.
If the C-space has n-dimensions, we can subdivide each dimension into k intervals, creating
a total of k-to-the-n grid cells.
Each cell is represented in the graph by a single node, representing the configuration
at the center of the cell.
For example, for a 2R robot, if we choose k equal to 32, then there are 32-squared or
1024 grid cells.
Let's draw the start and goal configuration on this grid.
If we add obstacles to the scene, the corresponding C-space obstacles can be made conservative
by marking every C-space grid cell they touch as an obstacle.
To turn this grid into a graph, we have to decide whether the centers of the grid cells
are 4-connected, meaning that an edge exists between two free grid cells if they are directly
north, south, east, or west of each other, or whether the centers of the grid cells are
8-connected, meaning that neighboring free grid cells along a diagonal are also connected.
With this choice, the center of each free grid cell is considered to be a node of the
graph, and edges are between the 4- or 8-connected free grid cells.
To find a short path, we can use A-star to search the graph.
We don't have to explicitly construct the entire graph in advance; we can construct
it as we search, using the 4-connected or 8-connected rule.
The optimistic cost-to-go is just the shortest straight-line path through joint space, accounting
for the wraparound at 0 and 2pi for each joint.
If the cells are 4-connected, this is an optimal path.
This optimal path is not unique; other paths with the same path length also exist.
If the start and goal configurations are in free grid cells, but not at the centers, then
the first path segment is from the start configuration to the center of its grid cell, and the last
path segment is from the center of a grid cell to the goal configuration.
If there are constraints on the robot's motion, such as for a car that can't move sideways,
or if the robot is dynamic and the controls are forces, not velocities, then grid-based
methods must be modified, as described in the book.
But, the simple grid-based path planner I just described can be applied to any fully
actuated kinematic system where the controls are velocities.
Because of the discretization of the C-space, the grid-based planner is not complete, but
it is resolution complete, meaning that it will find a path if one exists at the level
of discretization chosen.
The solution path is optimal for the underlying graph using A-star search.
But, a major drawback is that this approach is not practical for high-dimensional spaces.
The amount of memory needed to represent the grid, and the time to search the graph, grows
quickly with the number of degrees of freedom.
This problem can be mitigated, to some extent, by using multi-resolution grids.
The key idea here is not to choose the discretization level k in advance, but to represent the free
C-space coarsely in wide-open regions, and to use a finer resolution where the C-space
is cluttered.
This should keep the representation of the C-space relatively small, while still allowing
representation of narrow passages of free space.
As an example, imagine this dark gray box is a cell of the C-space, and the lighter
box is a C-space obstacle.
If we used a fixed resolution to represent the C-space, then this whole cell would be
marked as in collision.
If we subdivided the cell into 4 cells, then only one of them would be in collision.
We can subdivide again, then subdivide again, and in the end the original C-space cell is
represented by a tree that looks like this.
The 10 leaves of the tree are the final cells in our representation, and 2 of those cells,
colored gray, are in collision.
If we had used a fixed resolution grid, we would have needed 64 cells to achieve the
same level of resolution.
This kind of representation of a 2-dimensional C-space is called a quadtree, since cells
subdivide into 4.
In 3-dimensional space, cells subdivide into 8, and the representation is called an octree.
In the next video, we will see a different way to generate a graph representing the C-space,
based on random sampling.