Stage 13 · Lesson 01/16

Robot Perception, Planning & Manipulation · Lesson 01

From Perception to Robot Action

Est. 60–75 min

Why

Why this lesson matters

Up to now you have built pieces separately: kinematics and IK (Stage 8), trajectories and control (Stages 7–9), ROS 2 (Stage 10), vision (Stage 11) and simulation (Stage 12). Stage 13 joins them. A robot that manipulates things must sense the world, interpret what it sees, keep a model of the world, plan what to do, and then act — and then check again.

In this lesson you will make one full trip from camera pixel to gripper, in plain Python, and then break each step on purpose to see what goes wrong. On the bench, an Arduino UNO runs the same loop with an ultrasonic sensor and a servo pointer.

Concept

Sense → interpret → model → plan → act

1. Three ways to connect sensing and acting

Robotics books describe a robot by how Sense, Plan and Act are connected. In the hierarchical (deliberative) paradigm the robot senses, updates one internal model of the world, plans the next action from that model, and acts — every step. In the reactive paradigm, sensing is connected straight to action (“sense–act”) through small behaviours, with no central plan. The hybrid paradigm plans first (break the task into sub-tasks) and then runs reactive behaviours to carry them out.

Most real manipulation systems are hybrid: a planner on top, fast safety reflexes underneath.
ParadigmLoopGood atWeak atDesk-arm example
HierarchicalSense → Plan → ActCareful, multi-step tasksSlow if the world changesPlan a full pick-and-place from one photo
ReactiveSense → ActFast reflexesNo long-term goalsStop the moment the bump switch closes
HybridPlan, then reactive behavioursBothMore complex to buildPlan the pick, but re-look and stop on contact while moving
Tall wheeled robot from the 1960s with a camera head, a range finder and an antenna, with labels pointing to each part
Shakey (SRI International), the first general-purpose mobile robot able to reason about its own actions. It combined computer vision, planning and action — and gave us the A* search algorithm and the Hough transform.Photo: SRI International · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP

2. The five steps for a robot arm

  1. Sense: get raw data — a camera image, a depth image, an ultrasonic echo, joint encoders.
  2. Interpret: turn raw data into facts — “a red block, centred at pixel (67, 68)”.
  3. Model: put the facts into the robot’s world model, in the robot’s own frame, with a time stamp and how sure we are — “block at x = 22.0 cm, y = 4.9 cm in base_link, seen 0.1 s ago”.
  4. Plan: decide what to do — is it reachable? which waypoints? which IK solution? is the path clear?
  5. Act: send the trajectory to the controllers, and watch (sense again) while moving.

3. From pixel to point: the pinhole camera

A camera does not give you a position on the table. It gives you a pixel (u, v). OpenCV’s calibration documentation uses the pinhole camera model: a point (X, Y, Z) in the camera frame lands at pixel u = fx·X/Z + cx, v = fy·Y/Z + cy. Here fx, fy are the focal lengths in pixels and (cx, cy) is the principal point, near the image centre. All points along one ray land on the same pixel — so one pixel alone does not tell you the depth Z. You must get Z from somewhere: a depth camera, a known table height, or a second camera.

If you know Z, go backwards: X = (u − cx)·Z / fx, Y = (v − cy)·Z / fy. That is a point in the camera frame. The arm needs it in its base frame, so you apply the camera-to-base transform (rotation + translation) — exactly the frame chains you met in Stage 12 Lesson 5.

Diagram of a 3D point projected through a camera centre onto an image plane, shown in two views with axes labelled
The idealised pinhole (central projection) camera: a 3D point is mapped onto the image plane along a ray through the camera centre. Every point on that ray hits the same pixel.Photo: Olaf Peters (OlafTheScientist) · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

4. Open loop vs closed loop

The simplest pipeline looks once, plans, and moves (“look-then-move”). It is fine if nothing changes. But if the object moves, or the camera was bumped, the arm goes confidently to the wrong place. Visual servoing (vision-based robot control) uses camera feedback during the motion. The literature names eye-in-hand (camera on the gripper) and eye-to-hand (camera fixed in the world), and splits control into image-based (IBVS) and position-based (PBVS). In this stage we use a middle path: plan from a snapshot, then re-look at the pre-grasp pose before closing the gripper.

Visual

Visualise it: the robot’s world model

The robot never acts on the real world directly — it acts on its world model. Toggle layers and add perception noise to see the model drift from the truth. (Educational explorer, not an industrial perception stack.)

