Stage 09 · Lesson 15/15

Trajectory Planning & Motion Generation · Lesson 15

Capstone: Plan & Execute a 6DOF Robot Trajectory

Est. 90–120 min

Why

Why this capstone matters

This capstone puts all of Stage 9 together into one pipeline. You will take a task (home → pick → place → home), choose Cartesian or joint-space motion for each segment, give it a smooth time scaling, pick the shortest legal duration, check reach, joint limits, speed, acceleration and final pose error, and only then run it — in the browser and on the 2-servo pen arm.

By the end of this capstone you will be able to: write a complete motion plan; justify each design choice; produce a VALID/INVALID report with reasons; and run a checked cycle on hardware. The arm here is a simplified 2-link planar stand-in for the position part of a 6DOF arm — the same pipeline applies to all six joints.

A wide indoor stadium with a coloured robot game field in the centre and crowds in the stands
The arena at Robocon India 2017 in Pune. Contest robots must move fast and reliably — exactly what a checked trajectory gives you.Photo: Anandjpanchal · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Concept

The full planning pipeline

The pipeline in eight steps

  1. 1. Task → poses

    Write the key poses (home, above pick, pick, place…) as tool positions (and orientations on a 6DOF arm). Lesson 5: waypoints.

  2. 2. IK for each pose

    Check every pose is reachable and pick one IK branch for the whole job (Stage 8).

  3. 3. Choose the motion type

    Cartesian line/arc where the tool path matters (approach, insert, draw) — Lesson 9. Joint space for free transfers — Lesson 4. Orientation by slerp — Lesson 10.

  4. 4. Choose the time scaling

    Quintic or S-curve for smooth rest-to-rest; trapezoid when speed matters most — Lessons 7, 8, 11, 12.

  5. 5. Choose T

    One trial, then the 1/T and 1/T² rule plus a margin — Lesson 13.

  6. 6. Synchronise

    All joints share one s(t) — Lesson 14.

  7. 7. Check everything

    Reach, joint limits, speed, acceleration, (jerk), final pose error. Any failure → INVALID with the reason.

  8. 8. Execute and verify

    Only a VALID plan is streamed to the motors. Measure what really happened.

Which motion type where?

SegmentGood choiceWhy
Approach / pick / insertCartesian lineTool must move straight, e.g. down onto the part
Long transfer in free spaceJoint spaceSimple and fast; joint speeds stay predictable (no singularity blow-up mid-way)
Drawing / welding / gluingCartesian line + arc + orientationThe path itself is the job
Many points in a rowVia points with cubic or blended segmentsDo not stop at each point (Lesson 5)

Visual

Visualise it: plan → generate → check → play

The integrated workflow: start → target → IK → joint-space or Cartesian → time scaling → generate → checks → play → VALID / INVALID (simplified planar arm).

Interactive · Trajectory capstone (plan & execute · educational 2R)

Workflow: inspect start → set target EE → IK (Stage 8 ik2R) → choose joint-space or Cartesian → Linear/Cubic/Quintic → generate → check vel/acc limits → PVA graphs → play sim → final pose error → Valid/Invalid. Simplified planar FK/IK stand-in for a 6DOF position chain — not industrial analytic 6DOF or hardware streaming (Stage 10 ROS later).

INVALID / pending — see results

Results panel

Start EE
119.6, 91.4
Target EE
80, 90
IK target
ok
Goal joints
13.1°, 83.2°
Peak |q̇|
32.4 °/s
Peak |q̈|
39.8 °/s²
Final pose err
0.000
Live EE
119.6, 91.4
Verdict
Invalid
  • · Generate a trajectory first

Code lab

Code lab: the capstone planner

capstone.py plans home (8, 10) → pick (14, 4) as a Cartesian line, pick → place (2, 14) as a joint-space move, and place → home as a Cartesian line. Each segment uses a quintic; its T comes from one trial with the Lesson 13 rule plus 10% margin, rounded up to 0.05 s. Then every sample is checked and the final position is compared with the target by FK. Part 2 shows two targets that must be refused. Run python3 capstone.py.

capstone.py · Python
# capstone.py - Stage 9 Lesson 15: plan, check and report a complete trajectory
# 2-link arm (10 cm + 8 cm, elbow-up). Task: home -> pick -> place -> home.
#   segment 1: Cartesian line (tool must go straight down to the pick)
#   segment 2: joint-space move (fast transfer, tool path may curve)
#   segment 3: Cartesian line back home
# Every segment: quintic s(t); T chosen from ONE trial with the 1/T, 1/T^2 rule;
# then a full check: reach, joint limits, speed, acceleration, final pose error.
import math

