Stage 12 · Lesson 16/16

Robot Simulation & Digital Twin · Lesson 16

Capstone — Build & Test a Simulated 6DOF Robot

Est. 90–120 min

Why

Why this capstone matters

For fifteen lessons you built the pieces one at a time: why simulate, digital twins, URDF links and joints, worlds and frames, physics, mass and inertia, collisions, friction, sensors, controllers, the ROS 2 bridge, joint commands, testing and a full twin. This capstone joins all of them into one working simulated 6DOF desk arm that does a real job — pick a cup and drop it in a bin — and proves with tests that it is safe to try on hardware.

By the end you will have: a 6-joint model with limits, inverse kinematics with a reachability check, smooth quintic trajectories, a PD controller with gravity compensation running on a 1 kHz physics loop with noisy encoders, collision checks against the table and a tiffin box, a pass/fail test suite with a report card, and a bench gate on the Arduino that keeps the real servo locked until every simulated test passes.

Concept

One pipeline: model → plan → move → check

1. The capstone pipeline

Every serious robot cell follows the same order of steps, whatever the software. Read it like a recipe: each step must succeed before the next one runs. If one fails, the robot does not move.

The capstone stack. Python here; in a full setup the same steps run in ROS 2 with Gazebo, ros2_control and MoveIt.
StepWhat happensLesson it comes fromWhat can fail
1. Model6 joints, link lengths, limits, speeds, efforts, massesL03–L04, L07lower ≥ upper, zero inertia, wrong units
2. Worldtable at z = 0, a cup, a bin, a tiffin box obstacleL05frames in the wrong place
3. Plan (IK)target (x, y, z) → 6 joint angles, tool pointing downStage 8 IK, L03target out of reach
4. Limitsevery joint angle inside its URDF-style limitsL09, L13joint past its stop
5. Trajectoryquintic path, slowest joint sets the timeStage 9, L13too fast for a joint
6. ControlPD + gravity feed-forward, effort clampL11, Stage 7torque saturates, droop
7. Physics + sensors1 kHz semi-implicit Euler, noisy 12-bit-style encodersL06, L10step too big, noise too high
8. Collision checktip and wrist above the table, outside the tiffin boxL08touches an object
9. Feedback checkreal tip vs goal within 5 mm at every waypointL14, L15misses the cup
10. Report cardall tests PASS → bench run allowedL14any FAIL → fix the model first
A full-size six-wheeled Mars rover model with a mast and robotic arm parked on reddish sand and rocks in an outdoor test yard
OPTIMISM, the full-scale engineering model of NASA’s Perseverance rover, in the Mars Yard at JPL (2020). Engineers try commands on this Earth twin before sending them to the real rover on Mars — the same “twin first” rule as your capstone.Photo: NASA/JPL-Caltech · Public domain · Source: Wikimedia Commons · resized, converted to WebP

2. Your simulated 6DOF desk arm

The capstone arm is a classic desk arm: base yaw turns the whole arm, shoulder pitch and elbow pitch lift and reach, wrist pitch keeps the gripper pointing straight down, wrist roll turns the gripper, and the gripper opens and closes. Example sizes: base height 10 cm, upper arm 20 cm, forearm 15 cm, tool 6 cm. These are teaching numbers — measure your own arm and put its values in.

  • IK trick: with the tool pointing down, the wrist point is just above the target. Base yaw = atan2(y, x). Shoulder and elbow are a 2-link planar problem (Stage 8). Wrist pitch = −90° − shoulder − elbow, so the tool stays vertical.
  • Reachability first: if the wrist point is farther than upper arm + forearm (35 cm here), there is no answer. The code says “unreachable” and the arm does not move — it never guesses.
  • Quintic timing: a quintic move has a peak speed of 1.875 × distance ÷ time. So time = 1.875 × distance ÷ max speed, for the slowest joint. All joints share that time and finish together.
  • Gravity feed-forward: the controller adds the torque gravity needs at the current pose. If the model thinks the load is 50 g but it is really 600 g, the arm droops — exactly Rohan’s problem.

3. Tests that must FAIL

A good test suite does not only check that the happy path works. It checks that the twin refuses bad jobs: a cup out of reach, a target inside an obstacle, a load heavier than the model thinks. When the twin says FAIL for these, the test passes. A twin that always says PASS is useless — it will also say PASS on the day it is wrong.

An engineer holds up a white ball-shaped prototype sample canister while people photograph it; behind him is a robotic arm and a small rover model on a simulated Mars terrain
NASA JPL’s Mars Sample Return Sample Transfer Testbed (2020): a robot arm, a rover model and simulated Martian ground, built to rehearse a pick-and-place job long before it is done for real.Photo: NASAJPL · Public domain · Source: Wikimedia Commons · resized, converted to WebP

Visual

Visualise it: the capstone twin, step by step

The capstone twin: step Model → World → Physics → Sensors → ROS → Vision → IK → Trajectory → Controller → Motion → Feedback and get SUCCESS or FAILED. (Educational browser simulation, 2-link stand-in, not an industrial tool.)

Interactive · Simulation digital twin capstone · Educational approximation

Digital twin stack
ModelWorldPhysicsSensorsROSVisionIKTrajCtrlMotionFeedback