Interactive · World model explorer · Educational

Perception to actionRobot world model layers

Mesh/primitive shapes of table, walls, and arm links — a teaching stand-in for CAD/URDF visuals.

World honesty score (toy)

85

geometry

Educational browser world model — not MoveIt planning scene / not Gazebo / not industrial map stack.

Code lab

Code lab: one full trip from pixel to gripper

perception_to_action.py draws a fake overhead camera image (160 × 120 pixels, fx = fy = 150, camera 60 cm above the base plane — example values) with a 4 cm red block on the desk. It finds the red pixels, takes their centroid, back-projects the pixel with the pinhole model, transforms it to the arm’s base frame, checks reach (35 cm example), plans pre-grasp → grasp → lift, and checks whether the jaws (1 cm tolerance, example) would land around the block. Then it breaks each step. Run python3 perception_to_action.py — no libraries needed.

perception_to_action.py · Python
"""perception_to_action.py — Stage 13 L01 (AIHUB STATION lab)

One full trip: SENSE -> INTERPRET -> MODEL -> PLAN -> ACT, in plain Python.
A fake overhead camera "photographs" a red block on the desk, we find the
block's pixels, turn the pixel into a 3D point with the pinhole model,
move it into the arm's base frame, check reach, plan waypoints, and
"execute" them.  Then we break each step on purpose to see what goes wrong.

All numbers (camera, arm, block) are EXAMPLE values for a desk setup.
Pinhole model: u = fx*X/Z + cx, v = fy*Y/Z + cy (OpenCV calib3d docs).
No libraries needed. Run:  python3 perception_to_action.py
"""
import math, random

# ---------- example camera (small image so it runs fast) ----------
W, H = 160, 120                    # image size in pixels
FX = FY = 150.0                    # focal length in pixels (example)
CX, CY = 80.0, 60.0                # principal point (image centre)
CAM_IN_BASE = (0.25, 0.0, 0.60)    # camera 60 cm above the base plane, 25 cm in front
# Camera looks straight down.  Optical frame: x right, y down (in image), z forward.
# In the base frame (x forward, y left, z up):  x_cam = -y_base, y_cam = -x_base, z_cam = -z_base
def cam_to_base(p, tilt_deg=0.0):
    xc, yc, zc = p
    if tilt_deg:                    # unknown mounting tilt about the camera x axis
        t = math.radians(tilt_deg)
        yc, zc = yc*math.cos(t) - zc*math.sin(t), yc*math.sin(t) + zc*math.cos(t)
    bx, by, bz = CAM_IN_BASE
    return (bx - yc, by - xc, bz - zc)

def base_to_cam(p):
    bx, by, bz = CAM_IN_BASE
    x, y, z = p
    return (-(y - by), -(x - bx), -(z - bz))

def project(p_cam):
    x, y, z = p_cam
    return (FX*x/z + CX, FY*y/z + CY)

# ---------- SENSE: render a fake image of a 4 cm red block ----------
BLOCK = 0.04       # block side (m)
BLOCK_TOP = 0.03   # top face height above the base plane (m)
def render(block_xy, noise=0):
    img = [[(200, 190, 170)] * W for _ in range(H)]          # beige desk
    cx_, cy_ = block_xy
    for v in range(H):
        for u in range(W):
            # ray through pixel centre hits the plane z = BLOCK_TOP
            zc = CAM_IN_BASE[2] - BLOCK_TOP
            xc = (u + 0.5 - CX) * zc / FX
            yc = (v + 0.5 - CY) * zc / FY
            bx, by, _ = cam_to_base((xc, yc, zc))
            if abs(bx - cx_) <= BLOCK/2 and abs(by - cy_) <= BLOCK/2:
                img[v][u] = (210, 30, 30)                       # red block
    if noise:                                                   # flickering pixels
        for _ in range(noise):
            u, v = random.randrange(W), random.randrange(H)
            img[v][u] = (210, 30, 30)
    return img

# ---------- INTERPRET: colour threshold + centroid ----------
def find_red(img, min_pixels=30):
    su = sv = n = 0
    for v, row in enumerate(img):
        for u, (r, g, b) in enumerate(row):
            if r > 150 and g < 80 and b < 80:
                su += u + 0.5; sv += v + 0.5; n += 1
    if n < min_pixels:
        return None
    return (su/n, sv/n, n)

