Stage 13 · Lesson 08/16

Robot Perception, Planning & Manipulation · Lesson 08

Collision-Free Motion

Est. 60–75 min

Why

Why this lesson matters

In Lessons 6 and 7 you learnt to plan the whole arm in configuration space. Every planner in the next lessons — PRM, RRT, RRT* — does two things again and again: it checks whether a configuration is free, and it checks whether the straight motion between two configurations is free. The second check is where most bugs hide. Check too coarsely and thin obstacles slip between your samples. Check too finely and the planner crawls. And even a perfect check is only true for the world as it was when you checked.

In the code lab you will watch a coarse check miss a pen, compute a step size that cannot miss, see what padding costs, find collisions sooner by checking in bisection order, and see a planned motion become unsafe when someone moves the pen. On the bench, an Arduino moves a servo in small steps and checks an ultrasonic sensor before every step, stopping when something comes close.

Concept

Checking motions, not just poses

1. Pose check vs motion check

CheckQuestionUsed for
Pose (configuration) checkIs this one configuration in C-free?start, goal, every sampled node
Motion (edge) checkIs every configuration on the straight line between qa and qb in C-free?every edge of a road map or tree, every segment of the final path
Execution checkIs the world still as it was when we planned?just before and during the real move
A white square with grey obstacles and a red curved line from a start dot to a goal dot that passes through a grey obstacle
An invalid path in configuration space: start and goal are both in free space, but the curve between them passes through an obstacle region. Checking only the ends would call it valid.Photo: Simeon87 · CC BY-SA 3.0 · Source: Wikimedia Commons · resized, converted to WebP

2. Discrete checking: samples along the motion

The simplest motion check walks along the straight joint-space line from qa to qb in small steps and runs the pose check at each step. If one step collides, the motion is rejected. The danger is the step size: if an obstacle is thin, or the arm only grazes it, the collision may last for a tiny part of the motion — and a large step can jump right over it. In our script the collision lasts only 3.9% of the motion, and checks every 10° or 5° both miss it.

3. A step size that cannot miss

We can do better than guessing. If the shoulder turns by Δq1 (in radians), no point of a 2-link arm moves more than Δq1 × (L1 + L2); if the elbow turns by Δq2, no point moves more than Δq2 × L2. Add the two and you have an upper bound on how far any point of the arm can travel between two checks. Choose the step so this bound is smaller than the clearance you demand at every checked pose, and nothing can slip through in between. This is the idea behind conservative motion checking.

Movement bound for the 2-link desk arm · Text
max movement of any point  <=  |dq1| * (L1 + L2)  +  |dq2| * L2      (dq in radians)

Our motion: dq1 = 65 deg, dq2 = -40 deg  ->  at most 39.0 cm
To move at most 1 cm per check: 40 checks.  At most 0.5 cm: 79 checks.

4. Continuous collision checking

Collision libraries can also do continuous collision detection: instead of testing snapshots, they test whether two moving models touch at any time during the motion. The open-source Flexible Collision Library (FCL) lists continuous collision detection next to ordinary collision checking, distance computation and tolerance verification. Distance computation is what makes the step bound above useful: “how far is the arm from the nearest obstacle right now?” tells you how big the next safe step can be.

5. Padding

Models are never perfect: links are a bit fatter than their capsules, a servo overshoots, the camera puts the pen 5 mm off. So we pad — demand some clearance instead of just “not touching”. Padding makes motions safer but removes configurations: in our script, 2 cm of padding removes about 7% of the poses that were free with no padding, and narrow gaps can close completely. Pad by your real model error, not by fear.

6. The world can change

A motion checked at 10:00:00 is only known to be free at 10:00:00. If a person reaches in or someone moves the pen, the plan is stale. Real systems re-check the planning scene before executing, monitor it while moving, and stop when something enters the path. For people, industrial robots use separate certified safety systems — our ultrasonic bench guard is a learning tool, never a safety device for humans.

