Configuration space

A wall in the way

In Pick & Place the table was empty, so "lift, then swing across" always worked. Now a see-through wall stands between the balls and the cup, with a pillar at its far end. A copied Pick & Place program slams into it. Over the next steps you'll build the sampling-based planning stack that MoveIt and OMPL use to get around it.

Configuration space

Four joint angles $q = (q_0, q_1, q_2, q_3)$ pin down the pose of the whole arm. So every pose is a single point in a 4-D space, the configuration space (C-space). Planning a motion means drawing a curve through that space.

An obstacle is a simple box in the world. The set of configurations where the arm touches it, however, is a warped region with no neat formula:

$$\mathcal{C}_{\text{obs}} = \{\, q : \text{arm}(q) \cap \text{obstacles} \neq \varnothing \,\}, \qquad \mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}$$

Planners never build $\mathcal{C}_{\text{obs}}$ explicitly. They probe it one configuration at a time.

New tool: arm.in_collision(q)

It answers "what if the arm were at $q$?" without moving anything. It returns True if the arm would come within 5 mm of the wall, the pillar, the table, the cup or itself, or if $q$ breaks a joint limit. It always models the gripper fully open, its widest. world.obstacles lists the wall and pillar.

A 2-D slice

Four dimensions are hard to picture, so freeze two joints. Turn the base to line up with the wall ($q_0 = 1.08$) and fix the wrist ($q_3 = 1.2$). What's left, shoulder × elbow, is a flat slice you can draw as an image with one pixel per configuration.

Your task

Implement cspace_slice(base, wrist, n). It returns an $n \times n$ bool array where grid[i, j] is True when [base, shoulders[i], elbows[j], wrist] collides, with shoulders = np.linspace(*arm.limits[1], n) and elbows = np.linspace(*arm.limits[2], n).

Open the Images tab to see your slice. The grader compares two slices with the real C-space.