def pixel_to_base(u, v, depth_z, tilt_deg=0.0):
    xc = (u - CX) * depth_z / FX
    yc = (v - CY) * depth_z / FY
    return cam_to_base((xc, yc, depth_z), tilt_deg)

# ---------- PLAN: reach check + waypoints ----------
REACH = 0.35       # example desk-arm reach from the shoulder axis (m)
GRIP_TOL = 0.010   # example: jaws open 1 cm wider than the block on each side
def plan(target):
    x, y, z = target
    r = math.hypot(x, y)
    if r > REACH:
        return None, f"REFUSE: target {r*100:.1f} cm away, reach is {REACH*100:.0f} cm"
    yaw = math.degrees(math.atan2(y, x))
    wps = [("pre-grasp", (x, y, z + 0.08)), ("grasp", (x, y, z - 0.01)), ("lift", (x, y, z + 0.10))]
    return wps, f"OK: base yaw {yaw:+.1f} deg, radius {r*100:.1f} cm"

# ---------- ACT: did the jaws land around the block? ----------
def act(wps, true_xy):
    gx, gy, _ = dict(wps)["grasp"]
    err = math.hypot(gx - true_xy[0], gy - true_xy[1])
    return err, ("GRASPED" if err <= GRIP_TOL else "MISSED")

def mm(a): return f"{a*1000:.1f} mm"

def full_trip(title, true_xy, depth=None, tilt=0.0, ignore_frames=False, noise=0, moved_to=None, relook=False):
    print(f"\n--- {title} ---")
    img = render(true_xy, noise)
    det = find_red(img)
    if det is None:
        print("SENSE/INTERPRET: no red block found -> robot waits, does not move"); return
    u, v, n = det
    zc = depth if depth is not None else CAM_IN_BASE[2] - BLOCK_TOP
    if ignore_frames:
        xc = (u - CX) * zc / FX; yc = (v - CY) * zc / FY
        target = (xc, yc, BLOCK_TOP)                 # camera numbers used as if base numbers
    else:
        target = pixel_to_base(u, v, zc, tilt)
        target = (target[0], target[1], BLOCK_TOP)
    print(f"INTERPRET: {n} red pixels, centroid u={u:.1f} v={v:.1f} -> base x={target[0]*100:.1f} cm y={target[1]*100:.1f} cm")
    print(f"MODEL: error vs truth {mm(math.hypot(target[0]-true_xy[0], target[1]-true_xy[1]))}")
    wps, msg = plan(target)
    print("PLAN:", msg)
    if wps is None: return
    actual = true_xy
    if moved_to is not None:
        actual = moved_to
        print(f"(while the arm was moving, someone nudged the block to x={moved_to[0]*100:.1f} y={moved_to[1]*100:.1f} cm)")
        if relook:
            det2 = find_red(render(moved_to))
            t2 = pixel_to_base(det2[0], det2[1], zc)
            wps, msg = plan((t2[0], t2[1], BLOCK_TOP))
            print("RE-LOOK at pre-grasp:", msg)
    err, res = act(wps, actual)
    print(f"ACT: jaws off by {mm(err)} -> {res}")

if __name__ == "__main__":
    random.seed(7)
    print("Camera example: %dx%d px, fx=fy=%.0f px, 60 cm above base plane" % (W, H, FX))
    zc = CAM_IN_BASE[2] - BLOCK_TOP
    print("One pixel on the block top covers about %.1f mm" % (zc / FX * 1000))
    truth = (0.22, 0.05)
    print("Truth: block centre at x=22.0 cm y=5.0 cm (base frame), top 3 cm above base plane")
    print("Check: truth projects to pixel u=%.1f v=%.1f" % project(base_to_cam((0.22, 0.05, BLOCK_TOP))))
    full_trip("1. Nominal trip", truth)
    full_trip("2. Forgot the frame change (used camera numbers as base numbers)", truth, ignore_frames=True)
    full_trip("3. Wrong depth: assumed the desk (60 cm) instead of block top (57 cm)", truth, depth=0.60)
    full_trip("4. Camera bumped: real mount tilted 2 deg, model still says 0", truth, tilt=2.0)
    full_trip("5. Block out of reach", (0.42, 0.20))
    full_trip("6. Look once, then move (open loop) - block nudged 3 cm", truth, moved_to=(0.25, 0.05))
    full_trip("7. Re-look before grasping (closed the loop)", truth, moved_to=(0.25, 0.05), relook=True)
    full_trip("8. Flickering red pixels (40 noise pixels)", truth, noise=40)
    print("\n--- 9. Nothing on the desk ---")
    print("SENSE/INTERPRET:", "found" if find_red(render((5.0, 5.0))) else "no red block found -> robot waits, does not move")

