31Planning for arms: moving without hitting yourself

Reach for a cup on the far side of your laptop. Your hand does not go straight at it; it lifts, passes over the screen, and comes down, and your elbow clears the screen too, without a thought. Two modules ago a planner found a rover's way through a house; the last module dodged a moving box. Both were about a robot that is a dot on a floor. An arm is not a dot on anything, and this module is the trick that lets the same planner work for it: a map on which the arm is a dot.

The wall and the star

The arm is a real desk-sized one, fixed to the table, with a motor at each of its joints: a base that turns, a shoulder that lifts, an elbow, two joints at the wrist, and its hand, a gripper with two jaws. A wall stands twenty-six centimeters in front of its base: a slab twenty centimeters wide, two thick, sixteen tall. The star is on the far side of it, twelve centimeters beyond and six above the table. To keep the picture flat, the base does not turn and the wrist is held bent at one angle, so the only joints that move are the shoulder lift and the elbow, as in the inverse kinematics module's flat arm, and the hand stays in one vertical plane, the one the wall stands across.

From that module the robot already knows how to find the angles that put the fingertips on the star, and that a place can have two answers, elbow up and elbow down. This star has one; its joints cannot reach the other. It is shoulder lift 30 degrees, elbow −26. The arm starts leaning back, lift −98, elbow 61, its hand level with the top of the wall and a few centimeters short of it. So the obvious move is to turn each joint steadily from where it is to where it must be, both at once, and stop. Do it, and the arm swings into the wall.

Aiming the fingertips along a straight line through the air is no better, since the wall is on the line. The hand is the wrong thing to plan for. The whole arm has to get past the wall, elbow and forearm included, and where the fingertips go says little about where the rest sweeps.

A map with joint angles for axes

Here is the trick. Stop thinking about where the arm is in the room and think about its two joint angles. A pose of the arm is one pair of numbers, lift and elbow, and a pair of numbers can be drawn as a point: lift across the page, elbow up it. Every point on that page is one pose, and every pose is one point. This page is the arm's configuration space, a map of poses rather than of places, one axis per joint. The arm, a jumble of links in the room, is a single dot here, and a move of the arm, whatever it sweeps through the room, is a line across the map.

Now put the wall on it. Take a pose, any point on the map, and ask: with the joints at these angles, would any part of the arm be inside the wall? That question is a collision check. The robot can answer it without touching anything, because it knows the shape of each link and of the wall: it places every link where the angles say, with the forward working from the kinematics modules, and tests each shape against the wall's for overlap. If the answer is yes, shade that point. Ask it for every pose, and the wall appears on the map as a shadow: every pose that has some piece of the arm inside it.

The shadow does not look like a wall. On this map it is a long diagonal band, because the poses that hit the wall are the ones with the arm reaching forward and low, and many pairs of angles do that: a high shoulder with a folded elbow, a level shoulder with a straight one. The table casts a shadow too, everywhere forward and low of the wall's, and there is a third in the top corner, where lift and elbow are both large and the hand folds back into the arm's own upper arm. The arm hitting itself is an obstacle like any other, checked the same way.

The box asks the question at every two degrees of each joint, about ten thousand poses, and the simulation's own physics answers each one, in under a second. The arm's dot sits in the clear, so does the star's ring, and now the obvious move can be seen for what it was. Both joints turning steadily means both numbers changing at steady rates, and a point whose two coordinates change at steady rates moves in a straight line, so the move is a straight line on the map from one dot to the other, cutting through the band. The map says what it hits.

The planner from two modules ago

Look at what is left: a map with shaded parts and clear parts, a start, a goal, and the question of how to get from one to the other without crossing shade. That is the rover's problem in the house, and the same search answers it. Cut the map into cells; each is one pose. A cell's neighbors are the four poses one step away: the lift turned two degrees either way, or the elbow. Spread out from the start, always taking next the cell whose distance so far plus its straight-line guess to the goal is smallest, never stepping into a shaded cell. When the goal comes off the frontier, follow the chain back. This is A*, unchanged; a step is two degrees of one joint instead of ten centimeters of floor.

