Robot Dynamics & Control

// Equation of motion for n-DOF robot
// τ = M(q)·q̈ + C(q,q̇)·q̇ + g(q) + f(q̇)
//
// M(q): n×n inertia matrix  — configuration-dependent, always positive definite
// C(q,q̇): n×n Coriolis/centrifugal matrix
// g(q): n×1 gravity vector
// f(q̇): friction vector (viscous + Coulomb)
//
// 2-DOF planar: Link masses m1, m2; lengths L1, L2; CoM at lc1=L1/2, lc2=L2/2
// M(q) = [ I1+I2+m1*lc1²+m2*(L1²+lc2²+2*L1*lc2*cos(q2))   I2+m2*(lc2²+L1*lc2*cos(q2)) ]
//        [ I2+m2*(lc2²+L1*lc2*cos(q2))                     I2+m2*lc2²                  ]
//
// g(q) = [ -(m1*lc1+m2*L1)*g*cos(q1) - m2*g*lc2*cos(q1+q2) ]
//        [                           - m2*g*lc2*cos(q1+q2) ]
//
// Key insight: M(q) changes with configuration by up to 8:1 ratio.
// This is why fixed PID gains give poor performance — the "plant" changes.

// Moments of inertia for common robot link geometries
//
// Uniform rod (length L, mass m), about CENTRE:
//   I_cm = (1/12)*m*L²
//
// Uniform rod, about ONE END (parallel axis theorem):
//   I_end = (1/12)*m*L² + m*(L/2)² = (1/3)*m*L²   ← 4× larger!
//
// Hollow cylinder (R_outer, R_inner), about symmetry axis:
//   I = (1/2)*m*(R_o² + R_i²)
//
// Solid disk: I = (1/2)*m*R²
// Sphere:     I = (2/5)*m*R²
//
// PARALLEL AXIS THEOREM:
//   I_joint = I_cm + m*d²    (d = distance CM → joint axis)
//
// COMPOSITE LINK (structure + motor + gearbox + cables):
//   I_total = Σ(I_cm_i + m_i * d_i²)
//
// Example — KUKA KR 6 Link 2 (approximate):
//   Aluminium tube 2.8 kg:      I ≈ 0.093 kg·m²
//   Motor + gearbox 1.8 kg:     I ≈ 0.044 kg·m²
//   Wiring/covers 0.4 kg:       I ≈ 0.004 kg·m²
//   Total link 2:               I ≈ 0.141 kg·m² about joint 2
//   Payload 6 kg at 650mm:      I_pay = 6 × 0.65² = 2.54 kg·m² ← dominant!
//   → Payload doubles effective inertia in extended pose

// Gravity torque — 2-DOF planar robot
import numpy as np

m1, m2 = 2.5, 1.8  # kg
L1, L2 = 0.35, 0.25  # m
lc1, lc2 = 0.175, 0.125  # m  (CoM at mid-link)
g = 9.81

def gravity_torques(q1_deg, q2_deg):
    q1 = np.radians(q1_deg)
    q2 = np.radians(q2_deg)
    # Joint 1 supports weight of both links
    tau1 = -(m1*g*lc1*np.cos(q1)
           + m2*g*(L1*np.cos(q1) + lc2*np.cos(q1+q2)))
    # Joint 2 only supports weight of link 2
    tau2 = -m2*g*lc2*np.cos(q1+q2)
    return tau1, tau2

for q1,q2,desc in [(0,0,'Both horizontal'),(90,0,'Link1 up, Link2 horiz'),
                   (45,-45,'Folded back'),(0,90,'Link2 vertical')]:
    t1,t2 = gravity_torques(q1,q2)
    print(f'{desc:30s}: τ1={t1:7.2f} N·m  τ2={t2:6.2f} N·m')