Output (run with python3 perception_to_action.py)

Camera example: 160x120 px, fx=fy=150 px, 60 cm above base plane
One pixel on the block top covers about 3.8 mm
Truth: block centre at x=22.0 cm y=5.0 cm (base frame), top 3 cm above base plane
Check: truth projects to pixel u=66.8 v=67.9

--- 1. Nominal trip ---
INTERPRET: 100 red pixels, centroid u=67.0 v=68.0 -> base x=22.0 cm y=4.9 cm
MODEL: error vs truth 0.7 mm
PLAN: OK: base yaw +12.7 deg, radius 22.5 cm
ACT: jaws off by 0.7 mm -> GRASPED

--- 2. Forgot the frame change (used camera numbers as base numbers) ---
INTERPRET: 100 red pixels, centroid u=67.0 v=68.0 -> base x=-4.9 cm y=3.0 cm
MODEL: error vs truth 270.1 mm
PLAN: OK: base yaw +148.4 deg, radius 5.8 cm
ACT: jaws off by 270.1 mm -> MISSED

--- 3. Wrong depth: assumed the desk (60 cm) instead of block top (57 cm) ---
INTERPRET: 100 red pixels, centroid u=67.0 v=68.0 -> base x=21.8 cm y=5.2 cm
MODEL: error vs truth 2.8 mm
PLAN: OK: base yaw +13.4 deg, radius 22.4 cm
ACT: jaws off by 2.8 mm -> GRASPED

--- 4. Camera bumped: real mount tilted 2 deg, model still says 0 ---
INTERPRET: 100 red pixels, centroid u=67.0 v=68.0 -> base x=24.0 cm y=4.9 cm
MODEL: error vs truth 19.5 mm
PLAN: OK: base yaw +11.7 deg, radius 24.5 cm
ACT: jaws off by 19.5 mm -> MISSED

--- 5. Block out of reach ---
INTERPRET: 121 red pixels, centroid u=27.5 v=15.5 -> base x=41.9 cm y=19.9 cm
MODEL: error vs truth 1.0 mm
PLAN: REFUSE: target 46.4 cm away, reach is 35 cm

--- 6. Look once, then move (open loop) - block nudged 3 cm ---
INTERPRET: 100 red pixels, centroid u=67.0 v=68.0 -> base x=22.0 cm y=4.9 cm
MODEL: error vs truth 0.7 mm
PLAN: OK: base yaw +12.7 deg, radius 22.5 cm
(while the arm was moving, someone nudged the block to x=25.0 y=5.0 cm)
ACT: jaws off by 30.4 mm -> MISSED

--- 7. Re-look before grasping (closed the loop) ---
INTERPRET: 100 red pixels, centroid u=67.0 v=68.0 -> base x=22.0 cm y=4.9 cm
MODEL: error vs truth 0.7 mm
PLAN: OK: base yaw +12.7 deg, radius 22.5 cm
(while the arm was moving, someone nudged the block to x=25.0 y=5.0 cm)
RE-LOOK at pre-grasp: OK: base yaw +11.2 deg, radius 25.5 cm
ACT: jaws off by 0.6 mm -> GRASPED

--- 8. Flickering red pixels (40 noise pixels) ---
INTERPRET: 140 red pixels, centroid u=73.0 v=62.6 -> base x=24.0 cm y=2.7 cm
MODEL: error vs truth 30.7 mm
PLAN: OK: base yaw +6.4 deg, radius 24.2 cm
ACT: jaws off by 30.7 mm -> MISSED

--- 9. Nothing on the desk ---
SENSE/INTERPRET: no red block found -> robot waits, does not move

Try it: switch world-model layers on and off and raise perception noise. Watch how the model the planner sees drifts from the true scene. (Educational explorer, not an industrial perception stack.)

Interactive · World model explorer · Educational

Perception to actionRobot world model layers

Mesh/primitive shapes of table, walls, and arm links — a teaching stand-in for CAD/URDF visuals.

World honesty score (toy)

85

geometry