A blue HC-SR04 ultrasonic module with two round silver transducers marked T and R and four pins labelled Vcc, Trig, Echo, GND
The HC-SR04 ultrasonic module used on the bench: one transducer sends a ping (T), the other listens (R). Its datasheet gives distance in cm as the echo time in µs divided by 58.Photo: Suyash Dwivedi · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Visual

Visualise it: a straight line, a via point and an obstacle

A straight motion from start to goal can pass through an obstacle even when both ends are free. Adding a via point makes two motions that can each be checked and kept free. (Educational explorer, not an industrial motion planner.)

Interactive · Collision-free planner · Educational

Collision-free motion
Direct path: COLLIDES · Via path: CLEAR (edu)

Straight-line + one via — not industrial collision-free motion planning.

Code lab

Code lab: check the motion, not just its ends

collision_free_motion_lab.py uses the desk arm (links 15 cm and 12 cm, link radius 1.5 cm) and a thin pen (radius 5 mm) at (18, 7) cm. Six parts: (1) both end poses are free, but the motion is not; (2) uniform checks with different step sizes; (3) a safe step size from the movement bound; (4) the cost of padding; (5) bisection order; (6) a motion that becomes unsafe when the pen is moved. Plain Python 3. Run python3 collision_free_motion_lab.py.

collision_free_motion_lab.py · Python
"""collision_free_motion_lab.py — Stage 13 L08 (AIHUB STATION lab)

Checking a whole MOTION, not just its end poses.
Top view of the 2-link desk arm, example values in metres:
  links 0.15 m and 0.12 m, link capsule radius 1.5 cm.
  A thin PEN stands upright at (0.18, 0.07), radius 0.5 cm.
Run:  python3 collision_free_motion_lab.py
"""
import math

L1, L2, LINK_R = 0.15, 0.12, 0.015
PEN, PEN_R = (0.18, 0.07), 0.005

def fk(q1, q2):
    a, b = math.radians(q1), math.radians(q1 + q2)
    e = (L1*math.cos(a), L1*math.sin(a))
    return e, (e[0] + L2*math.cos(b), e[1] + L2*math.sin(b))

def seg_dist(a, b, p):
    ax, ay = a; bx, by = b; px, py = p
    L = (bx-ax)**2 + (by-ay)**2
    s = max(0, min(1, ((px-ax)*(bx-ax) + (py-ay)*(by-ay)) / L))
    return math.hypot(ax + s*(bx-ax) - px, ay + s*(by-ay) - py)

def clearance(q, obs=None):
    obs = obs or PEN
    e, t = fk(*q)
    return min(seg_dist((0, 0), e, obs), seg_dist(e, t, obs)) - PEN_R - LINK_R

def lerp(qa, qb, s): return (qa[0] + (qb[0]-qa[0])*s, qa[1] + (qb[1]-qa[1])*s)

def check_uniform(qa, qb, step_deg, pad=0.0, obs=None):
    n = max(1, math.ceil(max(abs(qb[0]-qa[0]), abs(qb[1]-qa[1])) / step_deg))
    for i in range(n + 1):
        if clearance(lerp(qa, qb, i/n), obs) < pad: return False, i + 1
    return True, n + 1

A, B = (-35.0, 110.0), (30.0, 70.0)
print("1. Both END poses are free:")
print(f"   start q={A}: clearance {100*clearance(A):.1f} cm;  goal q={B}: clearance {100*clearance(B):.1f} cm")
worst = min((clearance(lerp(A, B, i/2000)), i/2000) for i in range(2001))
print(f"   ...but on the way the worst clearance is {1000*worst[0]:.1f} mm at s={worst[1]:.3f} -> the arm HITS the pen")

print("\n2. Checking only some poses along the motion (uniform steps in joint space)")
for step in (35.0, 20.0, 10.0, 5.0, 2.0, 1.0):
    ok, checks = check_uniform(A, B, step)
    print(f"   step {step:4.0f} deg: {checks:3d} checks -> {'looks FREE (wrong!)' if ok else 'collision found'}")