Ready — step Model → … → Feedback. Educational twin only.

Pipeline: model → world → physics → sensors → ROS → vision → IK → traj → controller → motion → feedback. Reuses Stage 8 ik2R + Stage 9 quintic helpers. Limitations: planar 2R stand-in (not full 6DOF), no Gazebo/Isaac/MuJoCo/real ROS, not industrial pick accuracy.

Code lab

Code lab: the complete simulated 6DOF arm and its test suite

capstone_6dof_sim.py is the whole capstone in one file — plain Python 3, no libraries. It builds the model, solves IK, checks limits, plans quintic moves, runs a PD + gravity controller on a 1 kHz physics loop with noisy encoders, checks collisions with the table and a tiffin box, and runs six groups of tests that end in a report card. All values are examples. Run python3 capstone_6dof_sim.py (about 4 seconds).

capstone_6dof_sim.py · Python
"""capstone_6dof_sim.py — Stage 12 capstone: build and test a simulated 6DOF desk arm.

Plain Python 3, no libraries. Run:  python3 capstone_6dof_sim.py
Everything here is a TEACHING model with EXAMPLE numbers (lengths, masses,
limits, gains). It is not Gazebo, MuJoCo or a real robot — but it walks the
same path: model -> IK -> limits -> trajectory -> controller -> physics ->
sensors -> collision check -> pass/fail tests -> report card.
"""
import math
import random

G = 9.81
# ---------------------------------------------------------------- 1. MODEL
# joint: (name, lower rad, upper rad, max speed rad/s, effort N·m, inertia kg·m²)
JOINTS = [
    ("base_yaw",       -2.60, 2.60, 1.5, 1.0, 0.004),
    ("shoulder_pitch", -0.20, 2.20, 1.2, 1.2, 0.010),
    ("elbow_pitch",    -2.50, 0.20, 1.5, 0.8, 0.004),
    ("wrist_pitch",    -2.40, 2.40, 2.0, 0.3, 0.001),
    ("wrist_roll",     -3.00, 3.00, 2.0, 0.3, 0.001),
    ("gripper",         0.00, 1.00, 2.0, 0.3, 0.0005),
]
BASE_H, L1, L2, TOOL = 0.10, 0.20, 0.15, 0.06      # metres
M1, M2, M_TOOL = 0.15, 0.10, 0.05                   # link masses, kg
TABLE_Z, SAFE_GAP = 0.0, 0.005                      # tip must stay 5 mm above table
TIFFIN = (0.12, 0.20, -0.06, 0.06, 0.0, 0.08)       # obstacle box x0,x1,y0,y1,z0,z1
N = len(JOINTS)


def check_model():
    problems = []
    for name, lo, hi, v, e, inertia in JOINTS:
        if lo >= hi: problems.append(f"{name}: lower >= upper")
        if v <= 0 or e <= 0: problems.append(f"{name}: speed/effort must be > 0")
        if inertia <= 0: problems.append(f"{name}: inertia must be > 0")
    return problems


# ------------------------------------------------------- 2. FK and IK
def fk(q):
    """Tip position (x, y, z) and elbow point for joint angles q (radians)."""
    yaw, s, e, w = q[0], q[1], q[2], q[3]
    r1, z1 = L1 * math.cos(s), BASE_H + L1 * math.sin(s)
    r2, z2 = r1 + L2 * math.cos(s + e), z1 + L2 * math.sin(s + e)
    r3, z3 = r2 + TOOL * math.cos(s + e + w), z2 + TOOL * math.sin(s + e + w)
    c, sn = math.cos(yaw), math.sin(yaw)
    return (r3 * c, r3 * sn, z3), (r1 * c, r1 * sn, z1), (r2 * c, r2 * sn, z2)


def ik(x, y, z, roll=0.0, grip=0.0):
    """Tool pointing straight down. Returns 6 joint angles or raises ValueError."""
    yaw = math.atan2(y, x)
    r = math.hypot(x, y)
    wr, wz = r, z + TOOL - BASE_H          # wrist point (tool hangs down)
    d2 = wr * wr + wz * wz
    c = (d2 - L1 * L1 - L2 * L2) / (2 * L1 * L2)
    if c > 1.0 or c < -1.0:
        raise ValueError(f"unreachable: wrist {math.sqrt(d2):.3f} m from shoulder, max {L1 + L2:.3f}")
    e = -math.acos(c)                       # elbow-up solution
    s = math.atan2(wz, wr) - math.atan2(L2 * math.sin(e), L1 + L2 * math.cos(e))
    w = -math.pi / 2 - s - e
    return [yaw, s, e, w, roll, grip]


def limit_violations(q):
    return [f"{JOINTS[i][0]} {q[i]:+.2f} outside [{JOINTS[i][1]:+.2f}, {JOINTS[i][2]:+.2f}]"
            for i in range(N) if not (JOINTS[i][1] <= q[i] <= JOINTS[i][2])]


# ------------------------------------------------- 3. TRAJECTORY (quintic)
def quintic(a, b, T, t):
    u = min(max(t / T, 0.0), 1.0)
    return a + (b - a) * (10 * u**3 - 15 * u**4 + 6 * u**5)