L1, L2 = 10.0, 8.0
LIM = [(-60.0, 150.0), (0.0, 160.0)]          # joint limits (deg)
VMAX, AMAX = 120.0, 300.0                      # deg/s, deg/s^2
N = 200                                        # samples per segment

def fk(t1, t2):
    a, b = math.radians(t1), math.radians(t1 + t2)
    return L1*math.cos(a) + L2*math.cos(b), L1*math.sin(a) + L2*math.sin(b)

def ik(x, y):
    c2 = (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2)
    if abs(c2) > 1: return None
    t2 = math.acos(c2)
    t1 = math.atan2(y, x) - math.atan2(L2*math.sin(t2), L1 + L2*math.cos(t2))
    return math.degrees(t1), math.degrees(t2)

def quintic(u): return 10*u**3 - 15*u**4 + 6*u**5

def sample(kind, a, b):
    """Joint samples q_k for s_k = quintic(k/N), k = 0..N, or a problem string."""
    out = []
    if kind == "joint":
        qa, qb = ik(*a), ik(*b)
        if qa is None or qb is None: return "end point out of reach"
    for k in range(N + 1):
        s = quintic(k / N)
        if kind == "line":
            q = ik(a[0] + s*(b[0]-a[0]), a[1] + s*(b[1]-a[1]))
            if q is None: return f"out of reach at s = {s:.2f}"
        else:
            q = tuple(qa[i] + s*(qb[i]-qa[i]) for i in range(2))
        for i in range(2):
            if not LIM[i][0] <= q[i] <= LIM[i][1]:
                return f"joint {i+1} = {q[i]:.1f} deg outside {LIM[i]}"
        out.append(q)
    return out

def peaks(q, T):
    h = T / N
    pv = pa = 0.0
    for i in range(2):
        x = [p[i] for p in q]
        v = [(x[k+1]-x[k]) / h for k in range(N)]
        a = [(v[k+1]-v[k]) / h for k in range(N-1)]
        pv, pa = max(pv, max(map(abs, v))), max(pa, max(map(abs, a)))
    return pv, pa

def plan(name, kind, a, b):
    q = sample(kind, a, b)
    if isinstance(q, str):
        print(f"  {name:22s} INVALID - {q}")
        return None
    v1, a1 = peaks(q, 1.0)                                  # trial at T0 = 1 s
    T = max(v1 / VMAX, math.sqrt(a1 / AMAX))                # shortest legal T
    T = math.ceil(T * 1.1 / 0.05) * 0.05                    # +10% margin, round up to 0.05 s
    pv, pa = peaks(q, T)
    ex, ey = fk(*q[-1])
    err = math.hypot(ex - b[0], ey - b[1])
    ok = pv <= VMAX and pa <= AMAX and err < 0.01
    print(f"  {name:22s} {kind:5s} T = {T:4.2f} s  peak {pv:5.1f} deg/s  {pa:6.1f} deg/s^2  end error {err:.4f} cm  {'VALID' if ok else 'INVALID'}")
    return T if ok else None

HOME, PICK, PLACE = (8.0, 10.0), (14.0, 4.0), (2.0, 14.0)
print("Part 1 - home -> pick -> place -> home")
times = [plan("1 home -> pick", "line", HOME, PICK),
         plan("2 pick -> place", "joint", PICK, PLACE),
         plan("3 place -> home", "line", PLACE, HOME)]
if all(times):
    print(f"  whole trajectory VALID, cycle time {sum(times):.2f} s")

print("\nPart 2 - a target that cannot work")
plan("4 home -> far point", "line", HOME, (19.0, 5.0))
plan("5 home -> behind base", "joint", HOME, (-12.0, -6.0))

Output (run with python3 capstone.py)

Part 1 - home -> pick -> place -> home
  1 home -> pick         line  T = 0.95 s  peak  63.5 deg/s   246.5 deg/s^2  end error 0.0000 cm  VALID
  2 pick -> place        joint T = 1.25 s  peak  96.1 deg/s   236.8 deg/s^2  end error 0.0000 cm  VALID
  3 place -> home        line  T = 0.95 s  peak  72.0 deg/s   246.0 deg/s^2  end error 0.0000 cm  VALID
  whole trajectory VALID, cycle time 3.15 s

Part 2 - a target that cannot work
  4 home -> far point    INVALID - out of reach at s = 0.82
  5 home -> behind base  INVALID - joint 1 = -60.5 deg outside (-60.0, 150.0)