bad = [i/1000 for i in range(1001) if clearance(lerp(A, B, i/1000)) < 0]
print(f"   the collision lasts only from s={bad[0]:.3f} to s={bad[-1]:.3f} ({100*len(bad)/1001:.1f}% of the motion): big steps jump over it")

print("\n3. A safe step size from a distance bound")
# For a joint change dq (radians), no point of the arm moves more than
#   |dq1|*(L1+L2) + |dq2|*L2   (shoulder swings everything, elbow swings link 2)
def max_move(dq1_deg, dq2_deg):
    return abs(math.radians(dq1_deg))*(L1 + L2) + abs(math.radians(dq2_deg))*L2
d1, d2 = B[0]-A[0], B[1]-A[1]
for margin_cm in (1.0, 0.5):
    m = margin_cm / 100
    n = math.ceil(max_move(d1, d2) / m)
    print(f"   whole motion moves any point at most {100*max_move(d1, d2):.1f} cm; to move <= {margin_cm} cm per check: {n} steps")
print("   If every checked pose has clearance > that move, nothing can slip through in between.")

print("\n4. Padding: plan with a safety margin")
for pad_cm in (0.0, 0.5, 1.0, 2.0):
    free = sum(clearance((q1, q2)) >= pad_cm/100 for q1 in range(-90, 91, 2) for q2 in range(0, 151, 2))
    print(f"   padding {pad_cm:.1f} cm: {free} of {91*76} grid poses allowed")
print("   More padding = safer against model errors, but fewer allowed poses (and narrow gaps close).")

print("\n5. Check the middle first (bisection order)")
def bisect_order(n):
    out, segs = [0, n], [(0, n)]
    while segs:
        nxt = []
        for a, b in segs:
            if b - a > 1:
                m = (a + b) // 2; out.append(m); nxt += [(a, m), (m, b)]
        segs = nxt
    return out
n = 140
order_uniform = list(range(n + 1))
order_bisect = bisect_order(n)
def first_hit(order):
    for k, i in enumerate(order):
        if clearance(lerp(A, B, i/n)) < 0: return k + 1
for name, order in (("start-to-end", order_uniform), ("bisection", order_bisect)):
    print(f"   {name:12s}: collision found after {first_hit(order)} of {n+1} checks")
print("   Bisection spreads its first checks over the whole motion (ends, middle, quarters...),\n   so it usually finds a collision much sooner. A FREE motion still needs every check.")

print("\n6. The world moves: re-check before you go")
C, D = (30.0, 70.0), (60.0, 40.0)
ok, _ = check_uniform(C, D, 1.0)
print(f"   new motion q={C} -> q={D} with the pen at {PEN}: {'FREE' if ok else 'collision'}")
mid_tip = fk(*lerp(C, D, 0.5))[1]
moved = (round(mid_tip[0], 2), round(mid_tip[1], 2))
ok2, _ = check_uniform(C, D, 1.0, obs=moved)
print(f"   someone moves the pen to {moved}: {'FREE' if ok2 else 'collision -> stop and re-plan'}")

Output (run with python3 collision_free_motion_lab.py)

1. Both END poses are free:
   start q=(-35.0, 110.0): clearance 2.8 cm;  goal q=(30.0, 70.0): clearance 3.0 cm
   ...but on the way the worst clearance is -0.3 mm at s=0.250 -> the arm HITS the pen

2. Checking only some poses along the motion (uniform steps in joint space)
   step   35 deg:   3 checks -> looks FREE (wrong!)
   step   20 deg:   2 checks -> collision found
   step   10 deg:   8 checks -> looks FREE (wrong!)
   step    5 deg:  14 checks -> looks FREE (wrong!)
   step    2 deg:   9 checks -> collision found
   step    1 deg:  17 checks -> collision found
   the collision lasts only from s=0.231 to s=0.269 (3.9% of the motion): big steps jump over it

3. A safe step size from a distance bound
   whole motion moves any point at most 39.0 cm; to move <= 1.0 cm per check: 40 steps
   whole motion moves any point at most 39.0 cm; to move <= 0.5 cm per check: 79 steps
   If every checked pose has clearance > that move, nothing can slip through in between.