Educational browser world model — not MoveIt planning scene / not Gazebo / not industrial map stack.

Bench build: a sense–plan–act loop on the UNO

sense_plan_act.ino is the same loop, small enough for an UNO. Sense: an HC-SR04 ultrasonic sensor measures distance. The SparkFun-hosted HC-SR04 datasheet gives 2 cm – 4 m range, a 10 µs trigger pulse and a 15° measuring angle. Model: take the median of 5 readings, and only trust an object seen 3 times in a row. Plan: objects 5–25 cm away are “in reach”; map the distance to a pointer angle (30°–150°). Anything else → park at 90°. Act: move a servo pointer at most 60°/s, re-planning every loop.

Safety: a servo can draw far more current than a GPIO pin or the UNO 5 V pin can safely give, especially when it stalls. Never power motors or servos from a GPIO pin. Keep fingers clear of the pointer arm, and unplug the servo supply before changing wiring.
PartConnectionNote
HC-SR04VCC → 5V, GND → GND, TRIG → D7, ECHO → D8datasheet: 5 V, about 15 mA
Servo (SG90-class)signal → D9power from a separate 5–6 V supply, not the UNO
Servo supplyred → +5–6 V, brown → supply GNDsupply GND joined to UNO GND
Green LED + 220 ΩD5 → resistor → LED → GNDtarget trusted, acting
Red LED + 220 ΩD6 → resistor → LED → GNDno target / out of reach
sense_plan_act.ino · Arduino C++
// sense_plan_act.ino — Stage 13 L01 (AIHUB STATION lab)
// A tiny SENSE -> MODEL -> PLAN -> ACT loop on an Arduino UNO.
// SENSE : HC-SR04 measures how far an object is (2 cm - 4 m per datasheet).
// MODEL : median of 5 readings, and the object must be seen 3 times in a row.
// PLAN  : only objects 5-25 cm away are "in reach"; map distance to a pointer angle.
// ACT   : move a servo pointer there slowly (max 60 deg/s), re-checking every 50 ms.
//
// Wiring (signals only from the UNO):
//   HC-SR04 VCC -> 5V, GND -> GND, TRIG -> D7, ECHO -> D8
//   Servo signal -> D9.  Servo red/brown -> SEPARATE 5-6 V supply; supply GND to UNO GND.
//   Green LED: D5 -> 220 ohm -> LED -> GND  (acting on a target)
//   Red LED:   D6 -> 220 ohm -> LED -> GND  (no target / out of reach)
// Never power the servo from a GPIO pin or the UNO 5V pin.
#include <Servo.h>

const byte TRIG = 7, ECHO = 8, SERVO_PIN = 9, LED_OK = 5, LED_NO = 6;
const float NEAR_CM = 5.0, FAR_CM = 25.0;      // example "reach" of our pointer arm
const float MAX_DEG_PER_S = 60.0;              // speed limit for the pointer
const byte CONFIRM = 3;                        // see it 3 times before trusting it

Servo pointer;
float angleNow = 90.0;                         // where the pointer is (deg)
byte seenCount = 0;
unsigned long lastStep = 0;

float readCm() {                               // one HC-SR04 reading, -1 if no echo
  digitalWrite(TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH); delayMicroseconds(10);   // at least 10 us trigger
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 30000UL);   // ~5 m round trip max
  if (us == 0) return -1;
  return us * 0.0343 / 2.0;                    // speed of sound ~343 m/s, there and back
}

float medianOf5() {
  float r[5];
  for (byte i = 0; i < 5; i++) { r[i] = readCm(); delay(12); }
  for (byte i = 0; i < 5; i++)                 // tiny sort
    for (byte j = i + 1; j < 5; j++)
      if (r[j] < r[i]) { float t = r[i]; r[i] = r[j]; r[j] = t; }
  return r[2];
}

void setup() {
  Serial.begin(115200);
  pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT);
  pinMode(LED_OK, OUTPUT); pinMode(LED_NO, OUTPUT);
  pointer.attach(SERVO_PIN);
  pointer.write((int)angleNow);
  Serial.println(F("sense_plan_act: SENSE -> MODEL -> PLAN -> ACT every 50 ms"));
}