# Both horizontal          : τ1= -9.35 N·m  τ2= -2.21 N·m
# Link1 up, Link2 horiz   : τ1=  0.00 N·m  τ2= -2.21 N·m
# Folded back              : τ1= -5.08 N·m  τ2=  0.00 N·m
# Link2 vertical           : τ1= -8.58 N·m  τ2=  0.00 N·m

// BLDC Motor fundamentals — example joint motor
//
// Motor constants:
//   Kt = 0.35 N·m/A   (torque constant)
//   Ke = 0.35 V·s/rad (back-EMF = Kt in SI)
//   R  = 0.8 Ω        (winding resistance)
//   L  = 4.2 mH       (inductance)
//   J  = 0.00025 kg·m² (rotor inertia)
//
// Torque:         τ = Kt × I
// Back-EMF:       V_emf = Ke × ω
// Terminal eq:    V = I×R + L×dI/dt + V_emf
// Steady-state:   I = (V - Ke×ω) / R
//
// Speed-torque (V = 240V bus):
//   τ_stall = Kt × V/R = 0.35 × 240/0.8 = 105 N·m  (only at ω=0)
//   ω_no_load = V/Ke = 240/0.35 = 686 rad/s = 6550 rpm
//   At ω=3000rpm (314 rad/s): I = (240 - 0.35×314)/0.8 = 162 A → too high
//   → Operating point must stay within continuous envelope
//
// Continuous rating (thermal limit):
//   I_cont = 12 A  → τ_cont = 0.35×12 = 4.2 N·m
//   Copper losses: 12² × 0.8 = 115 W  (motor rated for this)
//   At I_peak = 36 A: τ_peak = 12.6 N·m, P_loss = 1037 W → 2s max
//
// RMS torque sizing:
//   τ_rms = sqrt(Σ τ_i² × t_i / T_cycle) ≤ τ_continuous

// Gearbox sizing — torque, speed, and inertia
//
// Reflected load inertia: I_reflected = I_load / N²
// Output torque:          τ_out = τ_motor × N × η
// Output speed:           ω_out = ω_motor / N
//
// KUKA LBR iiwa joint 1 (harmonic drive, N=160):
//   Motor peak torque: 12 N·m
//   Output peak:       12 × 160 × 0.82 = 1574 N·m  (η=82%)
//   Link inertia:      5.2 kg·m²
//   Reflected inertia: 5.2 / 160² = 0.000203 kg·m²
//   Motor inertia:     0.000210 kg·m²
//   Inertia ratio:     0.97:1  ← perfect match!
//
// Gearbox type comparison (same joint, same requirements):
// ─────────────────────────────────────────────────────────────
// Type        Ratio  Backlash   Efficiency   Backdrivable
// Harmonic    160    < 1 arcm   70-85%       With effort
// Cycloidal    57    < 1 arcm   85-90%       No
// Planetary   100    5-15 arcm  95-98%       Yes
// ─────────────────────────────────────────────────────────────
//
// Harmonic drive brands: Harmonic Drive AG (SHF, CSD, SHD series)
// Cycloidal brands: Nabtesco (RV-E, RV-C), Sumitomo
// Planetary brands: Neugart, Shimpo, Apex Dynamics

// Servo drive cascade control structure
//
// ROBOT CONTROLLER  (1 kHz EtherCAT)
//   q_d(t) → trajectory generator → θ_d, ω_d, τ_ff
//            ↓
// POSITION LOOP (1 kHz)
//   θ_d →[+]→ Kp_pos ─────────────────→ ω_d
//         [-]← θ_meas (23-bit encoder)
//            ↓
// VELOCITY LOOP (4 kHz)
//   ω_d →[+]→ Kp_vel + Ki_vel/s ───→ I_q_d
//         [-]← ω_meas (encoder diff)
//            ↓
// CURRENT LOOP / FOC (20 kHz)
//   Iq_d, Id_d → Park/Clarke → V_abc → SVPWM → IGBT → 3-phase motor
//
// Typical starting gains:
//   Position Kp:  50-200 (1/s)
//   Velocity Kp:  0.1-5.0 (N·m·s/rad)
//   Velocity Ki:  10-500 (N·m/rad)
//   Current Kp:   10-200 (V/A)
//
// Key specs:
//   EtherCAT cycle:   250 µs (4 kHz) or 1000 µs (1 kHz)
//   Encoder res:      23-bit = 8,388,608 counts/rev = 0.000043°
//   Current loop BW:  2-5 kHz
//   Velocity loop BW: 100-500 Hz
//   Position loop BW: 10-100 Hz
//
// Auto-tuning: inject chirp → measure freq response → compute gains
// Most modern drives (Beckhoff, Siemens) have one-click auto-tune