4. Padding: plan with a safety margin
   padding 0.0 cm: 6545 of 6916 grid poses allowed
   padding 0.5 cm: 6444 of 6916 grid poses allowed
   padding 1.0 cm: 6331 of 6916 grid poses allowed
   padding 2.0 cm: 6064 of 6916 grid poses allowed
   More padding = safer against model errors, but fewer allowed poses (and narrow gaps close).

5. Check the middle first (bisection order)
   start-to-end: collision found after 34 of 141 checks
   bisection   : collision found after 4 of 141 checks
   Bisection spreads its first checks over the whole motion (ends, middle, quarters...),
   so it usually finds a collision much sooner. A FREE motion still needs every check.

6. The world moves: re-check before you go
   new motion q=(30.0, 70.0) -> q=(60.0, 40.0) with the pen at (0.18, 0.07): FREE
   someone moves the pen to (0.09, 0.22): collision -> stop and re-plan

Try it: the straight line from start to goal hits the box. Move the via point up and down until both segments are free. (Educational explorer, not an industrial motion planner.)

Interactive · Collision-free planner · Educational

Collision-free motion
Direct path: COLLIDES · Via path: CLEAR (edu)

Straight-line + one via — not industrial collision-free motion planning.

Bench build: check before every step

motion_guard.ino sweeps a servo arm between 30° and 150° in 1° steps. Before every step it reads an HC-SR04 ultrasonic sensor mounted on the arm’s path. If anything is closer than 12 cm, it stops at once with the red LED on, waits until the space has been clear for a full second, then continues. Small steps mean each check covers only a small move — the same idea as the step bound in the script.

An Arduino UNO on a table, wired with coloured jumper wires to a breadboard and to a blue HC-SR04 ultrasonic module
An Arduino UNO wired to an HC-SR04 ultrasonic module through a breadboard — the same sensor wiring as the bench build (this photo also has a Bluetooth module on the breadboard).Photo: Pradana Aumars (talk · contribs) · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP
Safety: the servo must have its own supply — never power it from a GPIO pin or the UNO 5 V pin. This guard is a teaching demo, not a safety device: an ultrasonic sensor can miss soft, thin or angled objects. Never rely on it to protect hands; keep fingers out of the arm’s path.
PartConnectionNote
Arm servo (SG90-class)signal → D9separate 5–6 V supply; GND joined to UNO GND
HC-SR04 VCC / GNDUNO 5V / GNDsmall sensor, fine on the 5V pin
HC-SR04 TRIGD710 µs trigger pulse
HC-SR04 ECHOD8echo time ÷ 58 = cm
Red LED + 220 ΩD6 → resistor → LED → GNDlit while stopped
motion_guard.ino · Arduino C++
// motion_guard.ino — Stage 13 L08 (AIHUB STATION lab)
// A planned motion was collision-free WHEN IT WAS PLANNED. The world can change.
// This sketch moves a servo arm in small steps and checks an HC-SR04 ultrasonic
// sensor before EVERY step. If something comes closer than STOP_CM, it stops
// at once, waits until the space has been clear for CLEAR_MS, then continues.
//
// Wiring (Arduino UNO):
//   Arm servo signal -> D9; servo red/brown -> SEPARATE 5-6 V supply, GND joined to UNO GND
//   HC-SR04: VCC -> UNO 5V, GND -> GND, TRIG -> D7, ECHO -> D8
//   (the HC-SR04 is a small 5 V sensor, fine on the 5V pin; the SERVO is not)
//   Red LED + 220 ohm on D6: lit while stopped
#include <Servo.h>

const int PIN_SERVO = 9, PIN_TRIG = 7, PIN_ECHO = 8, PIN_LED = 6;
const float STOP_CM = 12.0;            // stop if anything is closer than this
const unsigned long CLEAR_MS = 1000;   // must stay clear this long before moving again
const int STEP_DEG = 1;                // small steps: each check covers a small move
const int STEP_MS = 20;