void loop() {
  if (millis() - lastStep < 50) return;        // at most 20 Hz (5 echoes can take longer)
  float dt = (millis() - lastStep) / 1000.0;
  if (dt > 0.2) dt = 0.2;                      // first loop / slow echo: cap the step
  lastStep = millis();

  // SENSE + MODEL
  float d = medianOf5();
  bool inReach = (d >= NEAR_CM && d <= FAR_CM);
  seenCount = inReach ? min(seenCount + 1, 255) : 0;
  bool trusted = seenCount >= CONFIRM;

  // PLAN
  float goal = 90.0;                           // park position when nothing trusted
  if (trusted) goal = 30.0 + (d - NEAR_CM) * (120.0 / (FAR_CM - NEAR_CM));

  // ACT (rate limited, re-planned every loop = closed loop)
  float maxStep = MAX_DEG_PER_S * dt;
  float err = goal - angleNow;
  if (err >  maxStep) err =  maxStep;
  if (err < -maxStep) err = -maxStep;
  angleNow += err;
  pointer.write((int)(angleNow + 0.5));

  digitalWrite(LED_OK, trusted);
  digitalWrite(LED_NO, !trusted);

  Serial.print(F("d=")); Serial.print(d, 1);
  Serial.print(F("cm seen=")); Serial.print(seenCount);
  Serial.print(trusted ? F(" PLAN goal=") : F(" WAIT park=")); Serial.print(goal, 0);
  Serial.print(F(" angle=")); Serial.println(angleNow, 0);
}
Serial Monitor (115200) — example session, your values will differ · Text
sense_plan_act: SENSE -> MODEL -> PLAN -> ACT every 50 ms
d=-1.0cm seen=0 WAIT park=90 angle=90
d=14.8cm seen=1 WAIT park=90 angle=90
d=14.9cm seen=2 WAIT park=90 angle=90
d=14.8cm seen=3 PLAN goal=89 angle=89
d=14.9cm seen=4 PLAN goal=89 angle=89
d=32.0cm seen=0 WAIT park=90 angle=90
A long black sensor bar with several camera lenses on the front, on a small stand
A Kinect sensor (2010). Depth cameras like this give a Z value for every pixel — the missing piece when you turn a pixel into a 3D point.Photo: James Pfaff (litheon) · CC BY 2.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

SymptomLikely causeFix
Arm goes to a mirror-image or rotated spotcamera-frame numbers used as base-frame numbers, or a wrong axis signWrite the frame next to every number; check one point by hand
Error grows towards the image edgeswrong depth, or lens distortion not removedUse the real object height or a depth camera; undistort with calibration
Everything off by the same few cm after someone touched the standcamera extrinsics changedRe-run the camera-to-base calibration
Arm jumps between two placesflickering detectionsLargest blob only; require N agreeing frames; filter
Arm reaches, but object has movedlook-once (open loop)Re-look at pre-grasp; check during the move
Bench: distance shows -1.0no echo (too far, soft surface, wiring)Check TRIG/ECHO pins; use a hard flat target 5–25 cm away
Bench: UNO resets when the servo movesservo powered from the UNOSeparate servo supply with common ground

Real world

Perception-to-action in the real world

Close-up of a robotic end effector holding a pool cue tip above a billiard table
A robotic pool cue: a computer-vision system finds the balls, the computer plans a shot, and a 6-DOF end effector takes it — perception to action on a table.Photo: Sdwighton · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP
  • Research history: Shakey at SRI combined computer vision, planning and action, and produced the A* search algorithm and the Hough transform — tools you will still use in this stage.
  • Warehouses and factories: a camera finds parts in a tray, software turns pixels into poses in the robot’s frame, a planner chooses a grasp and path, and safety systems stop motion if something unexpected enters the cell.
  • Games and entertainment: depth cameras like the Kinect turned pixels into body positions so games could react to players.
  • Your lab: the 6DOF desk arm with an overhead or wrist camera: detect → back-project → transform to base_link → reach check → plan → re-look → grasp.

Think

Think about it

Think

Your detector is 99% accurate, but the arm still misses about one pick in ten. List at least four places between “detection” and “gripper closed” where the error could come from, and how you would test each one.

Experiment

Try it in the lab

About 60 minutes. Python 3 for the script; Arduino UNO, HC-SR04, a small servo with its own 5–6 V supply, two LEDs and two 220 Ω resistors for the bench.

Experiment

Follow one pixel all the way to the gripper

You will need

  • `perception_to_action.py` (Python 3, no libraries)
  • World-model explorer (in your browser)
  • Arduino UNO, HC-SR04, SG90-class servo + separate 5–6 V supply, green and red LEDs, two 220 Ω resistors, jumper wires