def segment_time(qa, qb):
    """Slowest joint decides. Quintic peak speed = 1.875 * distance / T."""
    return max(0.3, max(1.875 * abs(qb[i] - qa[i]) / JOINTS[i][3] for i in range(N)))


# --------------------------------------- 4. PHYSICS + SENSORS + CONTROLLER
def gravity_torque(q, payload):
    """Gravity torque on shoulder and elbow (planar, tool mass + payload at tip)."""
    s, e = q[1], q[2]
    tip_r = L1 * math.cos(s) + L2 * math.cos(s + e)
    t_sh = G * (M1 * L1 / 2 * math.cos(s) + M2 * (L1 * math.cos(s) + L2 / 2 * math.cos(s + e))
                + (M_TOOL + payload) * tip_r)
    t_el = G * (M2 * L2 / 2 * math.cos(s + e) + (M_TOOL + payload) * L2 * math.cos(s + e))
    return t_sh, t_el


class SimArm:
    def __init__(self, q0, rng, payload=0.0, friction=0.02, noise=0.002, model_payload=0.0):
        self.q, self.dq = list(q0), [0.0] * N
        self.rng, self.payload, self.friction, self.noise = rng, payload, friction, noise
        self.model_payload = model_payload
        self.kp, self.kd = [8, 30, 20, 4, 4, 2], [0.3, 1.0, 0.6, 0.08, 0.08, 0.04]
        self.peak_effort = [0.0] * N
        self.saturated = [0] * N

    def read_encoders(self):            # truth + noise, 12-bit style 0.0015 rad steps
        return [round((qi + self.rng.gauss(0, self.noise)) / 0.0015) * 0.0015 for qi in self.q]

    def step(self, q_ref, dq_ref, ctrl_dt=0.005, phys_dt=0.001):
        meas = self.read_encoders()
        g_model = gravity_torque(meas, self.model_payload)
        tau = []
        for i in range(N):
            ff = g_model[0] if i == 1 else g_model[1] if i == 2 else 0.0
            t = self.kp[i] * (q_ref[i] - meas[i]) + self.kd[i] * (dq_ref[i] - self.dq[i]) + ff
            lim = JOINTS[i][4]
            if abs(t) > lim:
                self.saturated[i] += 1
                t = math.copysign(lim, t)
            self.peak_effort[i] = max(self.peak_effort[i], abs(t))
            tau.append(t)
        for _ in range(int(round(ctrl_dt / phys_dt))):   # 1 kHz physics, semi-implicit Euler
            g_true = gravity_torque(self.q, self.payload)
            for i in range(N):
                load = g_true[0] if i == 1 else g_true[1] if i == 2 else 0.0
                acc = (tau[i] - load - self.friction * self.dq[i] * 10) / JOINTS[i][5]
                self.dq[i] += acc * phys_dt
                self.q[i] += self.dq[i] * phys_dt
                lo, hi = JOINTS[i][1], JOINTS[i][2]
                if self.q[i] < lo or self.q[i] > hi:     # hard stop
                    self.q[i] = min(max(self.q[i], lo), hi)
                    self.dq[i] = 0.0


def collides(q):
    tip, elbow, wrist = fk(q)
    if tip[2] < TABLE_Z + SAFE_GAP or wrist[2] < TABLE_Z + SAFE_GAP:
        return "tip/wrist below table"
    x0, x1, y0, y1, z0, z1 = TIFFIN
    for p in (tip, wrist):
        if x0 <= p[0] <= x1 and y0 <= p[1] <= y1 and z0 <= p[2] <= z1:
            return "hit the tiffin box"
    return None


# ------------------------------------------------------------ 5. MISSION
CUP = (0.22, 0.12, 0.03)
BIN = (0.18, -0.20, 0.06)
HOME = [0.0, 1.2, -1.6, -1.17, 0.0, 0.0]


def mission(cup=CUP, bin_=BIN):
    up = 0.10
    return [
        ("above cup", (cup[0], cup[1], cup[2] + up), 0.0),
        ("down to cup", cup, 0.0),
        ("close gripper", cup, 0.8),
        ("lift", (cup[0], cup[1], cup[2] + up), 0.8),
        ("above bin", (bin_[0], bin_[1], bin_[2] + up), 0.8),
        ("down to bin", bin_, 0.8),
        ("open gripper", bin_, 0.0),
        ("retreat", (bin_[0], bin_[1], bin_[2] + up), 0.0),
    ]