Capstone lab: set start joints and a target, choose joint-space or Cartesian, pick Linear, Cubic or Quintic, set T and limits, generate, read the checks, and play. Try to make it fail on purpose too (simplified planar arm).

Interactive · Trajectory capstone (plan & execute · educational 2R)

Workflow: inspect start → set target EE → IK (Stage 8 ik2R) → choose joint-space or Cartesian → Linear/Cubic/Quintic → generate → check vel/acc limits → PVA graphs → play sim → final pose error → Valid/Invalid. Simplified planar FK/IK stand-in for a 6DOF position chain — not industrial analytic 6DOF or hardware streaming (Stage 10 ROS later).

INVALID / pending — see results

Results panel

Start EE
119.6, 91.4
Target EE
80, 90
IK target
ok
Goal joints
13.1°, 83.2°
Peak |q̇|
32.4 °/s
Peak |q̈|
39.8 °/s²
Final pose err
0.000
Live EE
119.6, 91.4
Verdict
Invalid
  • · Generate a trajectory first

Real world

Planning pipelines in the real world

  • Offline programming: a robot program can be built and checked in simulation (reach, limits, cycle time) before it runs on the real robot.
  • Robot languages combine joint moves and linear/circular moves in one program (MoveJ, MoveL, MoveC in ABB RAPID).
  • Pick-and-place lines such as delta robots over conveyors repeat the same planned cycle again and again, so every saved fraction of a second adds up.
  • Student contests like Robocon reward robots that are fast and reliable — a checked plan gives both.
Two small delta robots with three arms each above a green conveyor belt carrying purple boxes
Delta robots over a conveyor picking and placing parts — one planned cycle, repeated again and again.Photo: Trungdoanhong · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Bench build

Bench build: run the cycle on the pen arm

capstone_pen_arm.ino runs the same three segments with the durations from capstone.py. When you press D2 it checks all segments first (reach, joint limits, joint speed at every 20 ms step) and prints VALID or the reason; only a fully VALID plan moves the arm. It pauses half a second at pick and place.

Wiring
FromToNote
Shoulder servo signalD9Servo angle = θ₁ + 60°
Elbow servo signalD10Servo angle = θ₂
Both servos + and −Separate 5–6 V supply, 2 A or moreNever the Arduino 5 V pin
Supply −Arduino GNDCommon ground
ButtonD2 ↔ GNDINPUT_PULLUP
  1. Mark the targets

    On chart paper, mark the shoulder axis (0, 0), home (8, 10), pick (14, 4) and place (2, 14) in cm.

  2. Run

    Press D2. The Serial Monitor should say “all segments VALID – running”. With the pen down you will see two straight lines and one curved transfer.

  3. Measure

    How close does the pen get to each mark? Time a full cycle (planned 3.15 s plus two 0.5 s pauses).

  4. Break it

    Change the pick to (19, 5) or shorten a T to 0.4 s and upload. The arm must refuse and stay still.

  5. Your task

    Replace the poses with your own task (see the capstone project below), re-plan with capstone.py and copy the new T values.

capstone_pen_arm.ino · Arduino C++
// capstone_pen_arm.ino - Stage 9 Lesson 15: run the planned cycle on the 2-servo pen arm
// home -> pick (Cartesian line) -> place (joint move) -> home (Cartesian line),
// quintic timing, durations from capstone.py. Every segment is checked BEFORE the arm moves
// (reach, joint limits, joint speed); if any check fails, nothing moves.
// Wiring: shoulder servo D9 (servo angle = theta1 + 60), elbow servo D10 (servo angle = theta2).
//         Both servos on a SEPARATE 5-6 V supply (2 A+); supply GND -> Arduino GND. Button D2 -> GND.
#include <Servo.h>

Servo sh, el;
const float L1 = 10.0, L2 = 8.0, SH_OFFSET = 60.0;
const float SH_MIN = -60, SH_MAX = 150, EL_MIN = 0, EL_MAX = 160, V_MAX = 120.0;
const unsigned long DT_MS = 20;

struct Seg { float ax, ay, bx, by, T; bool line; };
const Seg SEGS[3] = {
  { 8, 10, 14, 4, 0.95, true },     // home -> pick
  { 14, 4, 2, 14, 1.25, false },    // pick -> place (joint space)
  { 2, 14, 8, 10, 0.95, true },     // place -> home
};

void writeDeg(Servo &s, float d) { d = constrain(d, 0, 180); s.writeMicroseconds(544 + (int)(d * (2400 - 544) / 180.0)); }
float quintic(float u) { return u * u * u * (10 - 15 * u + 6 * u * u); }