// Discrete PID — velocity form (industrial standard)
struct PID {
    float Kp, Ki, Kd, dt;
    float e_prev = 0, e_prev2 = 0, u = 0;
    float u_min = -1e9, u_max = 1e9;

    float update(float setpoint, float measured) {
        float e = setpoint - measured;
        // Velocity form: Δu = Kp*(e-e_prev) + Ki*e*dt + Kd*(e-2*e_prev+e_prev2)/dt
        float delta_u = Kp*(e - e_prev)
                      + Ki*e*dt
                      + Kd*(e - 2*e_prev + e_prev2)/dt;
        u = fclamp(u + delta_u, u_min, u_max); // anti-windup built-in
        e_prev2 = e_prev; e_prev = e;
        return u;
    }
};

// Typical robot joint PID ranges:
// Position loop (outer):
//   Kp: 50-300 (rad/s per rad error)
//   Ki: 5-50   (eliminates gravity droop at rest)
//   Kd: 0-5    (rarely needed — inner loop handles damping)
//
// Velocity loop (inner, fast):
//   Kp: 0.5-10   (N·m per rad/s error)
//   Ki: 50-500   (fast integral, removes velocity droop)
//
// Practical tuning start:
// 1. Zero Ki and Kd
// 2. Increase Kp until oscillation (Ku)
// 3. Set Kp = 0.6*Ku
// 4. Add Ki = Kp/10, increase until offset eliminated in < 100ms
// 5. Add Kd = Kp/10 if overshoot remains

// Frequency domain analysis — Python/scipy
from scipy import signal
import numpy as np

# Plant model: single inertia J with damping B
# Velocity output, torque input: P(s) = 1/(J*s + B)
J, B = 0.005, 0.02
P = signal.TransferFunction([1], [J, B])

# Velocity PI controller: C(s) = Kp + Ki/s = (Kp*s + Ki)/s
Kp, Ki = 2.0, 80.0
C = signal.TransferFunction([Kp, Ki], [1, 0])

# Open loop L(s) = C(s)*P(s)
L_num = np.polymul([Kp, Ki], [1])
L_den = np.polymul([1, 0], [J, B])
L = signal.TransferFunction(L_num, L_den)

# Bode analysis
w = np.logspace(0, 4, 1000)
w, mag, phase = signal.bode(L, w)

# Gain crossover (|L| = 0 dB)
gain_lin = 10**(mag/20)
cross_idx = np.argmin(np.abs(gain_lin - 1))
w_cross = w[cross_idx]
bw_hz = w_cross / (2*np.pi)
phase_margin = 180 + phase[cross_idx]

print(f'Bandwidth:     {bw_hz:.1f} Hz')
print(f'Phase margin:  {phase_margin:.1f}° (target > 45°)')
print(f'Gain margin:   from -180° phase crossing')

# Transport delay effect: phase lag = -ω*T_delay (radians)
# At BW=200Hz with T=1ms: lag = 2π*200*0.001 = 1.26 rad = 72°!
# → Delay is the primary bandwidth limiter in digital systems