def run_mission(rng, payload=0.0, friction=0.02, noise=0.002, model_payload=0.0,
                steps=None, tol=0.005, verbose=False):
    steps = steps or mission()
    arm = SimArm(HOME, rng, payload=0.0, friction=friction, noise=noise, model_payload=0.0)
    q_now, worst, sim_t = list(HOME), 0.0, 0.0
    for label, xyz, grip in steps:
        try:
            q_goal = ik(*xyz, grip=grip)
        except ValueError as err:
            return dict(ok=False, why=f"{label}: IK {err}", worst=worst, t=sim_t, arm=arm)
        bad = limit_violations(q_goal)
        if bad:
            return dict(ok=False, why=f"{label}: limits {bad[0]}", worst=worst, t=sim_t, arm=arm)
        if "close" in label:
            arm.payload, arm.model_payload = payload, model_payload
        if "open" in label:
            arm.payload, arm.model_payload = 0.0, 0.0
        T = segment_time(q_now, q_goal)
        t, dt = 0.0, 0.005
        while t < T + 0.4:                       # move, then 0.4 s to settle
            ref = [quintic(q_now[i], q_goal[i], T, t) for i in range(N)]
            ref2 = [quintic(q_now[i], q_goal[i], T, t + dt) for i in range(N)]
            vel = [(ref2[i] - ref[i]) / dt for i in range(N)]
            arm.step(ref, vel)
            hit = collides(arm.q)
            if hit:
                return dict(ok=False, why=f"{label}: COLLISION {hit} at t={sim_t + t:.2f}s",
                            worst=worst, t=sim_t + t, arm=arm)
            t += dt
        sim_t += t
        tip = fk(arm.q)[0]
        err = math.dist(tip, xyz)
        worst = max(worst, err)
        if verbose:
            print(f"  {label:<14} T={T:4.2f}s  tip=({tip[0]:+.3f},{tip[1]:+.3f},{tip[2]:+.3f})"
                  f"  error={err * 1000:5.1f} mm")
        if err > tol:
            return dict(ok=False, why=f"{label}: tip error {err * 1000:.1f} mm > {tol * 1000:.0f} mm",
                        worst=worst, t=sim_t, arm=arm)
        q_now = q_goal
    return dict(ok=True, why="all waypoints within tolerance", worst=worst, t=sim_t, arm=arm)


# ------------------------------------------------------------- 6. TESTS
def main():
    results = []
    print("== Part 1: model check ==")
    p = check_model()
    print("  model OK: 6 joints, limits, speeds, efforts, inertias" if not p else p)
    results.append(("model sanity", not p))

    print("\n== Part 2: IK round-trip on a grid of 60 tool-down targets ==")
    worst_rt, fails = 0.0, 0
    for x in (0.14, 0.18, 0.22, 0.26, 0.30):
        for y in (-0.15, 0.0, 0.15):
            for z in (0.02, 0.06, 0.10, 0.14):
                try:
                    q = ik(x, y, z)
                    tip = fk(q)[0]
                    worst_rt = max(worst_rt, math.dist(tip, (x, y, z)))
                except ValueError:
                    fails += 1
    print(f"  unreachable: {fails}   worst FK(IK(p)) - p = {worst_rt * 1e9:.2f} nm")
    results.append(("IK round-trip", worst_rt < 1e-6))

    print("\n== Part 3: nominal pick-and-place (cup -> bin), seed 16 ==")
    r = run_mission(random.Random(16), payload=0.05, model_payload=0.05, verbose=True)
    arm = r["arm"]
    print(f"  RESULT: {'PASS' if r['ok'] else 'FAIL'} — {r['why']}; mission time {r['t']:.2f} s")
    print("  peak effort used / limit (N·m): " + ", ".join(
        f"{JOINTS[i][0].split('_')[0]} {arm.peak_effort[i]:.2f}/{JOINTS[i][4]:.1f}" for i in range(3)))
    results.append(("nominal mission", r["ok"]))

    print("\n== Part 4: the twin must refuse bad jobs (expected FAIL = test passes) ==")
    far = run_mission(random.Random(1), steps=[("far cup", (0.45, 0.0, 0.03), 0.0)])
    print(f"  cup 45 cm away     -> {'PASS' if far['ok'] else 'FAIL'}: {far['why']}")
    results.append(("refuses unreachable", not far["ok"]))
    tiffin = run_mission(random.Random(1), steps=[("into tiffin", (0.16, 0.0, 0.04), 0.0)])
    print(f"  target inside box  -> {'PASS' if tiffin['ok'] else 'FAIL'}: {tiffin['why']}")
    results.append(("catches collision", not tiffin["ok"]))
    heavy = run_mission(random.Random(1), payload=0.60, model_payload=0.05)
    print(f"  600 g load, model thinks 50 g -> {'PASS' if heavy['ok'] else 'FAIL'}: {heavy['why']}")
    print(f"     shoulder hit its 1.2 N·m limit in {heavy['arm'].saturated[1]} control steps")
    results.append(("catches overload", not heavy["ok"]))

    print("\n== Part 5: robustness — 40 seeds each, sensor noise 0.002 / 0.010 / 0.030 rad ==")
    for noise in (0.002, 0.010, 0.030):
        passed, worst = 0, 0.0
        for seed in range(40):
            rr = run_mission(random.Random(1000 + seed), payload=0.05, model_payload=0.05, noise=noise)
            passed += rr["ok"]
            worst = max(worst, rr["worst"])
        print(f"  noise {noise:.3f} rad: {passed}/40 pass, worst tip error {worst * 1000:.1f} mm")
        if noise == 0.002:
            results.append(("40-seed robustness @0.002", passed == 40))

    print("\n== Part 6: sim-to-real gap — joint damping 0.02 in the twin, real arm 0.02 / 0.05 / 0.10 ==")
    for fr in (0.02, 0.05, 0.10):
        rr = run_mission(random.Random(7), payload=0.05, model_payload=0.05, friction=fr)
        print(f"  damping {fr:.2f}: {'PASS' if rr['ok'] else 'FAIL'}, worst {rr['worst'] * 1000:.1f} mm — {rr['why']}")

    print("\n== REPORT CARD ==")
    for name, ok in results:
        print(f"  [{'PASS' if ok else 'FAIL'}] {name}")
    total = sum(ok for _, ok in results)
    print(f"  {total}/{len(results)} tests passed -> "
          f"{'READY for a slow, supervised bench run' if total == len(results) else 'NOT ready: fix the model first'}")