Servo arm;
int pos = 30;
int target = 150;

float readCm() {
  digitalWrite(PIN_TRIG, LOW);  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH); delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  unsigned long us = pulseIn(PIN_ECHO, HIGH, 30000UL);   // time out after 30 ms
  if (us == 0) return 999.0;                             // no echo: treat as far
  return us / 58.0;                                      // about 58 us per cm (round trip)
}

bool spaceClear() { return readCm() >= STOP_CM; }

void setup() {
  Serial.begin(115200);
  pinMode(PIN_TRIG, OUTPUT); pinMode(PIN_ECHO, INPUT); pinMode(PIN_LED, OUTPUT);
  arm.attach(PIN_SERVO);
  arm.write(pos);
  delay(500);
  Serial.println(F("Motion guard: sweeping 30 <-> 150 deg, checking before every step."));
}

void loop() {
  if (pos == target) {                     // reached the end: plan the way back
    target = (target == 150) ? 30 : 150;
    Serial.print(F("reached ")); Serial.print(pos); Serial.println(F(" deg, turning back"));
    delay(500);
  }
  if (!spaceClear()) {
    digitalWrite(PIN_LED, HIGH);
    Serial.print(F("STOP at ")); Serial.print(pos); Serial.println(F(" deg: obstacle too close"));
    unsigned long clearSince = 0;
    while (true) {                         // wait until clear for CLEAR_MS
      if (spaceClear()) {
        if (clearSince == 0) clearSince = millis();
        if (millis() - clearSince >= CLEAR_MS) break;
      } else {
        clearSince = 0;
      }
      delay(50);
    }
    digitalWrite(PIN_LED, LOW);
    Serial.println(F("clear for 1 s: continuing"));
  }
  pos += (target > pos) ? STEP_DEG : -STEP_DEG;
  arm.write(pos);
  delay(STEP_MS);
}
Serial Monitor (115200) — example run with a hand placed in front of the sensor · Text
Motion guard: sweeping 30 <-> 150 deg, checking before every step.
reached 150 deg, turning back
STOP at 97 deg: obstacle too close
clear for 1 s: continuing
reached 30 deg, turning back

Debugging

SymptomLikely causeFix
Planner accepts a motion that hits a thin objectstep between checks too largeUse the movement bound to choose the step, or continuous collision checking
Motion checks make planning very slowtiny fixed step everywhereUse distance to the nearest obstacle to take bigger safe steps when far away
Arm brushes obstacles in real life but not in simulationmodel error, no paddingMeasure the error; pad by that amount
No motion found through a gap that looks wide enoughtoo much paddingReduce padding to the real error; check the gap width
Arm hits something that was not there when planningstale planning sceneRe-check before executing; monitor while moving; stop and re-plan
Bench: random stops with nothing thereecho noise, no timeout, sensor sees the tableUse a timeout, aim the sensor level, require two close readings in a row

Real world

Collision-free motion in the real world

  • Planning frameworks: MoveIt keeps its planning scene up to date from the robot state and sensors (Lesson 2), so collision checks are made against the latest picture of the world.
  • Collision libraries: FCL offers collision detection, distance computation, tolerance verification and continuous collision detection — the building blocks of the checks in this lesson.
  • Factories: robot cells use certified safety systems (fences, interlocks, safety scanners) to protect people; motion planning protects the robot and the parts.
  • Your lab: always ask two questions before pressing run — “did I check the whole motion?” and “is the world still the same as when I planned?”

Think

Think about it

Think

Your step bound says 40 checks are enough if every check has 1 cm of clearance. One checked pose has only 4 mm of clearance — but it is not in collision. Is the motion safe?

Experiment

Try it in the lab

About 60 minutes. Python 3 for the script; Arduino UNO, one servo with its own supply, an HC-SR04, an LED and a 220 Ω resistor for the bench.

Experiment

Catch the pen