// Feedforward + PID — combined position controller
//
// Total control:
// u = Kff_a * q̈_d       ← acceleration FF (predict inertia torque)
//   + Kff_v * q̇_d       ← velocity FF (predict friction/damping)
//   + Kff_g * g(q)       ← gravity FF (predict gravity torque)
//   + Kp * (q_d - q)     ← proportional error
//   + Ki * ∫(q_d - q)dt  ← integral (steady-state)
//   + Kd * (q̇_d - q̇)    ← derivative (damping)
//
// Ideal: Kff_a = J_total, Kff_v = B, Kff_g = 1.0
// With perfect FF: PID only handles payload variation,
//   temperature drift, friction changes → gains can be much lower
//
// Tracking error comparison (90° move, 1.0s trapezoidal):
//   PID only (high gains):       peak error 8.2 mm TCP
//   PID + velocity FF:           peak error 3.1 mm TCP
//   PID + velocity + accel FF:   peak error 0.9 mm TCP
//   Full computed-torque + PID:  peak error 0.2 mm TCP
//
// Setting FF gains (practical procedure):
// 1. Zero all, command trapezoidal move, note tracking error shape
// 2. If error follows velocity shape → increase Kff_v
// 3. If error peaks at start/end of move → increase Kff_a
// 4. If error persists at rest → increase Kff_g (gravity comp)
// 5. Fine-tune PID to handle residuals

# Computed Torque Control — 2-DOF planar robot
import numpy as np

def inertia_matrix(q, m1, m2, lc1, lc2, L1, I1, I2):
    c2 = np.cos(q[1])
    m11 = I1+I2+m1*lc1**2+m2*(L1**2+lc2**2+2*L1*lc2*c2)
    m12 = I2+m2*(lc2**2+L1*lc2*c2)
    m22 = I2+m2*lc2**2
    return np.array([[m11,m12],[m12,m22]])

def coriolis(q, qdot, m2, L1, lc2):
    h = -m2*L1*lc2*np.sin(q[1])
    return np.array([h*qdot[1]**2 + 2*h*qdot[0]*qdot[1], -h*qdot[0]**2])

def gravity(q, m1, m2, lc1, lc2, L1, g=9.81):
    c1, c12 = np.cos(q[0]), np.cos(q[0]+q[1])
    return np.array([-(m1*lc1+m2*L1)*g*c1 - m2*g*lc2*c12,
                     -m2*g*lc2*c12])

def computed_torque(q, qdot, q_d, qdot_d, qddot_d, params, Kp, Kd):
    e  = q_d - q
    de = qdot_d - qdot
    qddot_cmd = qddot_d + Kd*de + Kp*e  # desired accel with PD correction
    M = inertia_matrix(q, **params)
    C = coriolis(q, qdot, params['m2'], params['L1'], params['lc2'])
    G = gravity(q, **params)
    return M @ qddot_cmd + C + G

# With CTC, gains can be much lower than pure PID:
Kp = np.diag([400, 400])  # same response everywhere!
Kd = np.diag([40, 40])    # critically damped (ζ = Kd/(2√Kp) = 1.0)
print('Natural freq:', np.sqrt(400)/(2*np.pi), 'Hz')
print('Damping ratio:', 40/(2*np.sqrt(400)))

# Cartesian impedance control — stiffness profiles
import numpy as np

# Stiffness matrix K (diagonal — independent per DOF)
# Units: N/m for translation, N·m/rad for rotation
profiles = {
    'free_space_rigid':   np.diag([5000, 5000, 5000, 200, 200, 200]),
    'contact_assembly':   np.diag([ 500,  500, 2000,  50,  50, 100]),
    'polishing':          np.diag([  50,   50, 3000,  20,  20,  50]),
    'human_handguide':    np.diag([  10,   10,   10,   5,   5,   5]),
    'surgical':           np.diag([  30,   30,   30,  10,  10,  10]),
}

# Control law: τ = Jᵀ · (K·e + B·ė)
# J: Jacobian matrix (maps Cartesian forces to joint torques)
# e = x_d - x  (6D Cartesian error: 3 position + 3 orientation)
# ė = ẋ_d - ẋ  (6D velocity error)

