Stage 13 · Lesson 16/16

Robot Perception, Planning & Manipulation · Lesson 16

Capstone — Vision-Guided Collision-Free Pick & Place

Est. 2–3 hours

Why

Why this capstone matters

This capstone joins the whole stage into one working system. The camera sees the tray (Lessons 1–3). Calibration turns pixels into centimetres on the desk (Lesson 3). Detection builds the world model of objects and obstacles (Lessons 2 and 4). A task order is chosen and re-planned (Lesson 5). For each object, a grasp check (Lesson 12), inverse kinematics and a collision-checked joint path (Lessons 6–10) are planned. The UNO executes with limits and grasp verification (Lessons 11, 13–15), and the pipeline reports honestly what it did and did not do.

You will first run the complete planner in Python on a small synthetic camera image, so you can see every stage and every failure reason. Then you will build the bench version: the same planner on your PC, sending simple commands to an UNO that drives a 2-link arm with a lift and a gripper.

Concept

The full stack: see, model, plan, act, verify

1. The capstone pipeline

Every stage either hands a result to the next stage or a clear reason to the report. Nothing is silently skipped.
#StageWhat it producesFailure it must report
1Calibratepixel → cm mapping from two known markersmarkers not found / mapping check fails
2Detectblobs: colour, centre, sizenothing detected
3World modelobjects to move + obstacles to avoidobject too close to an obstacle
4Grasp checkfits the gripper?too wide → needs a human
5IKjoint angles (two solutions: elbow one way or the other)unreachable or no collision-free solution
6Pathcollision-checked joint path, direct or via HOMEno clear path
7Execute + verifyJ / D / G / U / O commands, OK or ERR from the UNOclosed on air, slip, joint limit, stop
8Re-plansecond pass for skipped objectsstill impossible → report and stop

2. From pixels to centimetres

With the camera looking straight down at a flat desk, a simple linear mapping is enough for a first version: measure two markers on the desk with a ruler, note their pixel positions, and compute centimetres per pixel and the offset. The script prints a check — marker 2 converted back must land exactly where you measured it. If your camera is tilted, the mapping is no longer linear and you need a full camera calibration with a camera model and lens distortion (see the Computer Vision stage).

An old cream-coloured electric robot arm with a shoulder, elbow and small gripper, displayed on a wooden chest next to a printed label
Victor Scheinman’s MIT Arm (about 1972). According to its display label, it was developed for the MIT Artificial Intelligence Laboratory and had an identifiable shoulder, elbow and wrist — the same joints your capstone arm plans for.Photo: ArnoldReinhold · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

3. Two IK solutions, one collision check

A planar 2-link arm can usually reach a point in two ways: with the elbow bent one way or the other. The planner computes both, throws away any that break joint limits or put a link into an obstacle, and then checks the joint path by sampling 60 points between the start and goal angles. If the direct path is blocked, it tries going through a folded HOME pose first. Other objects count as obstacles until they have been picked — which is why the order of picking matters.

4. Honest reporting

The capstone is judged on two things: what it placed correctly, and how clearly it explains what it did not place. “B at (19.0, −5.5): no collision-free IK solution” tells a human exactly where to look. “Done” when the tray is still full is a failure, even if no part broke.

Visual

Visualise it: the capstone pipeline

Detect → pose → world → grasp → IK → path → execute → verify, with a success or failure reason at every stage. (Educational simulation — not an industrial planner.)

Capstone · Vision-guided collision-free pick & place · Educational

Capstone pipeline
DetectPoseWorldGraspIKPathTrajExecute

Ready — run Detect → … → Execute. Educational pipeline only. · grasp A

Educational browser pipeline — not industrial RRT / IK / grasp / collision. Reuses Stage 8 ik2R + Stage 9 syncedJoints. Bridges Stages 8–12.

Code lab

Code lab: the whole planner, end to end