You will need

  • `collision_free_motion_lab.py` (Python 3, no libraries)
  • Collision-free planner explorer (in your browser)
  • Arduino UNO, SG90-class servo + separate 5–6 V supply, HC-SR04, red LED, 220 Ω resistor, jumper wires, a pen and a book as obstacles

Steps

  1. Run the script. Find the largest uniform step that still catches the pen. Is it the same for a pen twice as thick?
  2. Write a function that checks with the step from the movement bound and demands 1 cm of clearance at each check. Does it reject the motion?
  3. Make the check adaptive: at each pose, measure clearance d and take the next step so the movement bound is less than d. Count the checks.
  4. Move the pen in part 6 to three other places. Which ones block the new motion?
  5. In the explorer, find the lowest and highest via points that give a free path.
  6. Bench: measure the stop distance at STEP_DEG = 1 and 10 by moving a book towards the sensor at the same speed.

Interactive · Collision-free planner · Educational

Collision-free motion
Direct path: COLLIDES · Via path: CLEAR (edu)

Straight-line + one via — not industrial collision-free motion planning.

A motion is safe only if every pose along it is safe, in the world as it is now. Choose the checking step from a bound, pad by your real error, and re-check when the world may have changed.

Knowledge check

Check your understanding

Knowledge check

1. Start and goal poses are both collision-free. The straight motion between them…

2. Why did the 10° check miss the pen?

3. For a 2-link arm, the most any point can move for joint changes Δq1, Δq2 (radians) is at most…

4. Continuous collision detection tests…

5. More padding gives…

6. Checking in bisection order (ends, middle, quarters…) usually…

7. Someone moves an object after you planned. You should…

8. The HC-SR04 bench guard is…

Mini assignment

Hands-on assignment

Mini assignment

A motion checker you can trust

Build a motion checker for your desk arm that cannot miss an obstacle thinner than your chosen clearance, and test it.

Deliverables

  • The movement bound for your arm (your link lengths), written out and checked with one numeric example.
  • A motion-check function (Python) that uses the bound and a clearance demand, with a test where a uniform 10° check misses a collision and yours catches it.
  • A table of checks needed for 5 motions: uniform 1°, bound-based, and adaptive (distance-based).
  • A bench log from `motion_guard.ino` with stop distances for STEP_DEG = 1 and 10.

Do this, step by step

  1. Run collision_free_motion_lab.py and note where uniform checking misses the pen.
  2. Try different step_deg values in check_uniform() and find the largest step that never misses.
  3. Use max_move() to work out how far the arm tip moves for one step.
  4. Compare uniform order with bisect_order() and see which finds the hit sooner.
  5. Bench: upload motion_guard.ino and log a stop when your hand comes closer than STOP_CM (12 cm).

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. Start free and goal free does not mean the motion is free. Check the poses in between.
  2. Choose the step so the tip moves less than the thinnest obstacle (or the padding) between checks.
  3. After a stop, the sketch waits CLEAR_MS (1 s) of clear readings. If it restarts at once, check that timer.

Challenges (pick by level)

  • 🟢 Beginner: Draw the arm at the start, middle and end of the motion in part 1 on graph paper with the pen. Find the pose where it touches.
  • 🟡 Intermediate: Implement the adaptive checker (step from current clearance) and compare its number of checks with uniform 1° steps.
  • 🟠 Advanced: Add the movement bound for a 3-link arm and test it with a self-collision check from Lesson 7.
  • 🔴 Expert: Use the python-fcl bindings (or another collision library) to run distance queries between a capsule arm and a box, and use them in your adaptive checker.
  • ⚫ Research: Read about continuous collision detection and conservative advancement. Write one page on how they guarantee no missed collisions and what they cost.

Next step

You can now check any motion reliably — but searching a grid for a 6-joint arm is hopeless. In Lesson 9 — Sampling-Based Planning you will build planners that sample random configurations and connect them with exactly the motion checks you wrote today.

Look back: Lesson 7: Configuration Space for Manipulators. Next: Lesson 9: Sampling-Based Planning. Related: Stage 9 — Trajectory Planning.

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