Path planning done on a configuration space, for a robot with joints, is called motion planning. The route found is not a path for the hand but a sequence of poses, each one clear, each a small turn from the last, and it keeps the whole arm out of the wall because the shadow was drawn from the whole arm.

Two things about the route are worth knowing. It is the shortest on the map, which means the least total turning of the two joints, and that is not the same as the shortest trip for the hand. The route the box finds goes down the left of the map and then straight across under the band: the elbow opens first, all the way, and only then does the shoulder swing forward. In the room that sends the hand up in a high arc over the base before it comes down beyond the wall, a long way round for a star so near. A real planner would tidy the corners and shorten the arc; this one drives it as found. And the box grows every shadow by one cell before it searches, because the motors lag their instructions by a degree or so, and a route that brushed the edge of the shadow would brush the wall.

The other thing is what six joints do. Then the map has six axes, and a grid of it at two degrees has more cells than there are grains of sand on Earth. This is what the darts of the path planning module were for: a tree that looks only where its darts land, with a collision check at each, finds a route through a space no one could draw.

The box

The arm and the wall are seen from the side, the base on the left and the star on the right, so the arm swings the way its dot moves on the map below. Under the toolbar, a line says what Go will do, then the Go button beside the switch labeled Plan around the wall. Ticked, Go searches the map and drives the route; unticked, Go turns the joints straight to the star's angles, and the arm stops if it touches anything, which glows red. The star can be dragged in the arm's plane or nudged; it is gold where the hand can be put on it clear of everything and gray otherwise, the line saying why. "The arm's answer" is the two angles now.

Below that is the map: shoulder lift across, elbow up, the wall's shadow in brown, the table's in gray, the arm's own in violet. The blue dot is the arm's pose now, the green ring the star's (where a star has two answers, the one the robot's search reaches from where the arm is), and while a search runs, cells looked at turn orange, as on the house. The route found is a blue line, and the fingertips leave a blue trace as the arm drives it; its ends are the exact poses, not cell corners, so its travel can be a degree off a whole number of steps. Reset puts both back.

Loading the simulation. The robot runs in your browser, so the first one takes a few seconds to arrive.

ResetPauseSlowLabels

This is easier on a larger screen, but it works here too.

What you saw

The search never looked at the wall. It looked at a map on which the wall had already been turned into a shadow of poses, and did what it did in the house: spread toward the goal, refuse the shaded cells, hand back a chain. What came out was a move you would not have thought of, and it worked because every pose on the chain had been checked against the whole arm.

The straight move failed the way an arm fails in real life: not the fingertips but the gripper's body, sweeping into the wall. Then you put the star where the hand could reach it only through the wall, and the map showed the ring inside the shadow before anything moved: the planner's other answer, no, before a motor turned. Lifted over the wall, the same star cost a shorter route: what a route costs is the shadows between two dots, not the distance in the room.

What this is for

Every arm that works among things does this: a warehouse arm reaching into a bin between two others, a welding arm threading its torch under a car body, a surgical arm working through a small opening. Each has a model of itself and of what is around it, and before it moves it plans in its configuration space, checking thousands of poses for collisions, with itself as much as with the world. A planner that finds no route says so, instead of pushing.

What the simulation leaves out

Only two joints move here, so the map fits on a screen; a real arm plans over all six at once, with darts rather than a grid, and no one draws the map. The box shades the whole map before you press Go, where a real planner checks only the poses its search reaches, and drives the route with its corners sharp, where a real planner smooths it. The arm stops the moment it touches the wall because the simulation can see the contact; a real arm knows only if it has a force sensor or watches its motors strain. And the wall never moves: the last module's rule, for what does move, has no place here, and a robot that needs both runs both.