bool ik(float x, float y, float &t1, float &t2) {
  float c2 = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2);
  if (c2 < -1 || c2 > 1) return false;
  t2 = acos(c2);
  t1 = atan2(y, x) - atan2(L2 * sin(t2), L1 + L2 * cos(t2));
  t1 = degrees(t1); t2 = degrees(t2);
  return true;
}

// joint angles of segment g at progress s; false if unreachable
bool jointsAt(const Seg &g, float s, float &t1, float &t2) {
  if (g.line) return ik(g.ax + s * (g.bx - g.ax), g.ay + s * (g.by - g.ay), t1, t2);
  float a1, a2, b1, b2;
  if (!ik(g.ax, g.ay, a1, a2) || !ik(g.bx, g.by, b1, b2)) return false;
  t1 = a1 + s * (b1 - a1); t2 = a2 + s * (b2 - a2);
  return true;
}

bool check(const Seg &g, int idx) {
  int n = (int)(g.T * 1000 / DT_MS);
  float p1 = 0, p2 = 0;
  for (int k = 0; k <= n; k++) {
    float t1, t2;
    if (!jointsAt(g, quintic((float)k / n), t1, t2)) { Serial.print(F("seg ")); Serial.print(idx); Serial.println(F(": out of reach")); return false; }
    if (t1 < SH_MIN || t1 > SH_MAX || t2 < EL_MIN || t2 > EL_MAX) { Serial.print(F("seg ")); Serial.print(idx); Serial.println(F(": joint limit")); return false; }
    if (k > 0) {
      float sp = max(fabs(t1 - p1), fabs(t2 - p2)) / (DT_MS / 1000.0);
      if (sp > V_MAX) { Serial.print(F("seg ")); Serial.print(idx); Serial.println(F(": joint speed")); return false; }
    }
    p1 = t1; p2 = t2;
  }
  return true;
}

void play(const Seg &g) {
  unsigned long t0 = millis();
  while (true) {
    float u = (millis() - t0) / 1000.0 / g.T;
    if (u > 1) u = 1;
    float t1, t2;
    if (jointsAt(g, quintic(u), t1, t2)) { writeDeg(sh, t1 + SH_OFFSET); writeDeg(el, t2); }
    if (u >= 1) break;
    delay(DT_MS);
  }
}

void goHome() {
  float t1, t2;
  ik(SEGS[0].ax, SEGS[0].ay, t1, t2);
  writeDeg(sh, t1 + SH_OFFSET); writeDeg(el, t2);   // quick move at power-up
}

void setup() {
  Serial.begin(115200);
  pinMode(2, INPUT_PULLUP);
  sh.attach(9); el.attach(10);
  goHome();
  Serial.println(F("Press D2 to check and run home -> pick -> place -> home"));
}

void loop() {
  if (digitalRead(2) != LOW) return;
  bool ok = true;
  for (int i = 0; i < 3; i++) ok = ok && check(SEGS[i], i + 1);
  if (!ok) { Serial.println(F("INVALID - not moving")); }
  else {
    Serial.println(F("all segments VALID - running"));
    for (int i = 0; i < 3; i++) { play(SEGS[i]); delay(500); }   // short pause at pick and place
    Serial.println(F("cycle done"));
  }
  while (digitalRead(2) == LOW) delay(10);
}
A large group of engineering students standing behind a small wheeled robot in a workshop
Students of Project Manas, MIT Manipal, with their robot prototype. Student teams test and check every motion before a contest.Photo: Project manas · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging

Capstone debugging checklist

SymptomLikely causeLesson to revisit
INVALID: out of reach mid-pathCartesian line leaves the workspace9 — move the path inward or use a joint move
INVALID: joint limitWrong IK branch or pose too far2, 9 — change branch or pose
INVALID: speed/accelerationT too short11, 13 — stretch T
Arm wobbles at stopsAcceleration jumps12 — quintic or S-curve
Pen misses the marks by a fixed amountServo horn offsets or link lengthsStage 8 calibration
Joints arrive at different timesNot synchronised14 — shared s(t)

Think

Think about it

Think

Your capstone cycle is VALID but 30% too slow for the task. List three changes, in the order you would try them, and say which Stage 9 lesson each comes from.

Experiment

Capstone lab

About 60 minutes with capstone.py and the capstone lab; the pen-arm run takes 30 more minutes.

Experiment

Capstone lab

