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.