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.
| Step | What happens | Lesson it comes from | What can fail |
|---|---|---|---|
| 1. Model | 6 joints, link lengths, limits, speeds, efforts, masses | L03–L04, L07 | lower ≥ upper, zero inertia, wrong units |
| 2. World | table at z = 0, a cup, a bin, a tiffin box obstacle | L05 | frames in the wrong place |
| 3. Plan (IK) | target (x, y, z) → 6 joint angles, tool pointing down | Stage 8 IK, L03 | target out of reach |
| 4. Limits | every joint angle inside its URDF-style limits | L09, L13 | joint past its stop |
| 5. Trajectory | quintic path, slowest joint sets the time | Stage 9, L13 | too fast for a joint |
| 6. Control | PD + gravity feed-forward, effort clamp | L11, Stage 7 | torque saturates, droop |
| 7. Physics + sensors | 1 kHz semi-implicit Euler, noisy 12-bit-style encoders | L06, L10 | step too big, noise too high |
| 8. Collision check | tip and wrist above the table, outside the tiffin box | L08 | touches an object |
| 9. Feedback check | real tip vs goal within 5 mm at every waypoint | L14, L15 | misses the cup |
| 10. Report card | all tests PASS → bench run allowed | L14 | any FAIL → fix the model first |

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.

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
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 — 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 runTry 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
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.
| Part | Connection | Note |
|---|---|---|
| E-stop (normally-closed contact) | D2 ↔ GND, D2 uses INPUT_PULLUP | pressed or broken wire → D2 HIGH → STOP (fail-safe) |
| Hobby servo signal | D9 | one joint of your desk arm, or a loose servo with a pointer |
| Servo power | separate 5–6 V supply (+) to servo red, (−) to servo brown/black | join supply (−) to UNO GND; never power the servo from a GPIO or the UNO 5V pin |
| Green LED + 220 Ω | D5 → resistor → LED → GND | READY (unlocked) |
| Red LED + 220 Ω | D6 → resistor → LED → GND | LOCKED or E-STOP |
// 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);
}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.
Debugging the capstone
| Symptom | Likely cause | Fix |
|---|---|---|
| IK says unreachable for a target that looks close | target z too low, tool length forgotten, or base height wrong | Print the wrist point; check BASE_H and TOOL match your arm |
| Every waypoint misses by the same few mm | gravity model or payload wrong | Set 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 model | Change model_payload when the gripper closes and opens |
| Random seeds sometimes FAIL | encoder noise too large for the 5 mm tolerance | Filter the reading, use a better encoder, or relax tolerance honestly |
| COLLISION on a move between two safe points | straight joint-space path dips down through the table | Add an “above” waypoint, or plan in Cartesian space |
| Peak shoulder effort close to the limit | long reach + heavy load | Lighter tool, shorter reach, slower moves, or a stronger servo |
| Bench: servo jitters or the UNO resets | servo powered from the UNO | Separate 5–6 V supply, common ground, and a capacitor across the servo supply |
| Bench: always LOCKED | E-stop wired normally-open, or latched | Use the NC contact to GND; release, send R, then PASS 7/7 |

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_bridgeto 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
- Run `capstone_6dof_sim.py` as it is. Copy the report card into your notebook.
- 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?
- Move the cup to (0.30, 0.15, 0.03). Does the shoulder effort get closer to its limit? By how much?
- 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.
- Add a new test: a target on the far side of the tiffin box, reached via an “above” waypoint. Prove it passes without collision.
- Change the tolerance from 5 mm to 2 mm and re-run Part 5. How many seeds pass now at 0.010 rad noise?
- Interactive: run the capstone twin with low noise, then high noise. Note when it says FAILED and why.
- 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.
- 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
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
- Put your arm’s measured lengths and limits into
JOINTSincapstone_6dof_sim.py. - Move
CUP,BINand the obstacle box to your own desk layout and run the mission. - Add 2 tests of your own: one that must PASS and one that must FAIL.
- Fill the robustness table: 3 noise levels × 3 damping values.
- Bench: wire the gate (NC E-stop, separate servo supply, common ground), upload
sim_gate_servo.inoand log locked → refused → unlocked → limit refusal → E-stop → reset.
Stuck? Hints, one at a time
- Keep
check_model()passing after each edit. Fix model errors before looking at mission results. - If the path hits the obstacle, raise the travel height or move the bin before changing gains.
- 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/nand a slow list ofJcommands (with heartbeats) tosim_gate_servo.inoover pySerial. - 🔴 Expert: Recreate the arm as a URDF with
<inertial>and<limit>tags, load it in Gazebo with gz_ros2_control, bridge/joint_stateswith 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
- Wikipedia — Hardware-in-the-loop simulation — plant simulation, safety, repeatability and testing failure conditions; robotics self-models.
- Wikipedia — Kill switch (emergency stop) — E-stop purpose, normally-closed fail-safe wiring, reset before restart, complementary protective measure (ISO 13850 / ISO 12100).
- Wikipedia — Inverse kinematics — computing joint values for a desired end-effector position.
- Gazebo Harmonic — Use ROS 2 to interact with Gazebo — ros_gz_bridge, bridge directions, joint states and commands between ROS 2 and Gazebo.
- Arduino Servo library (GitHub) — controlling RC hobby servos from an Arduino.
- Photos: Wikimedia Commons (credits under each image).
Self-attested for now — keep artifacts in your lab notebook.
Complete
Today's goal
0 of 1 done today
Mark this lesson complete when you have worked the experiment, knowledge check, and assignment (even roughly).