capstone_pick_place.py uses a tiny synthetic camera image: each character is one pixel (R and B are blocks, X is the tall bottle, M are the two calibration markers). The arm is a planar 2-link arm with its base at (0, 0), links of 15 cm and 12 cm, joint limits −90° to 90° (hobby servos), a gripper that opens to 4 cm, and 1.5 cm of padding around obstacles. Red goes to the bin at (6, 19), blue to (6, −19). All numbers are examples — replace them with your own measurements.

capstone_pick_place.py · Python
"""Stage 13 capstone - vision-guided, collision-free pick & place (plain Python, no libraries).

Pipeline:  image -> calibrate pixels to cm -> detect blobs -> world model
           -> grasp check -> IK (2 solutions) -> collision-checked joint path -> commands
A tiny synthetic top-down "camera image" stands in for a real camera, so the
whole pipeline runs anywhere. All sizes are EXAMPLE numbers.
Arm: planar 2-link, base at world (0, 0), L1 = 15 cm, L2 = 12 cm, top-down gripper.
"""
import math

IMAGE = [  # 1 character = 1 pixel. R/B = red/blue blocks, X = obstacle, M = calibration marker
    "M..............................M",
    "..RRR...........................",
    "..RRR...........................",
    "..RRR......RRR..................",
    "...........RRR......BBB.........",
    "...........RRR......BBB.........",
    "....................BBB.........",
    "................................",
    "....................XXXX........",
    "....................XXXX........",
    "....................XXXX........",
    "....................XXXX........",
    "................................",
    "..........................BBBBBB",
    "..........................BBBBBB",
    "..........................BBBBBB",
    "..........................BBBBBB",
    "..........................BBBBBB",
]
# The two markers are at known world points (measured with a ruler on the desk):
MARKERS_PX = [(0, 0), (31, 0)]           # (u, v) pixels
MARKERS_CM = [(24.0, 15.5), (24.0, -15.5)]  # (x, y) cm  - camera looks straight down
CM_PER_PX_V = 1.0                        # rows: measured once with a ruler

L1, L2 = 15.0, 12.0
GRIPPER_MAX_CM = 4.0
PAD = 1.5                                # padding around obstacles (cm)
BINS = {'R': (6.0, 19.0), 'B': (6.0, -19.0)}  # where each colour goes
HOME = (0.0, 90.0)                       # shoulder, elbow (deg) - folded, safe


# ---------- 1. calibration ----------
(u0, v0), (u1, v1) = MARKERS_PX
(x0, y0), (x1, y1) = MARKERS_CM
cm_per_px_u = (y1 - y0) / (u1 - u0)       # u (columns) maps to world y


def px_to_cm(u, v):
    return (x0 - (v - v0) * CM_PER_PX_V, y0 + (u - u0) * cm_per_px_u)


# ---------- 2. detection ----------
def blobs(image):
    seen, out = set(), []
    for v, row in enumerate(image):
        for u, ch in enumerate(row):
            if ch in 'RBX' and (u, v) not in seen:
                stack, pix = [(u, v)], []
                seen.add((u, v))
                while stack:
                    a, b = stack.pop(); pix.append((a, b))
                    for da, db in ((1, 0), (-1, 0), (0, 1), (0, -1)):
                        na, nb = a + da, b + db
                        if 0 <= nb < len(image) and 0 <= na < len(image[0]) and (na, nb) not in seen and image[nb][na] == ch:
                            seen.add((na, nb)); stack.append((na, nb))
                us = [p[0] for p in pix]; vs = [p[1] for p in pix]
                cu, cv = sum(us) / len(us), sum(vs) / len(vs)
                w_px, h_px = max(us) - min(us) + 1, max(vs) - min(vs) + 1
                out.append(dict(kind=ch, px=(cu, cv), pos=px_to_cm(cu, cv),
                                size=(h_px * CM_PER_PX_V, w_px * abs(cm_per_px_u))))
    return out


