Motion Planning, step 5
Pick & place with a planner
Put the planner to work: plan every transit of a pick-and-place task around the wall, on a new layout every run.
Planning inside a task
A real pick-and-place program mixes two kinds of motion:
| Motion | How | Why |
|---|---|---|
| transit: home → above a ball, above a ball → above the cup | planned: rrt + shortcut + follow | long moves through clutter, where the straight line hits things |
| approach, grasp, lift, place | straight lines with arm.move_to | short, predictable moves near the object |
The collision checker only models the arm, not the ball in its gripper. The ball hangs about 7 mm below the fingertips, so it can brush the top of the wall. Industrial planners attach held objects to the robot model for exactly this reason.
Plan from where you are
Every plan starts from the arm's current configuration. After a grasp, the arm sits somewhere new (above that ball), so plan the next transit from arm.targets, the configuration you last commanded. Never replay an old path.
The same goes for failures. If gripper.close() misses, open it, lift back up and plan again from there.
Sampling where it matters
rrt now takes a sampling box lo, hi. Nothing in this task needs the arm pointing backwards or folded under the table, so LO and HI cover only the region between the balls and the cup. The tree stops wasting samples elsewhere, so the slow, wandering searches mostly disappear.
Your task
Put the red and green balls in the cup. The layout changes on every run, and the arm must never touch the wall, the pillar, the table or the cup.
The starter's go() just calls move_joints, so the carry drives straight into the wall. Make go() plan, then press Run a few times.