We know that any robot configuration is described uniquely by a point in its configuration space,
or C-space.
One of the key ideas in motion planning is to represent any obstacle in the robot's environment
as a set of points in C-space where the robot is in collision with the obstacle.
So we should become comfortable with the idea of transforming a real-world obstacle into
a C-space obstacle.
Let's use the 2R robot as an example.
It's C-space is represented by a square in the plane, where one axis corresponds to theta_1
and the other axis corresponds to theta_2.
As we saw in Chapter 2, the topology of the C-space is actually a torus, so when we represent
it as a square, we have to remember that the top and bottom edges are connected to each
other, and the left and right edges are connected to each other.
We can represent a specific configuration of the robot as a point in the C-space.
Here, theta_1 and theta_2 are both close to zero, so the point is in the bottom-left corner
of the C-space.
If there is an obstacle in the environment, the obstacle can be represented in C-space
by the set of robot configurations where the robot would collide with the obstacle.
Even though this C-space obstacle looks like three separate regions, if we remember the
topology of the C-space, we see that it is just a single connected region.
We can add two more obstacles and get the final picture of the robot's C-space.
An example configuration in collision with obstacle A is shown here.
Theta_1 is 45 degrees and theta_2 is 315 degrees.
Next we see an example configuration in collision with obstacle B, and finally a configuration
in collision with obstacle C. If the robot also had joint limits for joint 2, preventing
link 2 from rotating over link 1, we would get another obstacle, this one due to the
joint limits.
In the rest of this video, we will assume no joint limits.
Now that we've constructed our C-space and its C-obstacles, we can perform all motion
planning in the C-space, so let's focus on that for a moment.
Notice that if the robot were in a configuration in this red region, there would be no way
for it to escape being stuck between obstacles B and C. It could only move between configurations
in this region.
We call this region a connected component of the free space, and we label this connected
component 1.
In this example, there are three connected components of free space; this region labeled
2 is connected because the top and bottom edges of the square are connected, and this
region labeled 3 is connected because the left and right edges and top and bottom edges
are connected.
For a path to exist between two configurations, they must lie in the same connected component
of free space.
Now we would like to find a path between this start configuration and this goal configuration.
Both configurations are in the same connected component, so we know a solution exists.
Remembering that the left and right edges of the square are connected, let's animate
a solution path in both the real space and the C-space.
Planning collision-free paths for other robots is conceptually the same as for the 2R robot:
we transform obstacles to C-space obstacles, then we plan a path for a point in the free
portion of C-space.
It's impractical to explicitly construct C-space obstacles due to their geometric complexity,
however, especially for higher-dimensional C-spaces.
For this reason, most motion planners simply assume the existence of a collision-detection
routine that can check whether a given configuration, or path segment, is in free space.
One way to check if a particular configuration is in collision is by checking for intersection
of any of the polygons that represent the surfaces of the robot and the obstacle.
An even simpler way to check for collision between two objects is to approximate each
by a set of spheres.
Here is a lamp approximately represented as the union of spheres.
Checking whether two objects collide is then as simple as checking the distance between
the centers of the spheres of the two objects, which can be done very quickly.
The actual object should be strictly inside its sphere approximation, to make sure that
we don't declare a particular configuration to be free when it is actually in collision.
If we use more spheres, we can represent the objects more precisely, as shown here.
This makes collision detection less conservative, but increases the number of distance checks.