if __name__ == "__main__":
    main()

Output (run with python3 capstone_6dof_sim.py)

== Part 1: model check ==
  model OK: 6 joints, limits, speeds, efforts, inertias

== Part 2: IK round-trip on a grid of 60 tool-down targets ==
  unreachable: 0   worst FK(IK(p)) - p = 0.00 nm

== Part 3: nominal pick-and-place (cup -> bin), seed 16 ==
  above cup      T=0.62s  tip=(+0.220,+0.120,+0.130)  error=  0.2 mm
  down to cup    T=0.52s  tip=(+0.220,+0.120,+0.030)  error=  0.3 mm
  close gripper  T=0.75s  tip=(+0.220,+0.120,+0.030)  error=  0.3 mm
  lift           T=0.52s  tip=(+0.220,+0.120,+0.130)  error=  0.2 mm
  above bin      T=1.67s  tip=(+0.180,-0.200,+0.160)  error=  0.2 mm
  down to bin    T=0.46s  tip=(+0.180,-0.200,+0.060)  error=  0.2 mm
  open gripper   T=0.75s  tip=(+0.180,-0.200,+0.060)  error=  0.4 mm
  retreat        T=0.46s  tip=(+0.180,-0.200,+0.160)  error=  0.1 mm
  RESULT: PASS — all waypoints within tolerance; mission time 8.99 s
  peak effort used / limit (N·m): base 0.33/1.0, shoulder 0.99/1.2, elbow 0.41/0.8

== Part 4: the twin must refuse bad jobs (expected FAIL = test passes) ==
  cup 45 cm away     -> FAIL: far cup: IK unreachable: wrist 0.450 m from shoulder, max 0.350
  target inside box  -> FAIL: into tiffin: COLLISION hit the tiffin box at t=0.53s
  600 g load, model thinks 50 g -> FAIL: close gripper: COLLISION tip/wrist below table at t=2.01s
     shoulder hit its 1.2 N·m limit in 11 control steps

== Part 5: robustness — 40 seeds each, sensor noise 0.002 / 0.010 / 0.030 rad ==
  noise 0.002 rad: 40/40 pass, worst tip error 0.6 mm
  noise 0.010 rad: 40/40 pass, worst tip error 2.5 mm
  noise 0.030 rad: 7/40 pass, worst tip error 7.9 mm

== Part 6: sim-to-real gap — joint damping 0.02 in the twin, real arm 0.02 / 0.05 / 0.10 ==
  damping 0.02: PASS, worst 0.4 mm — all waypoints within tolerance
  damping 0.05: PASS, worst 2.8 mm — all waypoints within tolerance
  damping 0.10: FAIL, worst 13.6 mm — down to cup: tip error 13.6 mm > 5 mm

== REPORT CARD ==
  [PASS] model sanity
  [PASS] IK round-trip
  [PASS] nominal mission
  [PASS] refuses unreachable
  [PASS] catches collision
  [PASS] catches overload
  [PASS] 40-seed robustness @0.002
  7/7 tests passed -> READY for a slow, supervised bench run

Try it: step the capstone twin through Model → World → Physics → Sensors → ROS → Vision → IK → Trajectory → Controller → Motion → Feedback and read SUCCESS or FAILED. Raise the noise and try again. (Educational browser simulation with a 2-link stand-in for the 6DOF arm — not Gazebo, Isaac, MuJoCo or real ROS.)

Interactive · Simulation digital twin capstone · Educational approximation

Digital twin stack
ModelWorldPhysicsSensorsROSVisionIKTrajCtrlMotionFeedback

Ready — step Model → … → Feedback. Educational twin only.

Pipeline: model → world → physics → sensors → ROS → vision → IK → traj → controller → motion → feedback. Reuses Stage 8 ik2R + Stage 9 quintic helpers. Limitations: planar 2R stand-in (not full 6DOF), no Gazebo/Isaac/MuJoCo/real ROS, not industrial pick accuracy.

Bench build: the “tests first” gate on the UNO

sim_gate_servo.ino is the last safety link between the twin and the metal. The servo stays detached (no signal) until the PC sends the report card line PASS 7/7. Then it accepts J <degrees> commands, but only inside the twin’s joint limits (20–160° here), and moves at most 30°/s. If no message arrives for 2 s it holds still (a watchdog), and a normally-closed E-stop button latches a STOP that needs both release and an R command to clear.

