Manipulation planning: pick-and-place, regrasping and task and motion planning

A grasp is one moment in a longer sequence. To be useful, the robot must plan how to reach the object, take it, carry it without collisions, put it down in the right place and pose, and recover when something goes wrong. Manipulation planning connects the grasp choices of lesson 5 with the arm motion planning of ROB-350 and with the logic that sequences many actions into a task. This lesson covers the standard pick-and-place pipeline, placement and stability, regrasping when no single grasp works, and the family of methods called task and motion planning that reason about actions and geometry together.

The central difficulty is that choices interact. The grasp determines which placements are reachable. The placement determines which grasps are acceptable. An early choice, such as stacking a box on the left, can block a later action, such as reaching a part behind it. Simple systems avoid most of this by fixing the sequence and the grasp in advance; flexible systems must search.

The pick-and-place pipeline

Nearly every pick-and-place system follows the same skeleton. Perceive the scene. Choose a target object and a grasp. Plan a free-space motion to the pre-grasp pose. Approach in a straight line, close the gripper and verify the grasp. Retreat, carrying the object, along a straight line. Plan a free-space motion to the pre-place pose with the object attached to the hand in the collision model. Approach the place pose, release, verify the placement and retreat. Each step can fail, so each has a check and a recovery branch.

Writing the pipeline as an explicit state machine or behavior tree (ROB-110 and ROB-360) keeps the logic testable. Every transition has a condition: the grasp is verified when vacuum exceeds its threshold, the place is verified when the object is seen at the target within tolerance. Every failure has a defined response: retry with the next-best grasp, re-perceive, nudge the object, or ask for help after a set number of attempts. Production cells usually spend more engineering effort on these failure branches than on the nominal path.

Carrying the object

Once the object is in the hand, it becomes part of the robot for planning purposes. The motion planner must check collisions for the arm plus the gripper plus the held object, whose shape comes from the model or from the point cloud observed before grasping. A long object changes the arm's reach and collision envelope dramatically. Planners also respect limits that protect the grasp: maximum accelerations so the inertial load stays within the sizing of lesson 7, orientation limits so a tray of liquid stays level, and speed limits near people.

Carrying adds dynamic effects. A heavy object shifts the arm's payload, changing the loads on joints and the accuracy of position control. Robots that know the payload mass and center of mass, either from configuration or from a quick identification move with a wrist force-torque sensor, compensate better and avoid overloads.

Placing and placement stability

Placing is harder than it looks. The object must end up in a pose that is stable, meaning it will not tip or slide when released; that is within tolerance of the target; and that the next process can use. Stability for an object resting on a horizontal surface has a simple test: the vertical projection of its center of mass must lie inside the support polygon, the convex hull of the points where it touches the surface. Margin matters, because small disturbances and uncertainty in the center of mass can tip an object placed near the edge of its support.

# Placement stability: is the projected COM inside the support polygon, and by how much?
import numpy as np
def margin(poly, c):
    """Signed distance from point c to the edges of a convex polygon (counter-clockwise). Positive = inside."""
    d = []
    for i in range(len(poly)):
        p, q = np.array(poly[i]), np.array(poly[(i + 1) % len(poly)])
        e = q - p; n = np.array([e[1], -e[0]]) / np.linalg.norm(e)   # outward normal for CCW order
        d.append(-(np.array(c) - p) @ n)
    return min(d)
support = [(0, 0), (0.12, 0), (0.12, 0.08), (0, 0.08)]       # 120 x 80 mm footprint, m
for name, com in (("middle", (0.06, 0.04)), ("offset", (0.10, 0.04)), ("overhang", (0.13, 0.04))):
    m = margin(support, com)
    print(f"{name:9s} COM {com}: margin {1000*m:+.0f} mm -> {'stable' if m > 0 else 'tips'}")
# tilt needed to tip a 120 mm box whose COM is 50 mm above the base, from the middle placement
h, half = 0.050, 0.06
print(f"tipping angle about the near edge: {np.degrees(np.arctan(half / h)):.1f} deg")

middle    COM (0.06, 0.04): margin +40 mm -> stable
offset    COM (0.1, 0.04): margin +20 mm -> stable
overhang  COM (0.13, 0.04): margin -10 mm -> tips
tipping angle about the near edge: 50.2 deg

The margin tells the planner how much uncertainty the placement tolerates. A part placed with its center of mass 20 mm inside the edge survives a center-of-mass estimate that is off by 10 mm; a part placed with 2 mm margin does not. Placement planners therefore prefer poses with large margins, and for stacking they check the stability of the whole stack, not just the top item.

Regrasping

Sometimes no single grasp works for both pick and place. A part lying on its side may only be graspable from above in a way that leaves it upside down for the fixture; a mug grasped by the body cannot be hung by its handle. The robot must then regrasp: place the object somewhere intermediate in a new stable pose, or hand it from one gripper or hand to another, and grasp it again differently.

Regrasp planning is naturally a graph search. Nodes are combinations of a stable object placement and a grasp compatible with it; edges connect nodes that share a grasp (pick up and put down in a new pose) or share a placement (set down and pick up with a different grasp). The planner searches for a path from the initial placement to a node whose grasp allows the final placement, preferring paths with fewer steps because each regrasp costs time and risks a failure.

Many industrial cells avoid regrasping with mechanical aids: a passive flipping fixture, a tilting tray, a part feeder that presents parts in a known orientation, or a second robot. Choosing between planning sophistication and simple hardware is a recurring engineering trade-off; the cheapest reliable option usually wins.

Task and motion planning

Task and motion planning, often shortened to TAMP, combines symbolic reasoning about what to do with geometric reasoning about how to do it. A task planner works with discrete facts and actions: the cup is on the table, the robot can pick a free object, placing requires a clear region. A motion planner works with continuous configurations and collisions. Neither alone is enough. A symbolic plan such as "pick the cup, place it on the shelf" can be geometrically impossible if the shelf is blocked by a box, and a motion planner cannot decide that the box must be moved first.

TAMP methods interleave the two. One common approach generates a symbolic plan, tries to find grasps, placements and motions that realize it, and if the geometry fails, records why (for example, the box blocks the shelf) and asks the task planner for a plan that addresses the obstacle. Other approaches sample continuous values such as grasps and placements as part of the search itself. These methods have become practical for household and laboratory tasks with tens of objects, and research systems increasingly combine them with learned models that suggest promising actions and with language models that translate instructions into goals. In factories, the same ideas appear in a simpler form as offline sequence planning.

Reacting during execution

Plans meet reality: objects shift when neighbors are removed, grasps slip, people walk into the cell. Robust systems re-perceive at decision points, monitor force and vacuum during motion, and replan locally when the world differs from the model. Model predictive control for manipulation and reactive behavior trees are two ways to make execution responsive; at the simplest level, re-imaging the bin after every pick, as lesson 6 described, is the most effective reactive behavior in industry.