You will need

  • `capstone.py` and Python 3
  • Trajectory capstone lab (in your browser)
  • Optional: 2-servo pen arm, separate 5–6 V 2 A supply, button, sketch pen, chart paper

Steps

  1. Run `capstone.py`. Copy the VALID report into your notebook.
  2. Make segment 2 a Cartesian line instead of a joint move. Is it still VALID? What changes in T?
  3. Lower AMAX to 150. Recompute the cycle time.
  4. In the capstone lab, plan a reachable target with joint-space and Quintic. Generate, read the checks, play.
  5. Now make it fail on purpose: target outside the reach, then T too small. Read the reasons.
  6. Optional: run `capstone_pen_arm.ino`, measure the pen marks and the cycle time.

Interactive · Trajectory capstone (plan & execute · educational 2R)

Workflow: inspect start → set target EE → IK (Stage 8 ik2R) → choose joint-space or Cartesian → Linear/Cubic/Quintic → generate → check vel/acc limits → PVA graphs → play sim → final pose error → Valid/Invalid. Simplified planar FK/IK stand-in for a 6DOF position chain — not industrial analytic 6DOF or hardware streaming (Stage 10 ROS later).

INVALID / pending — see results

Results panel

Start EE
119.6, 91.4
Target EE
80, 90
IK target
ok
Goal joints
13.1°, 83.2°
Peak |q̇|
32.4 °/s
Peak |q̈|
39.8 °/s²
Final pose err
0.000
Live EE
119.6, 91.4
Verdict
Invalid
  • · Generate a trajectory first

A trajectory is only as good as its checks. The same pipeline — poses, motion type, timing, T, synchronisation, checks, execution — works for the 2-link arm and for a 6DOF industrial arm.

Knowledge check

Check your understanding

Knowledge check

1. What does a trajectory add to a set of IK poses?

2. For moving the gripper straight down onto a part, use…

3. For a long transfer in free space, a good choice is…

4. Before calling a plan VALID you should check…

5. Doubling T for the same shape changes peak joint speed by…

6. Why did the line towards (19, 5) fail?

7. This capstone’s arm model is…

Mini assignment

Capstone project

Mini assignment

Capstone project: your own pick-and-place

Design, check and run a complete trajectory for a task of your choice: for example placing three sweets in a box, drawing your initials, or sorting coloured blocks.

Deliverables

  • Task description and a labelled sketch of all poses (in cm).
  • Table: segment, motion type (joint or Cartesian), time scaling, T, reason for each choice.
  • The VALID report from your modified `capstone.py`, plus one INVALID case you fixed and how.
  • Cycle time, and one improvement that cuts it by at least 10% (with the new report).
  • Optional: a video or photo of the pen arm running your plan, with measured errors at each mark.
  • One paragraph: what would change for a real 6DOF arm (orientation, wrist joints, collisions)?

Do this, step by step

  1. Run python3 capstone.py and read the Part 1 report for home → pick → place → home.
  2. Read Part 2 and write which check failed for each of the two INVALID moves (reach for the far point, joint 1 limit for behind the base).
  3. Replace the poses with your own task (in cm, inside the reach) and run until the report says VALID.
  4. Change one segment’s time and show the new cycle time and peaks; aim for at least 10% faster.
  5. Optional bench: run capstone_pen_arm.ino and measure the error at each mark on the paper.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. Use joint moves for big free moves and Cartesian lines only where the tool must go straight (approach and retreat).
  2. Every sample must pass reach, joint limits, speed and acceleration — not just the final pose.
  3. If a line fails at reach, move the point closer or split the move with a waypoint.

Challenges (pick by level)

  • 🟢 Easy: Write the 8 pipeline steps from memory with one example each.
  • 🟢 Easy: For your task, mark which segments must be Cartesian and why.
  • 🟡 Medium: Add a jerk check to capstone.py (Lesson 12) and report peak jerk per segment.
  • 🟠 Hard-ish: Add a circle obstacle (Lesson 2) and refuse any segment where a link touches it.
  • 🔴 Hard: Replace the stops at pick and place with via points so the arm flows through them (Lesson 5), and compare cycle times.
  • ⚫ Research: Read *Modern Robotics* §9.5 Summary and list which parts of this pipeline it covers and which belong to motion planning (Chapter 10).

Next step

🎉 You have finished Stage 9 — Trajectory Planning. Next, carry this planning pipeline into the following stage, where trajectories meet real controllers and software.

Look back: Lesson 14: Multi-Joint Synchronization. Next: Stage 10 L01: Why Robots Need ROS 2. Related: Stage 13 — Perception, Planning & Manipulation.

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