# Peg insertion example:
# Z-axis: low stiffness (500 N/m) — allows compliance into hole
# XY-axes: moderate (2000 N/m) — guides peg to centre via contact
# As peg contacts chamfer at angle:
#   Contact force F_contact → Cartesian deflection Δx = F/K
#   Deflection guides peg toward centre → insertion succeeds
# With pure position control:
#   Robot fights contact → jams → servo fault

print('Deflection at 10N contact force:')
for task, K in profiles.items():
    defl_z = 10/K[2,2]*1000  # mm
    print(f'  {task:25s}: {defl_z:.1f} mm deflection in Z')

# Hybrid position/force control — surface following
import numpy as np

class HybridController:
    def __init__(self, K_pos, K_force, Ki_force, S, dt=0.001):
        """
        S: selection vector — 1=force control, 0=position per DOF
        """
        self.K_pos    = K_pos
        self.K_force  = K_force
        self.Ki_force = Ki_force
        self.S = np.diag(S)
        self.Sc = np.eye(6) - self.S  # complement
        self.integral = np.zeros(6)
        self.dt = dt

    def update(self, x_d, x, f_d, f_meas):
        pos_err   = x_d - x
        force_err = f_d - f_meas
        self.integral += force_err * self.dt

        v_force = self.S  @ (self.K_force * force_err
                           + self.Ki_force * self.integral)
        v_pos   = self.Sc @ (self.K_pos * pos_err)
        return v_force + v_pos

# Polishing task: control normal force (Z), follow path (XY)
hybrid = HybridController(
    K_pos=np.diag([100,100,0,10,10,10]),
    K_force=np.diag([0,0,0.03,0,0,0]),
    Ki_force=np.diag([0,0,0.4,0,0,0]),
    S=[0,0,1,0,0,0]
)
print('Force control applications:')
print('  Grinding:    20-50 N, ±2 N control')
print('  Deburring:   5-20 N, self-adapts to surface')
print('  Polishing:   2-10 N, constant contact pressure')
print('  Assembly:    < 5 N, position-dominated with force monitoring')
print('  F/T sensors: ATI Gamma (±32N), ATI Delta (±165N)')

// Servo tuning procedure — bandwidth-based method
//
// ══ Step 1: Velocity loop ═════════════════════════════
// Target BW: 200 Hz (typical industrial joint)
// Kp_vel = 2*pi*BW_target * J_total
//        = 2*pi*200 * 0.003 = 3.77 N·m/(rad/s)
// Ti    = 5 / BW_target = 5/200 = 0.025 s
// Ki_vel = Kp_vel / Ti = 3.77/0.025 = 151 N·m/rad
//
// Test: velocity step 10 rpm
// Target: rise time 5-20ms, overshoot < 10%, settling < 50ms
//
// ══ Step 2: Position loop ════════════════════════════
// BW_pos = BW_vel / 10 = 20 Hz
// Kp_pos = 2*pi*20 = 125.7 (1/s)
//
// Test: position step 5mm
// Target: rise 20-50ms, overshoot < 5%, settling < 150ms
//
// ══ Step 3: Feedforward ══════════════════════════════
// Command constant-velocity move:
//   If tracking error follows velocity profile → increase Kff_v
// Command trapezoidal move:
//   If error peaks at accel/decel → increase Kff_a
// Iterate until tracking error < 0.1° during motion
//
// ══ Performance targets (6 kg payload robot) ═════════
// Joint tracking error during move:  < 0.5°
// Position error at rest:            < 0.01°
// TCP repeatability:                 ±0.02 mm
// Step overshoot:                    < 5%
// Settling time (10mm step):         < 150 ms
// Following error at 100 mm/s TCP:   < 0.3 mm

# Resonance ID and notch filter design
from scipy import signal
import numpy as np