First run: servo horn off or the joint clear of everything, supply current-limited if you have a bench supply, one hand on the E-stop.
PartConnectionNote
E-stop (normally-closed contact)D2 ↔ GND, D2 uses INPUT_PULLUPpressed or broken wire → D2 HIGH → STOP (fail-safe)
Hobby servo signalD9one joint of your desk arm, or a loose servo with a pointer
Servo powerseparate 5–6 V supply (+) to servo red, (−) to servo brown/blackjoin supply (−) to UNO GND; never power the servo from a GPIO or the UNO 5V pin
Green LED + 220 ΩD5 → resistor → LED → GNDREADY (unlocked)
Red LED + 220 ΩD6 → resistor → LED → GNDLOCKED or E-STOP
sim_gate_servo.ino · Arduino C++
// sim_gate_servo.ino — Stage 12 capstone bench gate (Arduino UNO)
// The real servo may move ONLY after the simulated tests pass.
//   PC sends  "PASS 7/7"  -> unlocks (n must equal m)
//   PC sends  "J 90"      -> move the servo to 90° slowly (30°/s), inside the twin's limits
//   "L" locks again.  E-stop latches until released AND you send "R".
// Wiring:
//   E-stop: normally-closed (NC) contact between D2 and GND. Pressed (or a broken wire)
//           opens the circuit -> D2 reads HIGH -> STOP. This is the fail-safe way.
//   Servo signal on D9. Servo power from a SEPARATE 5–6 V supply, grounds joined.
//   Never power a servo from a GPIO pin or from the UNO's 5V pin under load.
//   Green LED + 220 Ω on D5 (READY), red LED + 220 Ω on D6 (LOCKED / E-STOP).
#include <Servo.h>

const int ESTOP_PIN = 2, SERVO_PIN = 9, LED_READY = 5, LED_LOCK = 6;
const float LIMIT_LO = 20.0, LIMIT_HI = 160.0;   // copy these from your twin's joint limits
const float MAX_SPEED = 30.0;                    // degrees per second, slow for first bench run
const unsigned long WATCHDOG_MS = 2000;          // no message for 2 s -> hold still

Servo joint;
bool unlocked = false, estopLatched = false;
float pos = 90.0, target = 90.0;
unsigned long lastMsg = 0, lastStep = 0;
char buf[24];
byte len = 0;

void lock(const char *why) {
  unlocked = false;
  target = pos;
  if (joint.attached()) joint.detach();          // no signal -> most hobby servos go limp
  Serial.print(F("LOCKED: "));
  Serial.println(why);
}

void handle(char *line) {
  lastMsg = millis();
  if (strncmp(line, "PASS ", 5) == 0) {
    int n = 0, m = 0;
    if (sscanf(line + 5, "%d/%d", &n, &m) == 2 && m > 0 && n == m && !estopLatched) {
      unlocked = true;
      joint.attach(SERVO_PIN);
      joint.write((int)pos);
      Serial.println(F("UNLOCKED: all simulated tests passed"));
    } else {
      lock("tests not all passed (or E-stop latched)");
    }
  } else if (line[0] == 'J' && line[1] == ' ') {
    float want = atof(line + 2);
    if (!unlocked) { Serial.println(F("REFUSED: locked — run the sim tests first")); return; }
    if (want < LIMIT_LO || want > LIMIT_HI) {
      Serial.print(F("REFUSED: ")); Serial.print(want); Serial.println(F(" outside twin limits"));
      return;
    }
    target = want;
    Serial.print(F("OK target ")); Serial.println(target);
  } else if (line[0] == 'L') {
    lock("by command");
  } else if (line[0] == 'R') {
    if (digitalRead(ESTOP_PIN) == HIGH) { Serial.println(F("REFUSED: E-stop still pressed")); return; }
    estopLatched = false;
    Serial.println(F("E-stop reset. Send PASS n/n again to unlock."));
  } else if (line[0] != 0) {
    Serial.println(F("? commands: PASS n/m, J deg, L, R"));
  }
}

void setup() {
  pinMode(ESTOP_PIN, INPUT_PULLUP);
  pinMode(LED_READY, OUTPUT);
  pinMode(LED_LOCK, OUTPUT);
  Serial.begin(115200);
  Serial.println(F("sim_gate_servo: locked. Send PASS n/m from the sim report card."));
}