Steps

  1. Run `perception_to_action.py`. Check the “Check” line by hand with the pinhole formula.
  2. Move the block to (0.30, −0.10). Is it in reach? What is the error in the nominal case?
  3. In case 4, try tilts of 0.5°, 1° and 5°. At what tilt does the grasp start to miss?
  4. In case 3, make the block 10 cm tall (BLOCK_TOP = 0.10) and keep the wrong depth. How big is the error now?
  5. Add a function that keeps only the largest connected group of red pixels and re-run case 8.
  6. Bench: test the pointer with your hand at 10, 15, 20 and 30 cm. Note when it acts and when it parks.
  7. Bench: set MAX_DEG_PER_S to 300. What changes, and why is a speed limit useful near people?

Interactive · World model explorer · Educational

Perception to actionRobot world model layers

Mesh/primitive shapes of table, walls, and arm links — a teaching stand-in for CAD/URDF visuals.

World honesty score (toy)

85

geometry

Educational browser world model — not MoveIt planning scene / not Gazebo / not industrial map stack.

Perception is only the first link. Most misses come from frames, depth, calibration, timing or acting on noise — not from the detector.

Knowledge check

Check your understanding

Knowledge check

1. In the hierarchical (deliberative) paradigm the robot…

2. Why can one camera pixel not give you the 3D position by itself?

3. In the pinhole model u = fx·X/Z + cx, what is cx?

4. In the script, forgetting the camera-to-base transform gave an error of about…

5. The block was nudged 3 cm during the move. What fixed the miss?

6. The detector finds nothing on the desk. The correct action is…

7. Why does the bench sketch need 3 readings in a row before acting?

8. How should the servo be powered?

Mini assignment

Hands-on assignment

Mini assignment

A perception-to-action report for your desk arm

Draw and test the full chain from camera pixel to gripper for your own desk arm setup.

Deliverables

  • A diagram of your chain: sensor → detector → back-projection → frame transform → world model → reach check → planner → controller, with the frame name on every arrow.
  • Your camera numbers (fx, fy, cx, cy, height above the table) — from a real calibration if you have one, or clearly marked as example values.
  • A table of five deliberate mistakes (from the script or your own setup) with the measured error in mm.
  • A bench log from `sense_plan_act.ino` showing WAIT, PLAN and a refusal for an out-of-reach object.

Do this, step by step

  1. Run perception_to_action.py once and copy the final error line into your notes.
  2. Put your own camera numbers into CAM_IN_BASE (and fx, fy, cx, cy if you have them) and run it again.
  3. Break one step at a time on purpose (wrong tilt, wrong frame, no depth) and write the error in mm for each.
  4. Draw your chain diagram with the frame name on every arrow.
  5. Bench: servo on its own 5–6 V supply, grounds joined, upload sense_plan_act.ino and log WAIT, PLAN and one refusal outside NEAR_CM–FAR_CM.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. If the error is many centimetres, check the frame first: a point in camera frame used as a base point is the most common mistake.
  2. REACH is 0.35 m in the example. If your block is farther, the script must refuse, not move.
  3. The sketch only acts after CONFIRM = 3 readings in a row. If it never plans, print each reading and look for jumps.

Challenges (pick by level)

  • 🟢 Beginner: For three pixels of your choice, compute X and Y by hand with Z = 0.57 m and check them with the script.
  • 🟡 Intermediate: Add a “largest blob only” step and a “3 frames must agree within 5 mm” rule. Show case 8 now passes.
  • 🟠 Advanced: Replace the fake camera with a real webcam and OpenCV: colour-threshold a red block, find its centroid, and print its base-frame position using your measured camera height.
  • 🔴 Expert: Calibrate your webcam with an OpenCV chessboard, undistort the image, and measure how much the corner error drops compared with no calibration.
  • ⚫ Research: Read about eye-in-hand vs eye-to-hand visual servoing and IBVS vs PBVS. Write one page on which would suit your desk arm and why.

Next step

The planner never sees the real world — it sees the world model. In Lesson 2 — Robot World Models you will build one: objects, obstacles, time stamps and confidence, and learn how to keep it honest as things change.

Look back: Stage 12 Lesson 16: Capstone — Build & Test a Simulated 6DOF Arm. Next: Lesson 2: Robot World Models. Related: Stage 11 — Computer Vision.

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