# Inject chirp signal at low amplitude (5% rated torque)
def make_chirp(f_start=10, f_end=500, duration=5, fs=4000):
    t = np.arange(0, duration, 1/fs)
    return t, signal.chirp(t, f0=f_start, f1=f_end,
                           t1=duration, method='logarithmic') * 0.05

# After measuring response: identify peaks in freq response
# Common finding: peak at 120 Hz (harmonic drive torsional mode)
f_resonance = 120  # Hz

# Design notch filter: Q=3, fs=4000Hz
def notch_filter(f_notch, Q, fs=4000):
    w0 = 2*np.pi*f_notch/fs  # normalized frequency
    b, a = signal.iirnotch(w0/np.pi, Q)
    return b, a

b, a = notch_filter(120, Q=3)
w, H = signal.freqz(b, a, fs=4000, worN=2000)
idx = np.argmin(np.abs(w-120))
print(f'Notch depth @ 120 Hz: {20*np.log10(abs(H[idx])):.1f} dB')
# Should be > -20 dB attenuation

# Rule of thumb: after adding notch:
# Bandwidth can be increased by 30-100% before instability
# Phase lag at BW (from notch): < 15° for Q=3 (acceptable)

# Low-pass filter on velocity feedback (tame encoder noise)
# Cutoff = 3 × velocity loop bandwidth = 3 × 200 = 600 Hz
sos_lpf = signal.butter(2, 600, fs=4000, btype='low', output='sos')
print('LPF phase lag @ 200Hz:', -signal.group_delay(
    signal.butter(2, 600, fs=4000, output='ba'), [200], fs=4000)[1][0]*200, '°')

# Payload identification — least-squares estimation
import numpy as np

# Model: τ_payload = J^T * W(q) * [m, m*rx, m*ry, m*rz]
# where W(q) is the gravity wrench regressor, built from FK

def build_regressor_row(J, g_in_flange):
    """
    Build one row of the regressor matrix.
    J: 3×n Jacobian (linear velocity part only)
    g_in_flange: 3×1 gravity vector in flange frame
    Returns 4 columns: [m, m*rx, m*ry, m*rz] coefficients
    """
    # Simplified: gravitational force at TCP
    F_gravity_cols = np.vstack([g_in_flange,       # mass term
                                np.zeros((3,3))])   # + moment terms
    return J.T @ F_gravity_cols[:3, :]

# Procedure:
print('Payload identification procedure:')
print('1. Attach payload and secure safely')
print('2. Move robot to 15+ diverse orientations')
print('3. At each pose (static): measure joint torques')
print('4. Build regressor matrix Y from FK + Jacobians')
print('5. Solve: [m, m*rx, m*ry, m*rz] = (Y^T Y)^-1 Y^T τ')
print('6. Enter result into controller payload table')
print()
print('Verification tests:')
print('  Static: hold arm at 45°, gravity comp ON — no sag')
print('  Dynamic: no false collision alarms at rated speed')
print('  Accuracy: mass ±0.1 kg, CoG ±5 mm (typical)')
print()
print('Wrong payload symptoms:')
print('  Sag at low speed → gravity comp wrong → check mass/CoG')
print('  Oscillation at stops → inertia FF wrong → check inertia')
print('  False collision faults → model mismatch → re-identify')

// Arc welding motion quality
//
// Path accuracy requirements (automotive body welding):
//   TCP path deviation:   ±0.5 mm  (visible bead wander)
//   TCP speed variation:  ±2%      (±3 mm/s at 150 mm/s)
//   Arc length:           ±0.5 mm  (torch-to-work distance)
//
// Interpolation error for circular welds:
//   Max chord error: Δ = R*(1-cos(φ/2)) ≈ R*φ²/8
//   R=80mm, φ=5° steps: Δ = 0.076mm ✓
//   R=80mm, φ=10° steps: Δ = 0.30mm — borderline!
//   → Use ≤5° steps for circular welds OR arc interpolation
//
// Speed continuity:
//   FINE stop mode:  decelerates to ZERO at every waypoint
//   CNT/CONT mode:   robot blends through corner at speed
//   → Always use CNT for welding seams
//
// Arc voltage sensing (TAST) — torch height control:
//   v_arc_high → TCP too far → move closer
//   v_arc_low  → TCP too close → move away
//   PI control: Kp=2mm/V, Ki=5mm/(V·s), BW~5Hz
//
// Welding robot specs:
//   FANUC Arc Mate 100iD: 6kg, 1373mm, ±0.08mm
//   ABB IRB 1600-6/1.45: 6kg, 1450mm, ±0.05mm
//   KUKA KR 6 R1840:      6kg, 1840mm, ±0.04mm
//   Yaskawa MA1440:       6kg, 1440mm, ±0.08mm