void loop() {
  if (digitalRead(ESTOP_PIN) == HIGH && !estopLatched) {
    estopLatched = true;
    lock("E-STOP");
  }
  while (Serial.available()) {
    char c = Serial.read();
    if (c == '\n' || c == '\r') { buf[len] = 0; if (len) handle(buf); len = 0; }
    else if (len < sizeof(buf) - 1) buf[len++] = c;
  }
  if (unlocked && millis() - lastMsg > WATCHDOG_MS && target != pos) {
    target = pos;
    Serial.println(F("WATCHDOG: no message for 2 s, holding"));
  }
  if (millis() - lastStep >= 20) {               // 50 Hz motion update
    lastStep = millis();
    if (unlocked) {
      float stepMax = MAX_SPEED * 0.02;
      float d = target - pos;
      if (d > stepMax) d = stepMax;
      if (d < -stepMax) d = -stepMax;
      pos += d;
      joint.write((int)(pos + 0.5));
    }
  }
  digitalWrite(LED_READY, unlocked ? HIGH : LOW);
  digitalWrite(LED_LOCK, unlocked ? LOW : HIGH);
}
Serial Monitor (115200, Newline) — example session · Text
sim_gate_servo: locked. Send PASS n/m from the sim report card.
> J 120
REFUSED: locked — run the sim tests first
> PASS 6/7
LOCKED: tests not all passed (or E-stop latched)
> PASS 7/7
UNLOCKED: all simulated tests passed
> J 175
REFUSED: 175.00 outside twin limits
> J 120
OK target 120.00
(E-stop pressed)
LOCKED: E-STOP
> R
REFUSED: E-stop still pressed
(E-stop released)
> R
E-stop reset. Send PASS n/n again to unlock.
Close-up of a large red mushroom-head emergency stop button on a yellow background on the control panel of a CNC machine
Emergency stop on a HAAS CNC machine. Wikipedia (Kill switch) notes industrial E-stops are usually normally-closed so a broken wire still stops the machine, and are a complementary measure — not a replacement for guards and safe design.Photo: Flygklubben · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Debugging the capstone

SymptomLikely causeFix
IK says unreachable for a target that looks closetarget z too low, tool length forgotten, or base height wrongPrint the wrist point; check BASE_H and TOOL match your arm
Every waypoint misses by the same few mmgravity model or payload wrongSet the real payload; check masses; add a small integral term with anti-windup (L11)
Tip error grows only after “close gripper”payload not switched on in the modelChange model_payload when the gripper closes and opens
Random seeds sometimes FAILencoder noise too large for the 5 mm toleranceFilter the reading, use a better encoder, or relax tolerance honestly
COLLISION on a move between two safe pointsstraight joint-space path dips down through the tableAdd an “above” waypoint, or plan in Cartesian space
Peak shoulder effort close to the limitlong reach + heavy loadLighter tool, shorter reach, slower moves, or a stronger servo
Bench: servo jitters or the UNO resetsservo powered from the UNOSeparate 5–6 V supply, common ground, and a capacitor across the servo supply
Bench: always LOCKEDE-stop wired normally-open, or latchedUse the NC contact to GND; release, send R, then PASS 7/7
Labelled product diagram of a collaborative robot arm with a vacuum gripper mounted on a mobile steel frame, next to a box conveyor, a pallet and an operator control cabinet
A collaborative palletising cell: robot, gripper (end-of-arm tool), conveyor, pallet and operator panel. Your capstone is a tiny version of the same job — pick from a known place, place in a known place, with every step checked.Photo: CollaborativePalletizer · CC BY-SA 4.0 · Source: Wikimedia Commons · resized, converted to WebP

Real world

Where engineers do exactly this

  • Space robotics: NASA JPL keeps OPTIMISM, an Earth twin of Perseverance, and testbeds such as the Mars Sample Return Sample Transfer Testbed to rehearse moves before they are done on Mars.
  • Automotive and aerospace: Wikipedia (Hardware-in-the-loop simulation) describes engine and brake controllers tested against plant simulations so failure cases can be tried safely and repeatably.
  • Factories: palletising and pick-and-place cells are planned and checked in simulation, then commissioned slowly on the floor with E-stops and guards in place.
  • ROS 2 world: Gazebo’s ROS 2 integration uses ros_gz_bridge to pass joint states and commands between ROS and the simulator, so the same controller code can drive the simulated arm and, later, the real one.
  • Your Kota lab: the capstone twin of your desk arm — tested 7/7 — becomes the gate every new program must pass before it touches the real arm.

Think

Think about it

Think

Your report card says 7/7 PASS, but on the real arm the gripper misses the cup by 1 cm every time, always to the same side. The twin says the error is 0.3 mm. What do you suspect, and what do you do?

Experiment

Capstone lab: build, test, then (and only then) move metal

About 90–120 minutes. Python 3 for the simulation; an Arduino UNO, one hobby servo with its own 5–6 V supply, a normally-closed push button as E-stop, and two LEDs with 220 Ω resistors for the bench gate.

Experiment

Capstone lab: build, test, break, fix, then move metal

You will need

  • `capstone_6dof_sim.py` (Python 3, no libraries)
  • Capstone twin interactive (in your browser)
  • Arduino UNO, hobby servo, separate 5–6 V servo supply, NC push button, green and red LEDs, two 220 Ω resistors, jumper wires
  • Ruler or tape to measure your own desk arm

Steps

  1. Run `capstone_6dof_sim.py` as it is. Copy the report card into your notebook.
  2. Measure your own arm (base height, upper arm, forearm, tool) and put the numbers into BASE_H, L1, L2, TOOL. Re-run. Which targets become unreachable?
  3. Move the cup to (0.30, 0.15, 0.03). Does the shoulder effort get closer to its limit? By how much?
  4. Make the payload 0.20 kg but leave model_payload at 0.05. Which test fails, and how? Then set model_payload to 0.20 and re-run.
  5. Add a new test: a target on the far side of the tiffin box, reached via an “above” waypoint. Prove it passes without collision.
  6. Change the tolerance from 5 mm to 2 mm and re-run Part 5. How many seeds pass now at 0.010 rad noise?
  7. Interactive: run the capstone twin with low noise, then high noise. Note when it says FAILED and why.
  8. Bench: wire the gate. Try `J 90` while locked, then `PASS 6/7`, then `PASS 7/7`, then a move outside the limits, then the E-stop and reset.
  9. Only after all of this: attach the servo to one joint of your arm, no load, and make one slow supervised move.