# ---------- 3. IK + collision ----------
def ik(x, y):
    r2 = x * x + y * y
    c = (r2 - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    if abs(c) > 1:
        return []
    sols = []
    for s in (1, -1):
        e = s * math.acos(c)
        sh = math.atan2(y, x) - math.atan2(L2 * math.sin(e), L1 + L2 * math.cos(e))
        sols.append((math.degrees(sh), math.degrees(e)))
    return sols


def ok_limits(q):
    return -90 <= q[0] <= 90 and -90 <= q[1] <= 90   # hobby servos: servo angle = joint + 90


def links(q):
    a, b = math.radians(q[0]), math.radians(q[0] + q[1])
    p1 = (L1 * math.cos(a), L1 * math.sin(a))
    p2 = (p1[0] + L2 * math.cos(b), p1[1] + L2 * math.sin(b))
    return [((0.0, 0.0), p1), (p1, p2)]


def seg_dist(p, a, b):
    ax, ay = a; bx, by = b; px, py = p
    dx, dy = bx - ax, by - ay
    t = max(0.0, min(1.0, ((px - ax) * dx + (py - ay) * dy) / (dx * dx + dy * dy or 1)))
    return math.hypot(px - ax - t * dx, py - ay - t * dy)


def collides(q, obstacles):
    for (cx, cy), r in obstacles:
        for a, b in links(q):
            if seg_dist((cx, cy), a, b) < r + PAD:
                return True
    return False


def path_clear(qa, qb, obstacles, steps=60):
    for i in range(steps + 1):
        t = i / steps
        q = (qa[0] + t * (qb[0] - qa[0]), qa[1] + t * (qb[1] - qa[1]))
        if collides(q, obstacles):
            return False
    return True


def plan(q_from, goal_xy, obstacles):
    """Try each IK solution, direct first, then via HOME. Returns (q_goal, path) or (None, reason)."""
    sols = [q for q in ik(*goal_xy) if ok_limits(q) and not collides(q, obstacles)]
    if not sols:
        return None, 'no collision-free IK solution'
    for q in sorted(sols, key=lambda q: abs(q[0] - q_from[0]) + abs(q[1] - q_from[1])):
        if path_clear(q_from, q, obstacles):
            return q, [q]
    for q in sols:
        if path_clear(q_from, HOME, obstacles) and path_clear(HOME, q, obstacles):
            return q, [HOME, q]
    return None, 'no clear path'


# ---------- run the pipeline ----------
print('1. Calibration')
print(f'   columns: {cm_per_px_u:+.2f} cm per pixel (world y), rows: {CM_PER_PX_V:.2f} cm per pixel (world x)')
print(f'   check: marker 2 pixel {MARKERS_PX[1]} -> {px_to_cm(*MARKERS_PX[1])} cm (expected {MARKERS_CM[1]})\n')

world = blobs(IMAGE)
objects = [b for b in world if b['kind'] in 'RB']
obstacles = []
print('2. Detection -> world model')
for b in world:
    x, y = b['pos']
    print(f"   {b['kind']} at pixel ({b['px'][0]:4.1f}, {b['px'][1]:4.1f}) -> ({x:5.1f}, {y:5.1f}) cm, size {b['size'][0]:.0f} x {b['size'][1]:.0f} cm")
    if b['kind'] == 'X':
        obstacles.append(((x, y), max(b['size']) / 2 * math.sqrt(2)))
for o in objects:  # objects are obstacles for each other while not being picked
    o['as_obstacle'] = (o['pos'], max(o['size']) / 2)
print(f'   {len(objects)} objects, {len(obstacles)} obstacle(s)\n')

print('3. Plan and "execute": nearest first, then re-plan what was skipped')
q = HOME
done, commands = 0, []
for pass_no in (1, 2):
    print(f'   pass {pass_no}')
    progress = False
    for o in sorted(objects, key=lambda o: math.hypot(*o['pos'])):
        if o.get('done') or o.get('never'):
            continue
        name = f"{o['kind']} at ({o['pos'][0]:.1f}, {o['pos'][1]:.1f})"
        width = min(o['size'])
        if width > GRIPPER_MAX_CM:
            print(f'     {name}: SKIP - {width:.0f} cm wide, gripper opens {GRIPPER_MAX_CM:.0f} cm (needs a human)')
            o['never'] = True
            continue
        others = obstacles + [p['as_obstacle'] for p in objects if p is not o and not p.get('done')]
        q_pick, path = plan(q, o['pos'], others)
        if q_pick is None:
            print(f'     {name}: SKIP - pick: {path}')
            continue
        q_place, path2 = plan(q_pick, BINS[o['kind']], others)
        if q_place is None:
            print(f'     {name}: SKIP - place: {path2}')
            continue
        via1 = ' via HOME' if len(path) > 1 else ''
        via2 = ' via HOME' if len(path2) > 1 else ''
        print(f'     {name}: pick q=({q_pick[0]:.0f}, {q_pick[1]:.0f}){via1}; place in bin {o["kind"]} q=({q_place[0]:.0f}, {q_place[1]:.0f}){via2}')
        for qq in path:
            commands.append(f'J {qq[0]:.0f} {qq[1]:.0f}')
        commands += ['D', 'G', 'U']
        for qq in path2:
            commands.append(f'J {qq[0]:.0f} {qq[1]:.0f}')
        commands += ['D', 'O', 'U']
        o['done'] = True
        q = q_place
        done += 1
        progress = True
    if not progress:
        break
left = [o for o in objects if not o.get('done')]
print(f'   -> {done} of {len(objects)} objects placed; left for a human: ' + ', '.join(f"{o['kind']} at ({o['pos'][0]:.1f}, {o['pos'][1]:.1f})" for o in left) + '\n')

print('4. Commands for the UNO executor (first object)')
first = commands[:commands.index('U', commands.index('O')) + 1] if commands else []
print('   ' + ' | '.join(first))

print('\n5. What if we pad obstacles by 4.5 cm instead of 1.5 cm?')
PAD = 4.5
target = [o for o in objects if o['kind'] == 'R'][-1]
q_pick, path = plan(HOME, target['pos'], obstacles)
print(f"   {target['kind']} at ({target['pos'][0]:.1f}, {target['pos'][1]:.1f}): " + ('reachable' if q_pick else f'blocked - {path}'))

Output (run with python3 capstone_pick_place.py)

1. Calibration
   columns: -1.00 cm per pixel (world y), rows: 1.00 cm per pixel (world x)
   check: marker 2 pixel (31, 0) -> (24.0, -15.5) cm (expected (24.0, -15.5))

2. Detection -> world model
   R at pixel ( 3.0,  2.0) -> ( 22.0,  12.5) cm, size 3 x 3 cm
   R at pixel (12.0,  4.0) -> ( 20.0,   3.5) cm, size 3 x 3 cm
   B at pixel (21.0,  5.0) -> ( 19.0,  -5.5) cm, size 3 x 3 cm
   X at pixel (21.5,  9.5) -> ( 14.5,  -6.0) cm, size 4 x 4 cm
   B at pixel (28.5, 15.0) -> (  9.0, -13.0) cm, size 5 x 6 cm
   4 objects, 1 obstacle(s)

3. Plan and "execute": nearest first, then re-plan what was skipped
   pass 1
     B at (9.0, -13.0): SKIP - 5 cm wide, gripper opens 4 cm (needs a human)
     B at (19.0, -5.5): SKIP - pick: no collision-free IK solution
     R at (20.0, 3.5): SKIP - pick: no clear path
     R at (22.0, 12.5): pick q=(11, 41); place in bin R q=(36, 86)
   pass 2
     B at (19.0, -5.5): SKIP - pick: no collision-free IK solution
     R at (20.0, 3.5): pick q=(46, -83); place in bin R q=(36, 86)
   -> 2 of 4 objects placed; left for a human: B at (19.0, -5.5), B at (9.0, -13.0)

4. Commands for the UNO executor (first object)
   J 11 41 | D | G | U | J 36 86 | D | O | U

5. What if we pad obstacles by 4.5 cm instead of 1.5 cm?
   R at (20.0, 3.5): blocked - no clear path

Try it: run the capstone pipeline in the browser and watch each stage report success or a failure reason. Change the scene and see which stage fails first. (Educational simulation — not an industrial planner.)

Capstone · Vision-guided collision-free pick & place · Educational

Capstone pipeline
DetectPoseWorldGraspIKPathTrajExecute

Ready — run Detect → … → Execute. Educational pipeline only. · grasp A

Educational browser pipeline — not industrial RRT / IK / grasp / collision. Reuses Stage 8 ik2R + Stage 9 syncedJoints. Bridges Stages 8–12.

Bench build: the UNO as an honest executor

capstone_executor.ino keeps the UNO simple and strict. It accepts five commands — J <shoulder> <elbow>, D, U, G, O — and answers OK or ERR <reason> to each. It refuses joint angles outside −90° to 90° (servo = joint + 90), refuses to swing the arm while the gripper is down, moves both joints together in small steps so the motion stays close to the planned joint path, closes the jaw until the FSR reaches HOLD and checks for slip with a small test lift. A stop button freezes everything until the board is reset. Your PC program sends one command, waits for OK, and stops the plan at the first ERR.

Safety: never power servos, motors or other high-current loads from a GPIO pin or the UNO 5 V pin — four servos can draw far more current than the board can supply. Use a separate 5–6 V supply with a common ground. The stop button is a software stop for hobby servos; for anything stronger, fit a real E-stop that cuts motor power. Keep hands out of the arm’s reach while a plan is running.
PartConnectionNote
Shoulder servosignal → D9joint 0° = servo 90° pointing along +x (towards the tray)
Elbow servosignal → D10joint 0° = servo 90° = arm straight
Lift servosignal → D11LIFT_UP / LIFT_DOWN measured with the gripper empty
Gripper servosignal → D6JAW_OPEN / JAW_CLOSED measured on your gripper
Servo supplyred → +5–6 V, brown → supply GNDseparate supply sized for four servos; supply GND joined to UNO GND
FSR on a jaw padUNO 5 V → FSR → A1 → 10 kΩ → GNDHOLD from Lesson 11
Stop buttonD3 → button → GNDINPUT_PULLUP; freezes until reset
USB webcamto the PC, looking straight downfixed mount; do not move it after calibrating
An Arduino UNO on a table, wired with coloured jumper wires to a breadboard and to a blue HC-SR04 ultrasonic module
A tidy UNO + breadboard bench like the one from earlier lessons. For the capstone, keep the servo supply on its own rail and join only the grounds.Photo: Pradana Aumars (talk · contribs) · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP
capstone_executor.ino · Arduino C++
// Stage 13 - Lesson 16 (capstone): command executor for a 2-link arm (Arduino UNO)
// The PC runs perception + planning (capstone_pick_place.py) and sends simple commands.
// The UNO checks limits, moves smoothly, verifies the grasp and answers OK / ERR.
//   J <shoulder> <elbow>   joint angles in degrees, each -90..90 (same limits as the planner)
//   D / U                  lower / raise the gripper (lift servo)
//   G                      close until the FSR says HOLD, then test-lift and verify
//   O                      open
// Wiring:
//   Shoulder servo D9, Elbow servo D10, Lift servo D11, Jaw servo D6
//   Servo power: separate 5-6 V supply (NOT the UNO 5V pin), grounds joined
//   FSR on a jaw pad: 5V -> FSR -> A1 -> 10k -> GND
//   Stop button D3 -> GND (INPUT_PULLUP): stops the queue, opens nothing, holds position
// A hobby servo turns about 0..180 deg, so servo = joint + 90. Commands outside
// -90..90 are refused here - keep the planner limits the same as YOUR arm's.
// All angles and thresholds are EXAMPLE values.
#include <Servo.h>

const int SH_PIN = 9, EL_PIN = 10, LIFT_PIN = 11, JAW_PIN = 6, FSR_PIN = A1, STOP_PIN = 3;
const int LIFT_UP = 40, LIFT_DOWN = 120;
const int JAW_OPEN = 100, JAW_CLOSED = 30;
const int HOLD = 300;

Servo sh, el, lift, jaw;
int shPos = 90, elPos = 90 + 90, liftPos = LIFT_UP, jawPos = JAW_OPEN;  // start folded (elbow joint +90)
bool stopped = false;

bool stopPressed() { return digitalRead(STOP_PIN) == LOW; }

// move one servo smoothly; returns false if the stop button was pressed
bool ease(Servo &s, int &pos, int target, int msPerDeg) {
  while (pos != target) {
    if (stopPressed()) { stopped = true; return false; }
    pos += (target > pos) ? 1 : -1;
    s.write(pos);
    delay(msPerDeg);
  }
  return true;
}

void reply(const __FlashStringHelper *msg) { Serial.println(msg); }

void doJoints(int shoulderDeg, int elbowDeg) {
  int s = shoulderDeg + 90, e = elbowDeg + 90;
  if (s < 0 || s > 180 || e < 0 || e > 180) { reply(F("ERR joint limit")); return; }
  if (liftPos != LIFT_UP) { reply(F("ERR lift first (U)")); return; }   // never swing low
  // move both joints together in small steps so the path stays close to the planned one
  while (shPos != s || elPos != e) {
    if (stopPressed()) { stopped = true; reply(F("ERR stopped")); return; }
    if (shPos != s) { shPos += (s > shPos) ? 1 : -1; sh.write(shPos); }
    if (elPos != e) { elPos += (e > elPos) ? 1 : -1; el.write(elPos); }
    delay(15);
  }
  reply(F("OK"));
}

void doGrasp() {
  while (analogRead(FSR_PIN) < HOLD) {
    if (stopPressed()) { stopped = true; reply(F("ERR stopped")); return; }
    if (jawPos <= JAW_CLOSED) { reply(F("ERR closed on air")); return; }
    jawPos--; jaw.write(jawPos); delay(15);
  }
  int before = analogRead(FSR_PIN);
  if (!ease(lift, liftPos, liftPos - 3, 20)) { reply(F("ERR stopped")); return; }
  delay(300);
  int after = analogRead(FSR_PIN);
  if (after < before * 0.7) { reply(F("ERR slip")); return; }
  reply(F("OK"));
}

void setup() {
  Serial.begin(115200);
  pinMode(STOP_PIN, INPUT_PULLUP);
  sh.attach(SH_PIN); el.attach(EL_PIN); lift.attach(LIFT_PIN); jaw.attach(JAW_PIN);
  sh.write(shPos); el.write(elPos); lift.write(liftPos); jaw.write(jawPos);
  Serial.println(F("capstone_executor ready: J s e | D | U | G | O"));
}

void loop() {
  if (stopped) {
    if (Serial.available()) { Serial.read(); reply(F("ERR stopped - reset the board")); }
    return;
  }
  if (!Serial.available()) return;
  char c = Serial.read();
  switch (c) {
    case 'J': { int s = Serial.parseInt(); int e = Serial.parseInt(); doJoints(s, e); break; }
    case 'D': if (ease(lift, liftPos, LIFT_DOWN, 12)) reply(F("OK")); else reply(F("ERR stopped")); break;
    case 'U': if (ease(lift, liftPos, LIFT_UP, 12)) reply(F("OK")); else reply(F("ERR stopped")); break;
    case 'G': doGrasp(); break;
    case 'O': jaw.write(JAW_OPEN); jawPos = JAW_OPEN; delay(300); reply(F("OK")); break;
    default: break;   // ignore spaces and newlines
  }
}
Serial Monitor (115200) — one object, typed by hand · Text
capstone_executor ready: J s e | D | U | G | O
J 11 41
OK
D
OK
G
OK
U
OK
J 36 86
OK
D
OK
O
OK
U
OK
J 120 0
ERR joint limit

Debugging

SymptomLikely causeFix
Arm goes to the right angle but the wrong placecalibration or link lengths wrongRe-measure markers, L1, L2; check joint zero directions
Placed several cm offservo zero not at joint 0°Adjust the horn / add a per-servo offset
Arm sweeps the bottlepadding too small, or bottle size measured from a bad blobIncrease PAD; check the detection printout
Many “no clear path”pick order, or padding too bigRe-plan in passes; choose padding from measured error
ERR joint limit for planned movesplanner limits differ from the sketchUse the same limits in both
ERR closed on airdetection or calibration offsetCheck the pixel → cm mapping on a known point
UNO resets during movesservos on UNO 5 V, or weak supplySeparate, bigger supply; grounds joined

Real world

Vision-guided picking in the real world

A white and grey mobile robot with one arm, a head with cameras and a round base, standing in a competition arena
PAL Robotics TIAGo at RoboCup 2016. Service robots combine cameras, a world model, grasp planning and motion planning — the same stack as this capstone, on a bigger robot.Photo: ubahnverleih · CC0 · Source: Wikimedia Commons · resized, converted to WebP
  • Service robots: mobile manipulators like TIAGo (above) combine cameras, a world model and an arm to pick objects in home-style tasks.
  • Factories and warehouses: cameras find parts or boxes, planners choose grasps and paths, and cells log what was done (Lessons 14–15).
  • MoveIt Task Constructor: the whole pick-and-place task is planned as stages before execution, and failures are shown per stage.
  • Your lab: webcam + Python planner + UNO executor — small, but the same structure, and honest about what it cannot do.

Think

Think about it

Think

The blue block behind the bottle can never be reached by your 2-link arm. Without moving the bottle by hand, what could a smarter system do?

Experiment

Build it in the lab

About 2–3 hours, in a pair or a small team. A PC with Python 3 and a fixed USB webcam looking straight down; an Arduino UNO with shoulder, elbow, lift and gripper servos on their own supply, an FSR with a 10 kΩ resistor, a stop button; small red and blue blocks, one too-big box, a tall bottle and two bins.

Experiment

Build and run the full capstone

You will need

  • `capstone_pick_place.py` (Python 3, no libraries)
  • Capstone interactive (in your browser)
  • Arduino UNO, four SG90/MG90-class servos, separate 5–6 V supply, FSR, 10 kΩ resistor, push button, jumper wires
  • USB webcam on a fixed stand, two paper markers, ruler, blocks, a box, a bottle, two bins

Steps

  1. Build the arm and measure L1, L2, the joint zero directions and the joint limits. Put them in BOTH the script and the sketch.
  2. Mount the webcam, place the two markers, measure their desk positions and update MARKERS_PX / MARKERS_CM. Check the mapping on a third point with a ruler.
  3. Turn your camera image into the character grid (threshold red, blue and dark pixels — or type a grid by hand for the first test).
  4. Run the planner. For every object, write down: placed, or the exact reason it was skipped.
  5. Send the commands for the first object by hand in the Serial Monitor. Measure where the block lands in the bin.
  6. Automate the sending (one command, wait for OK, stop on ERR) and run the whole tray. Then move the bottle and run again.

Capstone · Vision-guided collision-free pick & place · Educational

Capstone pipeline
DetectPoseWorldGraspIKPathTrajExecute

Ready — run Detect → … → Execute. Educational pipeline only. · grasp A

Educational browser pipeline — not industrial RRT / IK / grasp / collision. Reuses Stage 8 ik2R + Stage 9 syncedJoints. Bridges Stages 8–12.

A working capstone does not place everything. It places what it can safely place, and explains the rest in words a human can act on.

Knowledge check

Check your understanding

Knowledge check

1. Why does the script check marker 2 after calibrating?

2. Why was the big blue box skipped?

3. Why did the red block at (20.0, 3.5) fail in pass 1 but succeed in pass 2?

4. A planar 2-link arm can usually reach a point with…

5. Why does the UNO refuse `J` while the gripper is down?

6. The planner allows elbow angles up to 150° but the sketch refuses anything beyond 90°. What happens?

7. Increasing obstacle padding from 1.5 cm to 4.5 cm…

8. Which is the most professional final report?

Mini assignment

Capstone assignment

Mini assignment

Stage 13 capstone: sort the tray

Build a vision-guided, collision-free pick-and-place cell that sorts red and blue blocks into bins, never touches the obstacle, and reports honestly.

Deliverables

  • A photo or sketch of your cell with all measurements (links, markers, bins, padding).
  • Your calibration check: a third point measured with a ruler vs the script’s value.
  • The planner printout for your real tray, and the final report of a full run (placed, skipped with reasons).
  • A table of 10 placed blocks: distance from the bin centre (mm).
  • A short video (or a step-by-step log) of one complete run, including one honest “needs a human”.
  • One page: the biggest source of error in your cell, and what you would change first.

Do this, step by step

  1. Run capstone_pick_place.py and read the final report (placed and skipped, with reasons).
  2. Put your marker positions in MARKERS_CM and check a third point with a ruler.
  3. Change IMAGE, BINS and PAD to match your tray and run the planner again.
  4. Bench: servos on their own supply, grounds joined, stop button on pin 3; upload capstone_executor.ino and do one full run.
  5. Log 10 placed blocks: distance from the bin centre in mm.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. If every position is off by the same amount, your markers in MARKERS_CM do not match the real desk. Re-measure them first.
  2. A block wider than GRIPPER_MAX_CM (4 cm) must be skipped with a reason, not forced.
  3. If one IK solution hits the obstacle, try the other elbow before reporting “no collision-free IK”.

Challenges (pick by level)

  • 🟢 Beginner: Run the script, then move the bottle in the character grid (change the X positions). Which objects become reachable?
  • 🟡 Intermediate: Add a third colour and a third bin. Add a check that a bin is not full (count what was placed).
  • 🟠 Advanced: Replace the character grid with a real webcam frame: threshold the colours with OpenCV and build the grid (or the blob list) automatically.
  • 🔴 Expert: Add a “move the obstacle first” task plan: if the bottle is marked graspable and a free spot exists, plan bottle → spot, block → bin, bottle → back.
  • ⚫ Research: Compare your two-pass re-planning with planning the whole task at once, as MoveIt Task Constructor does. When can planning everything up front find a solution that greedy, one-object-at-a-time planning misses?

Stage 13 complete — next step

Congratulations — you have finished Stage 13: Perception, Planning & Manipulation. Your robot can see, build a world model, plan collision-free motions, grasp, place and report honestly. Next comes Stage 14 — Advanced Robotics / Reinforcement Learning, where robots start to learn behaviours from experience instead of only following the plans you write.

Look back: Lesson 15: Manipulation Pipeline. Next: Stage 14 — Advanced Robotics & Reinforcement Learning. Related: Stage 14 — Advanced Robotics & RL.

Sources

Self-attested for now — keep artifacts in your lab notebook.

Complete

Today's goal

0 of 1 done today

0-day streak
Daily goal:lessons, quizzes or assignments a day

Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).

Stuck somewhere?

Ask a doubt about this lesson

Saved with this lesson's id and your email so we can reply. Privacy · or email contact@aihubstation.in