# Force control — grinding/deburring
class GrindingForceController:
    def __init__(self, F_target, Kp=0.03, Ki=0.5, dt=0.001):
        self.F_target = F_target  # N, e.g. 25N for aluminium
        self.Kp = Kp    # m/s per N
        self.Ki = Ki    # m/s² per N
        self.dt = dt
        self.integral = 0.0
        self.F_filt = F_target
        self.alpha = 0.15  # low-pass filter (noisy F/T data)

    def update(self, F_measured):
        # Low-pass filter the F/T reading
        self.F_filt = self.alpha*F_measured + (1-self.alpha)*self.F_filt
        error = self.F_target - self.F_filt
        self.integral += error * self.dt
        # Returns: velocity correction in contact-normal direction
        return self.Kp*error + self.Ki*self.integral

# Production parameters (aluminium die casting deburring):
# Tool:           carbide burr, 12mm Ø, 25,000 rpm
# Normal force:   20-30 N  (nominal 25 N)
# Path speed:     40-80 mm/s along the seam
# F/T sensor:     ATI Gamma SI-130-10 at 1 kHz
# Force BW:       ~5 Hz (surface contact stiffness limits this)
#
# Quality metrics:
#   Material removal: 0.15 ± 0.02 mm³/mm (consistent)
#   Surface finish:   Ra ≤ 3.2 µm (no gouging)
#   Cycle time:       45 s/casting vs 120 s manual
#
# Why position control fails:
#   Surface varies ±1.5mm per casting
#   At Ks=10 N/mm: 1.5mm position error → 15N force variation
#   → Some passes remove too much, others skip entirely

// Delta robot pick-and-place — cycle time & thermal analysis
//
// ABB IRB 360 FlexPicker — standard 'ice cream cone' move
// (25mm vertical + 150mm horizontal, 1 kg payload)
//
// Motion profile:
//   Peak TCP speed:         10 m/s
//   Peak TCP acceleration:  100 m/s² = 10.2 g
//   Motor peak torque:      8.0 N·m  (350% of 2.3 N·m cont)
//   Cycle time:             0.58 s  (103 picks/min standard)
//   Rated max:              150 picks/min  (T=0.4s)
//
// RMS torque check at 150 picks/min:
//   Cycle = 0.4s: 0.1s @ 8 N·m peak + 0.3s @ 1 N·m idle
//   τ_rms = sqrt((8²×0.1 + 1²×0.3)/0.4) = sqrt(16.75) = 4.1 N·m
//   Motor continuous: 2.3 N·m → 4.1 > 2.3 !!
//   → Motor IS overloaded at 150 picks/min (liquid cooled!)
//   → With liquid cooling: continuous rating rises to ~5 N·m ✓
//
// Settling time:
//   Carbon-fibre links: very low structural damping
//   Active anti-sway: extra derivative impulse at final phase
//   Reduces settling: 120ms → 40ms (3× improvement)
//   Vision trigger: add 15-50ms latency
//
// Throughput limit calculation:
//   T_cycle = T_pick + T_place + T_transit + T_settle + T_vision
//           = 0.08  + 0.08   + 0.18     + 0.04    + 0.05 = 0.43s
//   Theoretical max: 140 picks/min (rated 150 = with motor reserve)