Interactive · Simulation digital twin capstone · Educational approximation

Digital twin stack
ModelWorldPhysicsSensorsROSVisionIKTrajCtrlMotionFeedback

Ready — step Model → … → Feedback. Educational twin only.

Pipeline: model → world → physics → sensors → ROS → vision → IK → traj → controller → motion → feedback. Reuses Stage 8 ik2R + Stage 9 quintic helpers. Limitations: planar 2R stand-in (not full 6DOF), no Gazebo/Isaac/MuJoCo/real ROS, not industrial pick accuracy.

A capstone is not “the robot moved once”. It is a pipeline where every step can say no, a test suite that proves the twin says no when it should, and a bench gate that keeps the real arm still until the tests pass.

Knowledge check

Check your understanding

Knowledge check

1. Why run the capstone in simulation before the real arm?

2. IK says the wrist point is 0.45 m from the shoulder; upper arm + forearm = 0.35 m. The correct action is…

3. With the tool pointing straight down, wrist pitch is…

4. A joint must move 1.2 rad with max speed 1.5 rad/s using a quintic. Minimum time?

5. In the script, a 600 g load the model thinks is 50 g causes…

6. The twin says FAIL for a target inside the tiffin box. The test result is…

7. At encoder noise 0.030 rad only 7/40 seeds pass. The honest fix is…

8. The twin assumes damping 0.02; the real arm has 0.10 and misses by 13.6 mm. This is…

9. Why is the E-stop wired as a normally-closed contact to GND?

10. How should the servo be powered on the bench?

11. The gate receives no message for 2 s during a move. It…

12. Stage 12 interactives (including this capstone) are…

Mini assignment

Capstone assignment

Mini assignment

Capstone: your simulated 6DOF desk arm, tested and gated

Build the complete simulated twin of your own 6DOF desk arm, prove it with tests, and use the bench gate for one slow, supervised real move.

Deliverables

  • Your edited `capstone_6dof_sim.py` with your arm’s measured lengths, limits and a mission on your own desk layout (cup, bin and one obstacle).
  • A report card with at least 9 tests: the 7 given plus 2 of your own (one that must PASS, one that must FAIL).
  • A robustness table: pass rate and worst error for 3 noise levels and 3 damping values, with one sentence on what it means for your arm.
  • A wiring photo or diagram of the bench gate (NC E-stop, separate servo supply, common ground) and a Serial Monitor log showing locked → refused → unlocked → limit refusal → E-stop → reset.
  • A one-page evidence card: pipeline diagram, which Stage 7–12 lesson each step came from, and three honest limitations (simplified physics, example numbers, not industrial accuracy).

Do this, step by step

  1. Put your arm’s measured lengths and limits into JOINTS in capstone_6dof_sim.py.
  2. Move CUP, BIN and the obstacle box to your own desk layout and run the mission.
  3. Add 2 tests of your own: one that must PASS and one that must FAIL.
  4. Fill the robustness table: 3 noise levels × 3 damping values.
  5. Bench: wire the gate (NC E-stop, separate servo supply, common ground), upload sim_gate_servo.ino and log locked → refused → unlocked → limit refusal → E-stop → reset.

Stuck? Hints, one at a time

NoteTry the task first. Read one hint, try again, and only then read the next one.
  1. Keep check_model() passing after each edit. Fix model errors before looking at mission results.
  2. If the path hits the obstacle, raise the travel height or move the bin before changing gains.
  3. The E-stop is normally closed, so a broken wire must also stop the arm. Test that by unplugging it.

Challenges (pick by level)

  • 🟢 Beginner: Change the cup and bin positions three times and record which missions pass and why the others fail.
  • 🟡 Intermediate: Add a small integral term with anti-windup (Lesson 11) to the shoulder and show the payload error shrinking in Part 3.
  • 🟠 Advanced: Write a Python script that runs the test suite and, only if every test passes, sends PASS n/n and a slow list of J commands (with heartbeats) to sim_gate_servo.ino over pySerial.
  • 🔴 Expert: Recreate the arm as a URDF with <inertial> and <limit> tags, load it in Gazebo with gz_ros2_control, bridge /joint_states with ros_gz_bridge, and run the same mission and tests from an rclpy node.
  • ⚫ Research: Read about hardware-in-the-loop testing and design a HIL rig for your arm: which signals the PC fakes, which the real board produces, and how you would test an encoder failure safely.

Next step

Congratulations — you have finished Stage 12: Robot Simulation & Digital Twin. Next, Stage 13 brings perception, planning and manipulation together: the arm will see the cup with a camera, plan around obstacles and pick it up on its own. Keep your capstone twin — every new skill gets tested in it first.

Look back: Lesson 15: Building a Complete Digital Twin. Next: Stage 13 — Perception, Planning & Manipulation. 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