X1.1.3

Lesson 1.1.3 — PID Control and Trajectory Tracking: The Feedback Loop, the Computed-Torque Feedforward, and the Real-Time Constraints

Week 6 · Prerequisites: 0.1.1, 0.1.2, 0.1.3, 1.1.1, 1.1.2.

Prior results used. This lesson uses 1.1.1’s geometric Jacobian and its moment-arm column, 1.1.2’s Newton–Raphson IK, DLS, and singularity analysis, 0.3.3’s Kalman-filter gain as a precision-weighted average, and 0.3.4’s DLS as a Tikhonov-regularized inverse. Each is restated at the point of use.

Hands forward. This lesson hands forward one capability: the learner can design a joint-space PID controller, a task-space resolved-rate controller, and a computed-torque controller for the running case, tune each from the plant’s parameters, discretize each at a real-time rate, and diagnose the four canonical control bugs (integral windup, derivative noise amplification, gain-induced instability, and computed-torque model mismatch) from their time-domain symptoms — the capability that Module 1.2 (robot learning for control), Module 2.1 (perception-driven control), and the entire Phase-3 VLA deployment stack assume.

Running case. The same 6-DOF MuJoCo arm from 1.1.1–1.1.2, now moving: the arm must track a smooth joint-space trajectory q_d(t) from t = 0 to t = 1 s (a minimum-jerk profile from q = 0 to q = [0.5, 0.5, −0.5, 0.5, −0.5, 0.5]), with the dynamics of a rigid-body arm: M(q) q̈ + C(q, q̇) q̇ + g(q) = τ, where M(q) ∈ ℝ^{6×6} is the inertia matrix, C(q, q̇) q̇ is the Coriolis/centrifugal torque, g(q) is the gravity torque, and τ is the joint torque (the control input). The lesson’s central result is that PID alone (per-joint, model-free) tracks the trajectory with bounded but non-zero error and is unstable if the gains are too high, while computed-torque (the dynamics’ feedforward plus PID’s feedback) tracks it with exponentially small error, and the real-time constraint (a 1 kHz sample rate) makes both controllers’ discrete-time behavior the actual behavior, not the continuous-time ideal.

§L.0 — Lesson map, objectives, and misconceptions

§L.0.1 — Lesson map and word budget

§L.0.2 — Objectives

By the end of this lesson you will be able to do eight things:

1. State the PID control law τ = K_p e + K_i ∫e dt + K_d ė, the role of each term, and the physical units of each gain.
2. Derive the closed-loop stability condition for a second-order plant m q̈ + b q̇ = τ under PD, and state the K_p and K_d’s relationship to the natural frequency ω_n and the damping ratio ζ.
3. Tune a PD controller for the running case’s 2-link planar sub-arm by matching the desired ω_n and ζ, and verify the step response’s overshoot and settling time against the analytic predictions.
4. Derive the computed-torque control law τ = M(q)(q̈_d + K_d ė + K_p e) + C(q, q̇) q̇ + g(q), and state why it linearizes the closed-loop dynamics and gives exponentially decaying error.
5. State the model mismatch effect — a wrong M or g leaves a residual error that the PID’s integral term cannot fully eliminate for a time-varying trajectory — and diagnose it from the steady-state lag.
6. Generate a minimum-jerk trajectory q_d(t) between two configurations, and state the trajectory’s peak velocity and peak acceleration as functions of the duration.
7. State the sampling rate’s effect on the closed-loop stability (ω_n · T_s ≪ 1), and diagnose a discrete-time instability from the oscillation at the Nyquist frequency.
8. Diagnose the four canonical control bugs — integral windup (overshoot after saturation), derivative noise (high-frequency chatter), gain-induced instability (growing oscillation), and computed-torque model mismatch (steady-state lag) — from their time-domain symptoms.

§L.0.3 — Misconception inventory

Each misconception is displaced in the section named.

Everything is self-contained. Where a prerequisite lesson is used, the result is restated at the point of use.

§L.0.5 — Retrieval warm-up

This is the fifteenth lesson. The prompt requires drawing from lessons two to five steps back; 1.1.2 is one step back and is excluded. The warm-up draws from 1.1.1 (two steps back), 0.3.4 (three), 0.3.3 (four), and 0.3.2 (five).

Warm-up 1 (from 1.1.1, the Jacobian’s moment arm). State the geometric Jacobian’s column for a revolute joint, and explain the moment arm’s role. Answer: J_i = [ω_i; ω_i × (p_ee − q_i)], where ω_i is the joint axis, q_i is a point on the axis, and p_ee is the end-effector’s position. The moment arm ω_i × (p_ee − q_i) is the linear velocity contribution from the joint’s rotation at the end-effector’s point. How this is used: §L.2’s task-space resolved-rate control uses the Jacobian to map the joint velocities to the end-effector’s velocity, and the moment arm is the coupling that makes the arm’s motion non-diagonal — a single joint’s motion affects multiple task-space directions, which is why PID per joint does not decouple the task-space error.

Warm-up 2 (from 0.3.4, the DLS and its damping). State the DLS update and the damping’s role. Answer: q_{k+1} = q_k + Jᵀ(JJᵀ + λ²I)⁻¹ e, where λ bounds the pseudoinverse’s 1/σ_min amplification at 1/(2λ), at the cost of a systematic error O(λ²). How this is used: §L.2’s resolved-rate control is the continuous-time version of the DLS — q̇ = J⁺ v_d — and its singularity behavior is the same as the IK’s, with the damping λ being the controller’s regularization for the velocity-level control.

Warm-up 3 (from 0.3.3, the Kalman filter’s gain). State the Kalman gain and its interpretation as a precision-weighted average. Answer: K = Σ⁻Hᵀ(HΣ⁻Hᵀ + R)⁻¹, and it weights the prior by the relative precision of the measurement and the prediction; a larger R (measurement noise) gives a smaller gain (trust the prediction more). How this is used: §L.1’s PID gains have the same structure — the proportional gain K_p weights the error against the plant’s inertia, and the derivative gain K_d weights the error’s rate; the optimal gains (from LQR) are K_p = M ω_n² and K_d = 2ζ M ω_n, which is the same precision-weighted structure with the inertia M as the “prior” and the “desired bandwidth” as the “measurement.”

Warm-up 4 (from 0.3.2, the MAP and regularization). State the MAP estimate and its regularization interpretation. Answer: θ_MAP = argmax_θ [log p(D|θ) + log p(θ)], and the Gaussian prior’s log is the L2 penalty λ‖θ‖²; the MAP is a regularized ML, and the regularization’s strength is the prior’s precision. How this is used: §L.2’s computed torque is the feedforward (the dynamics’ model) plus the feedback (the PID’s correction), and the feedback’s role is the regularization — the feedforward is the prior (the model’s best guess), and the feedback is the measurement (the actual error), with the gains weighting the two. The computed torque is a MAP-like combination of model and data, and the PID alone is the ML (data-only, no prior), which is why it has non-zero error under model uncertainty.

Part A — The Control Problem and PID

§L.1 — Objects: the plant, the error, and the three PID terms

The control problem is: given a plant (a dynamical system) and a desired trajectory q_d(t), find a control input τ(t) that makes the plant’s state q(t) track q_d(t) — i.e., the error e(t) = q_d(t) − q(t) is bounded (ideally zero).

The plant for the running case is the rigid-body arm’s dynamics:

where:

• q ∈ ℝ⁶ is the joint configuration.
• q̇ ∈ ℝ⁶ is the joint velocity.
• q̈ ∈ ℝ⁶ is the joint acceleration.
• M(q) ∈ ℝ^{6×6} is the inertia matrix — symmetric, positive-definite for every q (the arm’s kinetic energy is ½ q̇ᵀ M q̇ > 0 for q̇ ≠ 0).
• C(q, q̇) q̇ ∈ ℝ⁶ is the Coriolis/centrifugal torque — quadratic in q̇, and zero at q̇ = 0.
• g(q) ∈ ℝ⁶ is the gravity torque — the torque needed to hold the arm at q against gravity.
• τ ∈ ℝ⁶ is the joint torque — the control input.

The PID control law is:

where:

• K_p e — the proportional term, a “spring” that pulls q toward q_d; its gain K_p has units of torque per radian (N·m/rad), and it is the stiffness of the virtual spring.
• K_i ∫e — the integral term, a “memory” that accumulates the error and eliminates constant steady-state errors; its gain K_i has units of torque per (radian·second) (N·m/(rad·s)).
• K_d ė — the derivative term, a “damper” that resists the error’s rate of change; its gain K_d has units of torque per (radian/second) (N·m·s/rad), and it is the damping of the virtual spring.

The closed-loop for the 2-link arm’s single joint (a simple model J q̈ = τ, where J is the joint’s inertia) under PD (K_i = 0) is:

which rearranges to the error dynamics:

This is a second-order linear system with natural frequency ω_n = √(K_p/J) and damping ratio ζ = K_d / (2√(K_p J)). For a constant q_d (a step), the right side is zero, and the error decays as a damped oscillator. For a time-varying q_d (a trajectory), the right side is q̈_d ≠ 0, and the error is forced by the trajectory’s acceleration.

§L.1.1 — Result: PD’s closed-loop stability is governed by ω_n and ζ, and the gains that give a critically damped response are K_p = J ω_n², K_d = 2ζ J ω_n

Claim. For the single-joint model J q̈ = τ under PD, the closed-loop error dynamics are ë + 2ζω_n ė + ω_n² e = q̈_d with ω_n = √(K_p/J) and ζ = K_d/(2√(K_p J)). The closed loop is stable for K_p > 0 and K_d > 0, critically damped for ζ = 1, and oscillatory for ζ < 1. The step response’s overshoot is exp(−πζ/√(1 − ζ²)) for ζ < 1, and the settling time (2%) is 4/(ζ ω_n).

Proof sketch. The error dynamics are a second-order linear ODE with constant coefficients. The characteristic polynomial is s² + 2ζω_n s + ω_n², whose roots are s = −ζω_n ± ω_n√(ζ² − 1). For ζ > 0, the roots have negative real part (both −ζω_n and the real part of the square-root term), so the system is stable. For ζ < 1, the roots are complex conjugate (underdamped, oscillatory). For ζ = 1, they are real and equal (critically damped, the fastest non-oscillatory response). The overshoot formula is the standard step response of the second-order system.

Corollary. The critical damping gains are K_p = J ω_n² and K_d = 2J ω_n (for ζ = 1). A faster response (higher ω_n) requires higher K_p and K_d, and the higher gains are closer to the stability limit for a real plant with unmodeled dynamics (e.g., the actuator’s bandwidth, the sensor’s noise), which is why high gains are not always better (M2).

Corollary. The integral term’s closed-loop is third-order, and it is stable for K_i in a range determined by the other gains and the plant’s model; the integral’s stability is harder to characterize than the PD’s, and its windup (M3) can destabilize a PD-stable loop when the actuator saturates.

§L.1.2 — Worked-example ladder

WE-L.1.a — Fully worked: tuning PD for the 2-link arm’s single joint

Input. A single joint with J = 0.05 kg·m², desired ω_n = 20 rad/s, ζ = 0.7.

Trace.

1. K_p = J ω_n² = 0.05 · 400 = 20 N·m/rad.
2. K_d = 2ζ J ω_n = 2 · 0.7 · 0.05 · 20 = 1.4 N·m·s/rad.
3. Closed-loop poles: s = −ζω_n ± ω_n√(ζ² − 1) = −14 ± 20√(0.49 − 1) = −14 ± 20·0.714j = −14 ± 14.28j.
4. Settling time (2%): 4/(ζω_n) = 4/14 ≈ 0.286 s.
5. Overshoot: exp(−π·0.7/√(0.51)) = exp(−π·0.7/0.714) = exp(−3.08) ≈ 0.046 = 4.6%.

Output and interpretation. The closed loop has poles at −14 ± 14.28j, a settling time of 0.286 s, and an overshoot of 4.6%. The gains K_p = 20, K_d = 1.4 give the desired ω_n = 20, ζ = 0.7. The settling time’s 4/(ζω_n) = 0.286 s is the speed — a faster response would need ω_n > 20, which would need higher gains, which is the trade-off (M2).

Self-explanation 1. Before reading on: the settling time is 0.286 s, and the oscillation frequency (damped) is ω_d = ω_n√(1 − ζ²) = 20·0.714 = 14.28 rad/s. What is the period of the oscillation, and how many oscillations fit in the settling time?

Answer. The period is T_d = 2π/ω_d = 2π/14.28 ≈ 0.44 s. In the settling time 0.286 s, there are about 0.286/0.44 ≈ 0.65 oscillations — less than one full oscillation. The decay is faster than the oscillation, which is why the overshoot is only 4.6% — the envelope exp(−14t) decays by e^{-1} ≈ 0.37 in t = 1/14 ≈ 0.071 s, and the first peak (at t ≈ π/ω_d ≈ 0.22 s) has already decayed by exp(−14·0.22) ≈ exp(−3.08) ≈ 0.046, which is the overshoot’s value.

Self-explanation 2. Before reading on: if the joint’s inertia J is wrong (the actual J = 0.1, not 0.05), what are the actual ω_n and ζ under the tuned K_p = 20, K_d = 1.4?

Answer. ω_n = √(20/0.1) = √200 ≈ 14.14 rad/s, ζ = 1.4/(2√(20·0.1)) = 1.4/(2·1.414) = 1.4/2.828 ≈ 0.495. The actual ω_n is lower (14.14 vs. 20) and the actual ζ is lower (0.495 vs. 0.7) — the system is slower and more oscillatory than the design, and the overshoot is exp(−π·0.495/√(0.755)) ≈ exp(−1.79) ≈ 0.167 = 16.7% — 4× the design’s. The model mismatch in J is the source of the degradation, and the feedback (PD) is robust to the mismatch (it’s still stable), but its performance is worse.

WE-L.1.b — Completion problem

Input. Same joint (J = 0.05), but ω_n = 40 rad/s, ζ = 1.0 (critically damped).

Trace (final 25% as prompts).

1. K_p = J ω_n² = 0.05 · 1600 = 80. ⟨your turn⟩
2. K_d = 2ζ J ω_n = 2·1·0.05·40 = 4. ⟨your turn⟩
3. Closed-loop poles: s = −ω_n = −40 (double root). ⟨your turn: verify⟩
4. Settling time: 4/ω_n = 4/40 = 0.1 s. ⟨your turn⟩
5. Overshoot: 0 (critical damping). ⟨your turn⟩

Answers.

1. K_p = 80. ✓
2. K_d = 4. ✓
3. s = −40 (a double root). ✓
4. 0.1 s. ✓
5. 0%. ✓

The lesson. The critically damped design (ζ = 1) has no overshoot and a faster settling (0.1 s vs. 0.286 s), at the cost of higher gains (K_p = 80 vs. 20, K_d = 4 vs. 1.4). The higher gains are closer to the stability limit for a real plant with unmodeled dynamics (actuator’s bandwidth, sensor noise), and the choice between ζ = 0.7 (some overshoot, lower gains) and ζ = 1 (no overshoot, higher gains) is the trade-off.

WE-L.1.c — Faded: the integral term’s steady-state error elimination

Setup. Same joint (J = 0.05), a constant setpoint q_d = 1.0, a disturbance torque τ_d = 0.5 N·m (e.g., the gravity’s effect if the joint is vertical).

Question. What is the steady-state error under PD (K_p = 20, K_d = 1.4, K_i = 0)? Under PID with K_i = 5?

You must produce the intermediate steps. The setup and the answer are given.

Answer. Under PD, the steady-state (when q̈ = q̇ = 0) has K_p (q_d − q) = τ_d, so e_ss = τ_d / K_p = 0.5/20 = 0.025 rad. Under PID, the integral term accumulates the error until its contribution K_i ∫e cancels the disturbance, so e_ss = 0. The time to eliminate the error is O(1/(ζω_n)) (the settling time, ~0.286 s), and the transient has an overshoot (the integral’s memory adds a lag).

The lesson. The PD has a steady-state error proportional to the disturbance / K_p, and the PID eliminates it via the integral’s accumulation. The cost is the overshoot and the windup (M3) — the integral accumulates during the transient, and if the actuator saturates, the integral grows unboundedly and the overshoot is large.

WE-L.1.d — Contrastive near-miss: the derivative term without filtering

Correct solution. A filtered derivative K_d s/(1 + s/ω_f) with ω_f ≈ 10 ω_n, which attenuates the high-frequency noise before the derivative’s amplification.

Near-miss. A raw derivative K_d ė, which amplifies the noise by K_d · ω_noise at high frequencies.

Why the near-miss produces a plausible but incorrect number. The raw derivative’s noise amplification is K_d · ω_noise, and for a real encoder with quantization noise at ω_noise ≈ 1000 rad/s and K_d = 1.4, the amplified torque is 1.4 · 1000 = 1400 N·m — orders of magnitude larger than the signal (which is ~1 N·m). The torque is dominated by the noise, the joint chatters (a high-frequency vibration), and the system is unstable in practice (the actuator’s saturation and the mechanical wear). The symptom is the high-frequency chatter in the joint’s motion, and the fix is the filter.

The discrepancy, stated. The raw derivative s and the filtered s/(1 + s/ω_f) coincide for ω < ω_f (the low-frequency signal) and diverge for ω > ω_f (the high-frequency noise). The filter attenuates the noise by ω_f/ω at high frequencies, and the raw derivative amplifies it by ω. The difference is the high-frequency behavior, and the noise is exactly the high-frequency part.

Self-explanation 3. Before reading on: if the filter’s cutoff ω_f is too low (e.g., ω_f = ω_n), what happens to the closed-loop stability?

Answer. The filter adds a pole at −ω_f, which adds phase lag to the loop. At the crossover frequency, the phase margin is reduced by arctan(ω_c/ω_f), and for ω_f = ω_n, the lag is arctan(1) = 45° — a significant reduction. The system is less stable (lower phase margin), and the filter’s benefit (noise attenuation) is paid in stability. The choice of ω_f is the trade-off: ω_f ≈ 10 ω_n gives minimal phase lag (the arctan(ω_c/(10 ω_c)) = arctan(0.1) ≈ 5.7°) and adequate noise attenuation.

§L.1.3 — Comparative table: the three PID terms

Design principle (P15.6). Dimension the gains by the inertia, not by the number. K_p = J ω_n² and K_d = 2ζ J ω_n both carry J, so a K_p copied from another arm is a different ω_n and a different ζ: the same K_p = 20, K_d = 1.4 gives ζ = 0.7 and a 4.6% overshoot on J = 0.05, and ζ = 0.495 and a 16.7% overshoot on J = 0.1. Tune by choosing ω_n and ζ and converting through the inertia you actually have, and write that inertia down beside the gains.

§L.1.4 — Concept check

Concept check (displaces M2). A learner says: “the error is too big, so I’ll raise K_p — a higher gain is faster and the error shrinks.”

Answer. The learner has failed to model the second-order closed loop’s two parameters and the fact that K_p does not set both. With K_d fixed, ζ ω_n = K_d/(2J) is independent of K_p, so the settling time 4/(ζ ω_n) = 8J/K_d does not move at all: on the J = 0.05 arm, doubling K_p from 20 to 40 at K_d = 1.4 leaves the settling time at 0.286 s and raises the overshoot from 4.6% to 16.7%, because ζ falls from 0.7 to 0.495. What the higher gain buys is bandwidth, and what it costs is phase margin. The bug class is gain-induced oscillation read as insufficient gain: the joint rings, the learner raises K_p again, the ringing grows, and eventually the loop is unstable — the response is read as “still too slow” all the way to divergence. The correct move is to state the target (ω_n, ζ), convert to both gains, and change K_d when the damping is the problem.

§L.1.5 — Consolidation checkpoint 1

(a) You should now be able to: (i) state each term’s law, units, and the physical object it models — a spring, a memory, and a damper; (ii) convert a target (ω_n, ζ) into (K_p, K_d) for a given J, and read back the poles, the settling time, and the overshoot; (iii) state why raising K_p at fixed K_d leaves the settling time 8J/K_d unchanged while the overshoot grows.

(b) Transfer probe. A colleague mounts the same joint’s controller on a heavier link, J = 0.2 instead of 0.05, and keeps K_p = 20, K_d = 1.4. Without running it, give ω_n, ζ, the overshoot, and the settling time. Answer sketch: ω_n = √(20/0.2) = 10 rad/s, ζ = 1.4/(2√(20 · 0.2)) = 0.35, the overshoot is exp(−π · 0.35/√(1 − 0.1225)) ≈ 30.9%, and the settling time is 4/(0.35 · 10) = 1.143 s. The same gains on a four-times heavier link give half the bandwidth and nearly seven times the overshoot. The learner who reports only “slower” has not modeled the damping ratio, and the learner who computes the settling time with the design’s ζ = 0.7 has used the wrong J.

(c) Remediation rule. If you cannot do (a)(iii) or (b), return to §L.1.1 and redo WE-L.1.a, then re-attempt (b) from the two formulas alone, without the worked example in front of you.

Part B — Computed Torque: The Dynamics’ Feedforward and the Linearization

§L.2 — Objects: the dynamics’ terms, the computed torque, and the linearized error

The computed-torque control law (also called feedback linearization or inverse dynamics control) is:

It has two parts:

• The feedforward M(q) q̈_d + C(q, q̇) q̇ + g(q) — the torque needed to execute the desired trajectory q_d(t) if the arm were exactly at q_d(t), computed from the model (M, C, g).
• The feedback M(q)(K_d ė + K_p e) — the correction for the error e = q_d − q, weighted by the inertia M(q) so that the closed-loop error dynamics are linear with the desired ω_n and ζ.

The linearized closed loop: substituting the control law into the plant gives:

Canceling the C\dot{q} + g terms (if the model is exact), and using q̈ = q̈_d − ë:

The error dynamics are linear, time-invariant (for constant gains K_p, K_d), and exponentially stable (the gains K_p, K_d > 0 give poles with negative real part). The nonlinearity (M(q), C(q, q̇), g(q)) is canceled by the feedforward, and the error decays exponentially with the gains’ ω_n and ζ.

§L.2.1 — Result: computed torque linearizes the closed-loop error dynamics, giving exponential convergence; PID alone gives only bounded error under the arm’s nonlinearity

Claim. For the rigid-body arm’s dynamics under computed-torque control with exact model (M, C, g), the error dynamics are ë + K_d ė + K_p e = 0 — linear and exponentially stable — regardless of the arm’s configuration, velocity, or trajectory. Under PID alone (per-joint), the error dynamics are nonlinear and coupled, and the error is bounded (by the gains and the model’s nonlinearity) but not exponentially decaying.

Proof sketch. The computed torque cancels the plant’s nonlinear terms (M, C, g) and replaces them with the desired linear dynamics q̈_d + K_d ė + K_p e; the substitution gives the error’s linear ODE. The PID alone does not cancel the nonlinearities — the M(q) varies with the configuration, the C(q, q̇) varies with the velocity, and the g(q) is a disturbance — and the error is forced by these terms, giving a bounded (but not exponentially decaying) error.

Corollary. The model error — a wrong M, C, or g — leaves a residual Δ = (M̂ − M)q̈_d + (Ĉ − C)q̇ + (ĝ − g) + ..., which acts as a disturbance on the linearized error dynamics. The residual is canceled by the feedback’s integral term (if present) only if it is constant — for a time-varying trajectory, the residual is time-varying, and the steady-state error is non-zero. The size of that error is Δ divided by the loop’s stiffness, and the stiffness is the product of the feedback gain and the inertia it acts through. Here the law is τ_fb = M̂(K_d ė + K_p e), so the loop’s torque per radian of error is M̂ K_p, and the error a residual torque Δ leaves is

not Δ/K_p. The gain K_p in computed torque is an acceleration gain — it multiplies e inside a bracket that is then multiplied by M. The M is easy to drop because it is not printed next to the gain, and dropping it understates the error by exactly 1/M. On the lesson’s arm (J = 0.05) that is a factor of 20: a 5 N·m residual leaves 0.25 rad ≈ 14.3°, not 0.0125 rad ≈ 0.7°. The same Δ/K_p formula is right for plain PD, where τ = K_p e directly — which is why the mistake survives review: it is the correct formula for the neighboring controller, quoted one section too far.

Corollary. The computed torque’s exponential stability is conditional on the model’s accuracy — a large model error (e.g., an unmodeled payload, a wrong inertia) can destabilize the loop, because the feedforward’s M̂ q̈_d is wrong by (M̂ − M)q̈_d, and the error is forced by this large time-varying term. The feedback’s gains must be large enough to dominate the residual, and the trade-off is the model’s accuracy vs. the gains’ magnitude.

§L.2.2 — Worked-example ladder

WE-L.2.a — Fully worked: computed torque for a 1-DOF arm with gravity

Input. A 1-DOF arm (a pendulum) with J = 0.05 kg·m², m = 1 kg, l = 0.5 m (the COM’s distance from the pivot), g = 9.81 m/s². The dynamics: J q̈ + m g l sin(q) = τ. Desired trajectory: q_d(t) = 0.5 sin(2πt) (a sinusoid with amplitude 0.5 rad, frequency 1 Hz).

Trace (the feedforward).

1. q_d(t) = 0.5 sin(2πt), q̇_d = π cos(2πt), q̈_d = −2π² sin(2πt).
2. The gravity torque at q_d: g(q_d) = m g l sin(q_d) = 1·9.81·0.5·sin(q_d) ≈ 4.905 sin(q_d).
3. The inertia torque: J q̈_d = 0.05 · (−2π² sin(2πt)) ≈ −0.987 sin(2πt).
4. The Coriolis: zero for a 1-DOF arm (C = 0).
5. The feedforward τ_ff = J q̈_d + g(q_d) = −0.987 sin(2πt) + 4.905 sin(0.5 sin(2πt)).

Trace (the feedback).

6. The error e = q_d − q, the rate ė = q̇_d − q̇.
7. The feedback τ_fb = J(K_d ė + K_p e) with K_p = 400 (ω_n = 20), K_d = 28 (ζ = 0.7).
8. The total τ = τ_ff + τ_fb.

Output and interpretation. The feedforward is the torque that would execute the trajectory if the arm were exactly at q_d — it includes the gravity (the 4.905 sin(q_d)) and the inertia (the −0.987 sin(2πt)) terms. The feedback corrects the error with the J-weighted gains. The combination tracks the trajectory with exponential error decay (in the ideal case, with the exact model), and the model’s accuracy determines the residual error.

Self-explanation 4. Before reading on: the feedforward’s gravity term is 4.905 sin(q_d), which is much larger than the inertia term −0.987 sin(2πt). What does this say about the relative importance of the gravity’s model vs. the inertia’s?

Answer. The gravity is ~5× larger, so an error in g (e.g., a wrong mass) is 5× more impactful than an error in J. The feedback’s gains must dominate the larger of the two errors, and a large gravity error (a wrong payload) is harder to correct than a small inertia error. The practical guidance: model the gravity accurately (the payload’s mass and the COM’s position), and the inertia can be approximate — the feedback handles the residual inertia error, but the gravity error is persistent and large.

WE-L.2.b — Completion problem

Input. Same 1-DOF arm, but the mass is wrong in the model: m̂ = 0.8 kg (actual 1.0). The trajectory is q_d = 0.5 sin(2πt), the gains are K_p = 400, K_d = 28.

Trace (final 25% as prompts).

1. The feedforward ĝ(q_d) = m̂ g l sin(q_d) = 0.8·9.81·0.5·sin(q_d) = 3.924 sin(q_d). ⟨your turn⟩
2. The residual Δg = (m − m̂) g l sin(q_d) = 0.2·9.81·0.5·sin(q_d) = 0.981 sin(q_d). ⟨your turn⟩
3. The steady-state error (assuming the loop settles): for a slowly-varying q_d, e_ss = Δg/(J K_p) — the residual torque divided by the loop’s stiffness J K_p, not by K_p alone. For a sinusoid, the error is time-varying, and the amplitude is Δg_amp / |J(s² + K_d s + K_p)| evaluated at s = jω with ω = 2π. ⟨your turn: estimate the amplitude⟩

Answers.

1. ĝ = 3.924 sin(q_d). ✓
2. Δg = 0.981 sin(q_d). ✓
3. The error’s transfer function is E(s)/Δg(s) = 1/(J(s² + K_d s + K_p)) — the J multiplies the whole bracket, because the feedback law is τ_fb = J(K_d ė + K_p e) and the plant is J q̈ = τ. With J = 0.05, K_d = 28, K_p = 400, at ω = 2π ≈ 6.28: the bracket is (400 − 39.5) + j·175.9 ≈ 360.5 + 175.9j, magnitude 402, and the denominator’s magnitude is 0.05 · 402 ≈ 20.1. The error’s amplitude is 0.981/20.1 ≈ 0.0488 rad — about 2.80°. The error is a factor of 20 smaller than the residual, not a factor of 435; the feedback reduces it but by the loop’s stiffness J K_p, and J is what makes the reduction modest rather than dramatic.

The lesson. The residual from the wrong mass is 0.981 sin(q_d), and the feedback reduces it to 0.0488 rad ≈ 2.8° (a factor of 20). Two things go wrong if the J is dropped from the denominator: the transfer function’s magnitude comes out 435 instead of 20, and the error 0.13° instead of 2.8° — a prediction that says the loop is essentially perfect when it is off by a quarter of a degree per radian of commanded motion. The J sits in the denominator for a physical reason: the same torque produces 1/J times the acceleration, so a heavy arm tolerates a given residual better than a light one at the same gain. K_p alone is not the stiffness; J K_p is. The error is small but non-zero, and it is time-varying (a sinusoid at the trajectory’s frequency) — the integral term would not eliminate it (it is not constant), and the only fix is a more accurate model (the actual mass).

WE-L.2.c — Faded: the 6-DOF arm’s model mismatch

Setup. The running case’s 6-DOF arm, with a wrong inertia matrix M̂ = αM (a scalar factor α = 1.2 — the model overestimates the inertia by 20%).

Question. What is the residual in the linearized error dynamics, and how does it affect the steady-state error for a constant-velocity trajectory (q̈_d = 0)?

You must produce the intermediate steps. The setup and the answer are given.

Answer. The residual is (M̂ − M) q̈_d = 0.2 M q̈_d. For a constant-velocity trajectory, q̈_d = 0, so the residual is zero — the inertia mismatch does not affect the constant-velocity tracking. For an accelerating trajectory (q̈_d ≠ 0), the residual is 0.2 M q̈_d, which is a time-varying disturbance proportional to the trajectory’s acceleration. The error is e ≈ (M̂ − M) q̈_d / (M K_p) = 0.2 q̈_d / K_p — the M cancels, which is worth noticing: a proportional inertia error costs a steady error proportional to the acceleration and independent of the inertia’s size. The error is non-zero and proportional to the acceleration. The symptom is a larger tracking error during the accelerating phases (the trajectory’s start and end) and a smaller error during the constant-velocity phase.

The lesson. The inertia’s mismatch affects only the accelerating trajectories (the q̈_d ≠ 0 phases), and the velocity trajectories are immune to it (the q̈_d = 0). The gravity’s mismatch, in contrast, is always present (it is g(q) regardless of the velocity), and the two errors have different time signatures. The diagnosis distinguishes them: a gravity error is constant (or slowly-varying), while an inertia error is proportional to the acceleration.

WE-L.2.d — Contrastive near-miss: computed torque without the C and g terms

Correct solution. The full computed torque τ = M̂(q̈_d + K_d ė + K_p e) + Ĉ q̇ + ĝ.

Near-miss. The partial computed torque τ = M̂(q̈_d + K_d ė + K_p e), omitting the Ĉ q̇ + ĝ terms.

Why the near-miss produces a plausible but incorrect number. The omitted Ĉ q̇ and ĝ are torques that the plant needs to execute the trajectory, and the omission means the plant must supply them from the feedback’s error — the error grows to generate the missing torque, and the steady-state error is e_ss = (C q̇ + g)/(M K_p). For a vertical arm (a large g), the error is large: the gravity torque ~5 N·m divided by the loop’s stiffness M K_p = 0.05 · 400 = 20 N·m/rad gives 0.25 rad ≈ 14.3°, and the arm droops under gravity by a quarter of a radian. Dividing by K_p = 400 alone gives 0.0125 rad ≈ 0.7° — a droop twenty times smaller than the truth, and one that would read as “the arm is fine” when it is visibly sagging. The feedback is stable (the K_p and K_d are positive), so the error is bounded, but it is non-zero and systematic (the arm sags by a fixed amount for a fixed configuration). The symptom is the sag — the arm is below the desired configuration, and the integral term (if present) eliminates it slowly.

The discrepancy, stated. The full computed torque cancels the C q̇ + g and replaces them with the desired q̈_d + K_d ė + K_p e, giving zero steady-state error (for the exact model). The partial version leaves the C q̇ + g as a disturbance on the error dynamics, and the feedback reduces it by the gain but does not eliminate it. The difference is the systematic error, and the magnitude is the disturbance divided by the gain.

Self-explanation 5. Before reading on: in the near-miss, if the arm is horizontal (g(q) = 0 for the horizontal configuration), does the gravity omission matter?

Answer. For the horizontal configuration, the gravity torque is zero (g = 0), and the omitted ĝ is zero — the near-miss’s residual is zero in that configuration. But the Coriolis term Ĉ q̇ is non-zero at non-zero velocity (it is quadratic in q̇), so the omission is still an error for moving arms, even in the horizontal plane. The gravity omission is zero at the specific configuration but non-zero elsewhere, and the Coriolis omission is non-zero whenever the arm moves — the two are different and both matter.

§L.2.3 — Comparative table: what each controller cancels

Design principle (P15.7). Put the model in the feedforward and the gains in the feedback, and never let the feedback carry a term the feedforward should have canceled. The feedback’s torque per radian is M̂ K_p, so a residual Δ it is asked to absorb becomes an error Δ/(M̂ K_p). That division is the design’s budget: a 5 N·m unmodeled gravity torque on the J = 0.05 arm at K_p = 400 costs 0.25 rad, and no amount of gain tuning removes it without eating the phase margin. Model what is large and slowly varying (gravity, payload), and let the feedback handle what is small and fast.

§L.2.4 — Concept check

Concept check (displaces M5). A learner says: “computed torque is PID with the plant’s terms added in — the extra terms improve the tracking, but the gains still do the work.”

Answer. The learner has failed to model feedback linearization: the extra terms are not an improvement on the gains, they are a change of the plant. Substituting the law into M q̈ + C q̇ + g = τ cancels M, C, and g identically and leaves ë + K_d ė + K_p e = 0, an equation with no plant parameters in it at all — the same error dynamics at every configuration, every velocity, and every trajectory. The exponential convergence comes from that cancellation, not from the gains: the gains only choose where the poles sit once the plant is gone. The bug class is residual misattribution: the learner sees a steady-state lag under computed torque and raises K_p, when the lag is Δ/(M K_p) from a model error and the fix is a better M̂ or ĝ. The gains are a poor lever on a residual — the M in the denominator means the same gain buys a fifth of the correction on a heavy arm.

§L.2.5 — Consolidation checkpoint 2

(a) You should now be able to: (i) write the computed-torque law and separate it into its feedforward and its feedback parts; (ii) show by substitution that the exact-model closed loop is ë + K_d ė + K_p e = 0, and state what the cancellation depends on; (iii) compute the steady-state error from a residual as Δ/(M K_p), and say why it is not Δ/K_p; (iv) predict the time signature of a gravity mismatch (constant) against an inertia mismatch (proportional to q̈_d).

(b) Transfer probe. The 1-DOF pendulum’s model has the right mass but the wrong link length: l̂ = 0.4 m against the true l = 0.5 m, with J = 0.05, m = 1, K_p = 400, K_d = 28, and a constant setpoint q_d = 0.3 rad. Does the integral term remove the resulting error? Give the error’s size. Answer sketch: the residual is Δg = m g (l − l̂) sin(q_d) = 1 · 9.81 · 0.1 · sin(0.3) ≈ 0.290 N·m, and the error is Δg/(J K_p) = 0.290/20 ≈ 0.0145 rad ≈ 0.83°. For a constant setpoint the residual is constant, so an integral term does drive it to zero — the 0.0145 rad is the PD-only error. The probe’s point is the pair of distinctions: J K_p is the stiffness (not K_p), and “the integral removes it” is a statement about a constant residual, which a trajectory would not give you. A learner who answers 0.0007 rad has dropped the J; a learner who says the integral cannot help has confused this case with the time-varying one.

(c) Remediation rule. If you cannot do (a)(iii) or (b), return to §L.2.1’s first corollary and WE-L.2.b, then redo (b) by writing the feedback torque per radian of error before substituting any numbers.

Part C — Trajectory Generation: Minimum-Jerk, Polynomial, and the Speed–Error Trade-Off

§L.3 — Objects: the trajectory’s constraints and the three profiles

A trajectory q_d(t) for t ∈ [0, T] is specified by its endpoints q_d(0) = q_0, q_d(T) = q_f, and its smoothness — how many derivatives are continuous and zero at the endpoints.

The three common profiles:

1. Linear (the naive) — q_d(t) = q_0 + (q_f − q_0) t/T. The velocity is a step (discontinuous at t = 0 and t = T), and the acceleration is an impulse (infinite at the endpoints). The tracking error is large (the infinite acceleration cannot be followed), and the actuator saturates.

2. Cubic polynomial — q_d(t) = a_0 + a_1 t + a_2 t² + a_3 t³, with the coefficients chosen to satisfy q_d(0) = q_0, q_d(T) = q_f, q̇_d(0) = q̇_d(T) = 0. The velocity is continuous (a parabola), and the acceleration is continuous (linear), but the jerk is a step (discontinuous at the endpoints). The error is smaller than linear’s, but the jerk’s discontinuity causes vibration in a flexible arm.

3. Minimum-jerk (quintic) — q_d(t) = q_0 + (q_f − q_0)(10 (t/T)³ − 15 (t/T)⁴ + 6 (t/T)⁵). The velocity and acceleration are continuous and zero at the endpoints, and the jerk is continuous but not zero there — it is ±60 Δq/T³ at both ends, its extreme value. What the quintic buys over the cubic is that the jerk has no step: the cubic’s jerk jumps discontinuously at t = 0 and t = T, the quintic’s rises from a finite value. That distinction is the whole point of the profile and it is worth stating precisely, because “all derivatives zero at the endpoints” is a description of a seventh-order polynomial, not this one. The peak velocity is 1.875 (q_f − q_0)/T, and the peak acceleration is 5.773 (q_f − q_0)/T². The error is the smallest of the three (the trajectory is the easiest to follow).

The speed–error trade-off: the tracking error under computed torque is dominated by the residual, and the error a residual torque produces is Δ/(M K_p) — the residual divided by the loop’s stiffness, not by the gain alone. The residual is proportional to the acceleration (the M q̈_d term) and the velocity (the C q̇_d term). The minimum-jerk trajectory’s peak acceleration is 5.773 Δq/T², which scales as 1/T² — a faster trajectory (smaller T) has a quadratically larger acceleration, and the error grows quadratically with the speed.

§L.3.1 — Result: the tracking error scales as 1/T² for a minimum-jerk trajectory under computed torque with a model mismatch, and the speed’s limit is the actuator’s torque’s saturation

Claim. For a minimum-jerk trajectory of duration T between configurations q_0 and q_f, the peak acceleration is 5.773 |Δq|/T², and the steady-state tracking error under computed torque with an inertia mismatch ΔM = M̂ − M is e_ss ≈ (|ΔM|/M) · 5.773 |Δq| / (T² K_p). The error scales as 1/T², and it is larger for faster trajectories (smaller T) and larger Δq. Note the |ΔM|/M — a relative mismatch, dimensionless — rather than |ΔM| alone: the mismatch is a torque, the M K_p converts it to an angle, and the M in the numerator cancels. A 1% inertia error costs the same angle on a heavy arm as on a light one at the same gain and the same acceleration.

Proof sketch. The minimum-jerk polynomial’s second derivative is (q_f − q_0)/T² · 60 (τ − 3τ² + 2τ³) with τ = t/T, whose peak magnitude is 5.773 |Δq|/T² at τ = (3 − √3)/6 ≈ 0.211 and τ ≈ 0.789. (Write the cubic with −3τ², not −2τ²: the −2 version peaks at 8.889, not 5.773, so the formula and the number quoted from it would disagree.) The residual’s contribution to the error is (M̂ − M) q̈_d / (M K_p) — the inertia mismatch’s effect on the feedforward, divided by the loop’s stiffness — and substituting the peak acceleration gives the displayed formula.

Corollary. The actuator’s torque limit τ_max imposes a minimum T (a maximum speed), because the peak torque τ_peak ≈ M · 5.773 |Δq|/T² + g must be ≤ τ_max. The time-optimal trajectory is the one with τ_peak = τ_max, and its duration is T_min = √(5.773 M |Δq| / (τ_max − g)) — the faster is limited by the actuator, not the trajectory’s shape.

Corollary. The error’s 1/T² scaling means that a 10× faster trajectory (a 10× smaller T) has a 100× larger error. The control’s gains must increase by 100× to maintain the error, and the higher gains are closer to the stability limit — the speed’s limit is the combination of the actuator’s saturation and the gains’ stability.

§L.3.2 — Worked-example ladder

WE-L.3.a — Fully worked: the minimum-jerk trajectory for a single joint

Input. A single joint from q_0 = 0 to q_f = 0.5 rad in T = 1 s.

Trace.

1. Δq = 0.5.
2. Position: q_d(t) = 0.5 (10 (t)³ − 15 (t)⁴ + 6 (t)⁵) for t ∈ [0, 1].
3. Velocity: q̇_d(t) = 0.5 (30 t² − 60 t³ + 30 t⁴) = 15 t²(1 − t)². Peak at t = 0.5: q̇_peak = 15·0.25·0.25 = 0.9375 rad/s. (1.875 · 0.5/1 = 0.9375. ✓)
4. Acceleration: q̈_d(t) = 0.5 (60 t − 180 t² + 120 t³) = 30 t(1 − 3t + 2t²). Peak at t ≈ 0.211 and 0.789: q̈_peak ≈ 5.773 · 0.5/1² = 2.887 rad/s².
5. Jerk: q⃛_d(t) = 0.5 (60 − 360 t + 360 t²) = 30 − 180 t + 180 t². At t = 0: 30; at t = 1: 30 — non-zero at the endpoints (the jerk is continuous but not zero at the endpoints; the derivative of jerk — the snap — is zero). The jerk is maximal at t = 0, 1 and minimal at t = 0.5 (zero? no — 180·0.25 − 180·0.5 + 30 = 45 − 90 + 30 = −15, non-zero).

Output and interpretation. The minimum-jerk trajectory’s position is the standard quintic, its velocity peaks at 0.9375 rad/s at the midpoint, its acceleration peaks at 2.887 rad/s² at t ≈ 0.211 and 0.789, and its jerk is finite and continuous (the quintic is the lowest-degree polynomial with continuous jerk).

Self-explanation 6. Before reading on: the peak velocity is 0.9375 rad/s, and the average velocity is Δq/T = 0.5 rad/s. What is the ratio, and why is it not 1?

Answer. The ratio is 0.9375/0.5 = 1.875 — the minimum-jerk trajectory’s peak velocity is 1.875× the average. The ratio is not 1 because the trajectory accelerates and decelerates smoothly, and the peak is higher than the average to compensate for the slow start and end. The linear trajectory has peak = average = 0.5 (a step velocity), and the smooth trajectory has peak > average (the smoothness’s cost). The 1.875 is a fixed property of the minimum-jerk profile.

WE-L.3.b — Completion problem

Input. Same joint, T = 0.5 s (twice as fast).

Trace (final 25% as prompts).

1. Δq = 0.5, T = 0.5. ⟨your turn⟩
2. q̇_peak = 1.875 Δq/T = 1.875 · 0.5/0.5 = 1.875 rad/s. ⟨your turn⟩
3. q̈_peak = 5.773 Δq/T² = 5.773 · 0.5/0.25 = 11.546 rad/s². ⟨your turn⟩
4. The error under an inertia mismatch of ΔM = 0.01 kg·m² on an arm with M = 0.05 and K_p = 400: e_ss ≈ (ΔM/M) · q̈_peak / K_p = (0.01/0.05) · 11.546 / 400 ≈ 5.77e-3 rad. ⟨your turn⟩
5. Compare to the T = 1 case: e_ss(T=1) ≈ (0.01/0.05) · 2.887 / 400 ≈ 1.44e-3 rad. ⟨your turn: ratio?⟩

Answers.

1. Δq = 0.5, T = 0.5. ✓
2. 1.875 rad/s — twice the T = 1 case. ✓
3. 11.546 rad/s² — four times the T = 1 case. ✓
4. 5.77e-3 rad. The mismatch is ΔM/M = 20% of the inertia, so the residual torque is 0.2 M q̈, and dividing by the stiffness M K_p leaves 0.2 q̈/K_p = 0.2 · 11.546/400. Note that the M cancels — writing ΔM q̈/K_p instead gives 2.89e-4, smaller by the factor 1/M = 20, and that is the value a learner who forgets the M in the loop’s stiffness will get.
5. 1.44e-3 rad; the ratio is 4 — the error is 4× larger for the 2× faster trajectory (the 1/T² scaling). The ratio is 4 under either formula; only the magnitude moves, which is why the scaling conclusion survives the bug and the absolute error does not.

The lesson. The velocity scales as 1/T and the acceleration as 1/T², so a 2× faster trajectory has a 2× higher velocity and a 4× higher acceleration. The error scales as 1/T² (the acceleration’s effect), so the 2× faster trajectory has a 4× larger error. The actuator’s peak torque is also 4× larger, and the saturation limit is the binding constraint.

WE-L.3.c — Faded: the cubic polynomial’s jerk

Setup. The cubic polynomial for the same q_0 = 0, q_f = 0.5, T = 1.

Question. What are the peak velocity and peak acceleration? What is the jerk at t = 0 and t = 1?

You must produce the intermediate steps. The setup and the answer are given.

Answer. The cubic with zero endpoint velocities is q_d(t) = 0.5 (3 t² − 2 t³) (the standard smoothstep). Velocity: q̇_d = 0.5 (6 t − 6 t²) = 3 t(1 − t). Peak at t = 0.5: 3 · 0.25 = 0.75 rad/s (1.5× the average 0.5, vs. the minimum-jerk’s 1.875×). Acceleration: q̈_d = 0.5 (6 − 12 t) = 3 − 6 t, with q̈_peak = 3 at t = 0 (−3 at t = 1). Jerk: q⃛_d = −6 — a constant (a step at the endpoints, from 0 before t = 0 to −6 at t > 0).

The lesson. The cubic has a smaller peak velocity (0.75 vs. 0.9375) and a smaller peak acceleration (3 vs. 2.887 — actually larger!), but its jerk is a step (discontinuous at the endpoints). The jerk’s discontinuity excites the arm’s resonances (the flexible modes), and the tracking error in practice is larger than the minimum-jerk despite the smaller peak acceleration. The minimum-jerk is the better choice for a flexible arm (the jerk’s continuity is the key), while the cubic is adequate for a rigid arm.

WE-L.3.d — Contrastive near-miss: the linear trajectory with a PD controller

Correct solution. A minimum-jerk (or cubic) trajectory with the computed-torque controller.

Near-miss. A linear trajectory (a step in velocity at t = 0 and t = T) with a PD controller.

Why the near-miss produces a plausible but incorrect number. The linear trajectory’s velocity step requires an infinite acceleration at the endpoints, and the PD controller cannot produce the infinite torque — the actuator saturates, and the arm lags behind the linear trajectory by a large error (the saturation’s effect). The tracking error is large at the start (the arm accelerates from rest) and at the end (the arm decelerates), and the intermediate phase (constant velocity) is tracked with zero error (the acceleration is zero). The error’s time signature is the double pulse (one at each endpoint), and it is systematic (always present at the endpoints).

The discrepancy, stated. The linear trajectory’s acceleration is an impulse at t = 0 and t = T, and the PD controller’s torque is finite (bounded by the gains and the saturation), so the impulse is not tracked — the arm lags. The minimum-jerk trajectory’s acceleration is finite and continuous, and the computed-torque controller’s feedforward anticipates it (the M q̈_d term), so the tracking is accurate. The two differences — the trajectory’s smoothness and the controller’s feedforward — are both necessary for good tracking.

Self-explanation 7. Before reading on: in the near-miss, if the PD gains are increased to reduce the endpoint error, what happens to the intermediate phase?

Answer. The higher gains reduce the endpoint error (the K_p dominates the saturation’s effect more), but the intermediate phase’s error is zero (the q̈_d = 0 in the constant-velocity phase), and the higher gains amplify the noise (the derivative’s high-frequency amplification) and reduce the stability margin. The higher gains cannot fix the endpoint error without destabilizing the loop, and the fundamental issue is the trajectory’s smoothness — the linear trajectory requires the infinite torque, which no finite gain can produce.

§L.3.3 — Comparative table: the three trajectory profiles

Design principle (P15.8). The duration is a plant parameter, not a style choice. The peak torque is M · 5.773 |Δq|/T² + g, so the actuator’s limit gives T_min = √(5.773 M |Δq|/(τ_max − g)) before the trajectory’s shape is even a question. Choose T from τ_max, M, and the motion’s size, then choose the profile from the arm’s flexibility — and check the second choice against the first, because the profile changes the peak acceleration only by the constant (5.773 for the quintic, 6 for the cubic), not by its 1/T² scaling. What a smoother profile buys is jerk, and jerk is not in the torque budget.

§L.3.4 — Concept check

Concept check (displaces M3). A learner says: “the tracking error under a trajectory is steady, so I’ll add integral action and drive it to zero.”

Answer. The learner has failed to model what the integral integrates against. The integral eliminates an error only if the error is constant; on a trajectory the error is forced by the residual Δ(t) = (M̂ − M)q̈_d + …, which varies with the acceleration, so the integral’s contribution is a lagging copy of a moving target and the error stays non-zero. What sets its size is the loop’s stiffness, e_ss ≈ (|ΔM|/M) · 5.773|Δq|/(T² K_p): halving T quadruples the error, and the integral does not change that scaling at all. Worse, a large K_i under actuator saturation is exactly the windup condition from §L.4. The bug class is a fixed integral gain chasing a moving residual: the learner raises K_i, the error grows because the trajectory got faster, and the loop overshoots at the endpoints. The fixes are the ones the lesson gives — a better model, a longer T, or a higher K_p — and the diagnosis is the error’s time signature: proportional to q̈_d means inertia mismatch, roughly constant means gravity.

§L.3.5 — Consolidation checkpoint 3

(a) You should now be able to: (i) state each profile’s continuity class and the derivative at which it stops being continuous; (ii) compute a minimum-jerk trajectory’s peak velocity 1.875 Δq/T and peak acceleration 5.773 Δq/T², and the endpoint jerk ±60 Δq/T³; (iii) explain why the tracking error scales as 1/T² and why the M cancels out of (ΔM/M) · q̈/K_p; (iv) derive the actuator’s minimum duration T_min and say what binds it.

(b) Transfer probe. A 0.2 rad joint move must complete in T = 0.2 s on an arm with M = 0.05 kg·m² and a gravity load of 0.5 N·m. Give the minimum-jerk peak velocity, peak acceleration, and endpoint jerk; then state whether a cubic profile is acceptable if the link is flexible. Answer sketch: q̇_peak = 1.875 · 0.2/0.2 = 1.875 rad/s, q̈_peak = 5.773 · 0.2/0.04 = 28.9 rad/s², and the jerk at both ends is 60 · 0.2/0.008 = 1500 rad/s³ — finite and continuous, but not zero, which is the point that separates the quintic from a seventh-order polynomial. The peak torque is 0.05 · 28.9 + 0.5 ≈ 1.94 N·m, comfortably inside a 10 N·m actuator, so the duration is not actuator-bound here. A cubic is not acceptable on a flexible link: its acceleration peaks at 6 Δq/T² = 30 rad/s², slightly higher than the quintic’s, and its jerk steps from 0 to −6Δq/T³ = −150 rad/s³ at t = 0, which is the discontinuity that excites the flexible mode. The learner who says “the cubic is smoother because it has a smaller peak” has compared the wrong derivative.

(c) Remediation rule. If you cannot do (a)(ii) or (b), return to §L.3 and WE-L.3.a, then re-derive the endpoint jerk from the quintic’s third derivative rather than quoting the coefficient.

Part D — Discretization and Real-Time Constraints

§L.4 — Objects: the sample rate, the discrete PID, and the real-time loop

The discrete PID is the sampled version of the continuous PID, with the control law computed at discrete timesteps t_k = k T_s, where T_s is the sample period (the control loop’s period):

The integral is a sum (a Riemann approximation of the integral), and the derivative is a backward difference (a first-order approximation). The sample rate f_s = 1/T_s determines the accuracy of the approximations and the stability of the closed loop.

The real-time loop must complete one iteration (the sensor read, the control computation, the actuator write) within T_s, and the deadline is the hard constraint — a missed deadline (a late iteration) is a discontinuity in the control, and a persistent miss is instability.

The three real-time issues:

1. Sample rate vs. stability — the discrete loop’s stability depends on ω_n T_s. The rule of thumb is ω_n T_s ≤ 0.1 (the sample rate is 10× the closed-loop bandwidth), which gives a negligible phase lag from the sampling; a lower ω_n T_s (a higher sample rate) is safer, and a higher ω_n T_s (a lower sample rate) reduces the phase margin and can destabilize the loop.
2. Integral windup — the integral accumulates the error, and when the actuator saturates (the torque is capped at τ_max), the integral keeps growing (the error is still non-zero, but the actuator cannot reduce it), and the overshoot after the saturation is large. The fix is anti-windup: stop the integral’s accumulation when the actuator is saturated (a clamping or back-calculation scheme).
3. Derivative noise — the derivative’s K_d(e_k − e_{k-1})/T_s is a high-frequency amplification: the difference e_k − e_{k-1} is noise-dominated at high k, and the 1/T_s factor amplifies it. The fix is a filtered derivative K_d s/(1 + s/ω_f) (from WE-L.1.d), or a lower K_d with a higher K_p, or a low-pass filter on the measurement before the difference.

§L.4.1 — Result: the discrete PID’s stability requires ω_n T_s ≪ 1, and the sampling’s phase lag reduces the phase margin by ω_c T_s/2 radians

Claim. For a continuous-time loop with crossover frequency ω_c and phase margin PM, the discrete-time loop with sample period T_s has phase margin PM − ω_c T_s/2 (approximately), and the loss is the sampling’s zero-order hold’s phase lag. For ω_c T_s = 0.1, the lag is 0.05 rad ≈ 2.9° — negligible; for ω_c T_s = 1, the lag is 0.5 rad ≈ 28.6° — significant, and the loop can become unstable if the continuous PM is less than 28.6°.

Proof sketch. The zero-order hold (the digital-to-analog converter’s sample-and-hold) has a transfer function (1 − e^{−s T_s})/s, whose phase lag at frequency ω is −ω T_s/2 for ω T_s ≪ 1. The phase margin is reduced by this lag, and the stability is lost when PM < ω_c T_s/2.

Corollary. The rule of thumb f_s = 10 · f_c (the sample rate is 10× the crossover frequency) gives a phase lag of 0.05 rad ≈ 2.9° — a small reduction in the margin. The real-time constraint is the sample rate’s cost — the higher f_s is safer but requires a faster processor and a tighter deadline.

Corollary. The integral windup and the derivative noise are discrete-time issues that the continuous-time PID does not have — the integral is a sum (not an integral), and the derivative is a difference (not a derivative), and the sum and difference have different numerical properties. The continuous design’s gains must be adjusted for the discrete implementation, and the adjustments are the anti-windup and the derivative filter.

§L.4.2 — Worked-example ladder

WE-L.4.a — Fully worked: the discrete PID at T_s = 1 ms

Input. A single joint with J = 0.05, K_p = 20, K_d = 1.4, K_i = 5, T_s = 0.001 s, setpoint q_d = 1.0, initial q = 0, q̇ = 0.

Trace (first three steps).

1. k = 0: e_0 = 1.0 − 0 = 1.0. ∫e ≈ T_s e_0 = 0.001. ė_0 = 0 (no previous). τ_0 = 20·1.0 + 5·0.001 + 1.4·0 = 20.005. The joint accelerates; the simulation updates q, q̇.
2. k = 1: q_1 ≈ q_0 + q̇_0 T_s + ½ (τ_0/J) T_s² ≈ 0 + 0 + ½ · 400 · 1e-6 = 2e-4. e_1 = 1.0 − 2e-4 ≈ 0.9998. ∫e ≈ 0.001 + 0.001·0.9998 ≈ 0.002. ė_1 = (e_1 − e_0)/T_s = (0.9998 − 1.0)/0.001 = -0.2. τ_1 ≈ 20·0.9998 + 5·0.002 + 1.4·(-0.2) ≈ 19.996 + 0.01 − 0.28 = 19.726.
3. k = 2: the joint has accelerated to q̇ ≈ τ_0/J · T_s ≈ 400 · 0.001 = 0.4 rad/s; q_2 ≈ 2e-4 + 0.4·0.001 = 6e-4. e_2 ≈ 0.9994. ∫e ≈ 0.003. ė_2 = (0.9994 − 0.9998)/0.001 = -0.4. τ_2 ≈ 20·0.9994 + 5·0.003 + 1.4·(-0.4) ≈ 19.988 + 0.015 − 0.56 = 19.443.

Output and interpretation. The torque decreases as the joint accelerates (the derivative term’s negative contribution −K_d ė damps the approach to the setpoint), and the integral accumulates slowly (the K_i T_s e is small per step). The discrete steps’ behavior approximates the continuous response, and the sample rate 1 ms (1 kHz) is 314× the ω_n = 20 rad/s ≈ 3.18 Hz — far inside the rule of thumb (ω_n T_s = 0.02, which is a factor of 5 below the 0.1 threshold). Say 314×, not 10×: the ratio of a rate in Hz to a frequency in Hz is 1000/3.18, and the 10× in the rule of thumb is a statement about ω_n T_s, a dimensionless product, not about the ratio of the two rates. The two happen to be related by 10 · ω_n/(2π), which is why the confusion is easy — but quoting 10× for a ratio of 314 is a factor-of-31 error sitting inside a sentence whose conclusion (the rate is comfortable) is right.

Self-explanation 8. Before reading on: in step 3, ė_2 = -0.4 — the derivative term’s contribution is K_d ė_2 = 1.4 · (-0.4) = -0.56 — a negative torque. Why is the derivative term negative when the error is positive and decreasing?

Answer. The error is positive (the joint is below the setpoint) but decreasing (the joint is accelerating toward the setpoint, so the error’s rate is negative). The derivative term K_d ė resists the error’s rate — a negative rate gives a negative contribution, which reduces the total torque. This is the damping: the derivative reduces the torque when the error is shrinking fast, preventing overshoot. If the derivative were absent, the torque would stay at K_p e (which is large), and the joint would overshoot the setpoint. The negative derivative’s contribution is the damping’s effect, and it is essential for a well-behaved response.

WE-L.4.b — Completion problem

Input. Same joint, but T_s = 0.1 s (a slow sample rate, 100× slower).

Trace (final 25% as prompts).

1. ω_n T_s = 20 · 0.1 = 2. ⟨your turn: is this within the rule of thumb ≤ 0.1?⟩
2. The crossover frequency of K_p/(J s² + K_d s) with K_p = 20, K_d = 1.4, J = 0.05 is ω_c ≈ 13.0 rad/s — not ω_n. (For this plant ω_c is below ω_n because the integrator’s −20 dB/decade reaches unity gain before the second-order break.) The ZOH’s phase lag there is ω_c T_s/2 = 13.0 · 0.1/2 = 0.65 rad ≈ 37°. ⟨your turn: is this larger or smaller than the continuous PM?⟩
3. The loop’s stability: the continuous PM for ζ = 0.7 is 65°. The discrete PM ≈ 65 − 37 = 28° — reduced, still positive. ⟨your turn: what does a reduced but positive PM mean for the time-domain response?⟩
4. The response: this analysis says the loop is stable with degraded margin, and the lab shows something else. ⟨your turn: what is the discrepancy?⟩

Answers.

1. ω_n T_s = 2, much larger than 0.1 — the sample rate is too slow.
2. 37° — comparable to the continuous PM (65°), so the reduction is severe.
3. The discrete PM ≈ 28° is degraded but not marginal — the loop is stable, with a larger overshoot and a slower settling than the continuous design.
4. The lab’s dt = 0.1 run does not oscillate — it diverges. The linear analysis in steps 1–3 is about exact ZOH sampling of the continuous loop, whose state matrix has |eig| = 0.247 and is stable. The lab’s plant_step is forward Euler, whose state matrix has |eig| = 1.483, and the sequence grows without bound (q reaches −26 within the first second and the last samples are 10²¹). The “sign changes” the lab counts are not an oscillation: they are the alternating sign of a monotonically growing sequence. The two failures have different causes — sampling at T_s = 0.1 costs you margin, forward Euler at T_s = 0.1 costs you stability — and the lab’s numbers come from the second.

The lesson. The sample rate must be ≥ 10× the closed-loop bandwidth, and a slower rate reduces the phase margin. But “the sample rate destabilized it” and “the integrator destabilized it” are different claims with different fixes, and the symptom that distinguishes them is the shape of the divergence. A sampled stable loop with a degraded margin oscillates — bounded, periodic, at a frequency set by the sampling. A forward-Euler discretization of an unstable step diverges — unbounded, growing by a fixed factor per step. Before diagnosing “the sample rate is too slow”, check the growth: if the amplitude grows geometrically, the culprit is the integrator, and raising the sample rate will fix it only because it also shrinks the Euler step. The honest statement of the rule of thumb is ω_n T_s ≤ 0.1 and an integrator whose stability region contains the loop’s poles; either alone is a half-condition. The symptom of the sampling-induced case is a high-frequency oscillation near the Nyquist frequency, which is distinct from the gain-induced oscillation (at ω_n) and the derivative-noise chatter (broadband).

WE-L.4.c — Faded: the integral windup

Setup. Same joint, K_i = 100 (a large integral), a step setpoint q_d = 1.0, and an actuator saturation τ_max = 10 N·m.

Question. What is the torque’s behavior, and how does the integral wind up?

You must produce the intermediate steps. The setup and the answer are given.

Answer. The initial torque τ_0 ≈ K_p e_0 = 20·1.0 = 20 > τ_max = 10, so the torque is saturated at 10 N·m. The error is still positive, and the integral accumulates it: ∫e grows by T_s e ≈ 0.001 · 1 = 0.001 per step, and after 1 s (1000 steps), ∫e ≈ 1.0, so K_i ∫e ≈ 100 · 1.0 = 100 — 10× larger than the saturation limit! When the joint reaches the setpoint (e → 0), the integral is still 100, and the torque is still K_i ∫e ≈ 100 — the joint overshoots wildly. The overshoot is the windup’s symptom, and the fix is anti-windup: stop the integral’s accumulation when τ = τ_max (the actuator is saturated).

The lesson. The windup is the integral’s accumulation during saturation, and the symptom is a large overshoot (the integral’s stored energy is released after the saturation ends). The anti-windup is the clamping (K_i’s accumulation paused during saturation) or the back-calculation (the integral’s value is reduced by the saturation’s excess). The integral term’s large gain is dangerous with saturation, and the anti-windup is essential for real actuators.

WE-L.4.d — Contrastive near-miss: the high-gain PID without saturation

Correct solution. A moderate gain with anti-windup and a derivative filter.

Near-miss. A high gain (K_p = 1000) with no anti-windup and no filter.

Why the near-miss produces a plausible but incorrect number. The high gain gives a fast response and a small tracking error in simulation (where the actuator is ideal), and the designer is satisfied. In reality, the actuator saturates at τ_max, the integral winds up, the joint overshoots, and the derivative amplifies the noise — the high gain is unstable in practice. The simulation does not show the saturation or the noise (they are idealized away), and the designer who tunes in simulation discovers the instability at deployment. The symptom is the large overshoot and the chatter at deployment, and the fix is the lower gain and the anti-windup.

The discrepancy, stated. The simulation with the ideal actuator and the noiseless sensor hides the real issues, and the high gain is safe in the simulation but unsafe in the real system. The design must be done with the saturation and the noise included, and the gains must be tuned for the worst-case actuator and the noisiest sensor.

Self-explanation 9. Before reading on: if the simulation includes the saturation and the noise, does the high gain still show the instability?

Answer. Yes — the saturation and the noise are the sources of the instability, and the simulation with them shows the overshoot (the saturation’s effect) and the chatter (the noise’s effect). The idealized simulation hides them, and the realistic simulation shows them. The design’s robustness requires the realistic simulation, and the gains must be chosen for it — the idealized simulation’s gains are optimistic and unsafe.

§L.4.3 — Comparative table: the discrete-time realizations

Design principle (P15.9). Diagnose a discrete-time failure by the shape of the divergence, not by the sample rate. A sampled stable loop with a degraded margin oscillates — bounded, periodic, at a frequency set by the sampling. A forward-Euler discretization of an unstable step diverges — unbounded, geometric. At T_s = 0.1 the exact-ZOH loop has |eig| = 0.247 and is stable while the lab’s forward-Euler step has |eig| = 1.483 and is not, so the run’s q reaches −26 within a second and 10²¹ by the end. Raising the sample rate fixes the second because it shrinks the Euler step, which is why the wrong diagnosis survives: the cure works for a reason other than the one claimed.

§L.4.4 — Concept check

Concept check (displaces M4). A learner says: “the joint chatters, so the derivative gain is too high — I’ll lower K_d and raise K_p to keep the response fast.”

Answer. The learner has modeled the derivative as a damping that adds noise and has not modeled it as a high-pass filter with gain K_d s. The chatter is not K_d being large in absolute terms; it is K_d · ω_noise against a signal of order 1 N·m, and at ω_noise ≈ 1000 rad/s even K_d = 1.4 produces 1400 N·m of noise torque. Lowering K_d reduces the amplification, but raising K_p in compensation restores the bandwidth that the derivative was damping, and the loop now has less phase margin than before — so the chatter trades for ringing. The bug class is a first-order difference masquerading as a derivative: (e_k − e_{k−1})/T_s has gain rising without bound with frequency, and no choice of gain makes it a differentiator over the band where the encoder noise lives. The fix is structural — the filtered derivative K_d s/(1 + s/ω_f) with ω_f ≈ 10 ω_n, which attenuates above ω_f and costs only arctan(ω_c/ω_f) ≈ 5.7° of phase at ω_c.

§L.4.5 — Consolidation checkpoint 4

(a) You should now be able to: (i) write the discrete PID and say which term is a sum, which is a difference, and what each approximation costs; (ii) apply ω_n T_s ≤ 0.1 and compute the phase-margin loss ω_c T_s/2 at a given rate; (iii) distinguish a sampling-induced oscillation from an integrator-induced divergence by the growth’s shape; (iv) name the fix for each of windup, derivative noise, and a marginal sample rate.

(b) Transfer probe. A loop runs at f_s = 500 Hz with ω_c = 60 rad/s and a continuous phase margin of 50°. A colleague proposes halving the rate to 250 Hz to free processor time, and separately proposes adding K_i = 200 to remove a small steady offset. Which proposal is safe, and what would you check first? Answer sketch: halving the rate doubles ω_c T_s from 60/500 = 0.12 to 0.24, and the margin loss goes from 0.06 rad ≈ 3.4° to 0.12 rad ≈ 6.9°, leaving about 43° — degraded but safe, and the check is whether the resulting overshoot is acceptable, not whether the loop survives. The integral proposal is the dangerous one: K_i = 200 is forty times the K_i = 5 of the worked example and twice the K_i = 100 that already wound the loop to ten times the saturation limit, so the first check is whether the actuator saturates at all during the transient. A learner who approves the rate change and waves through the integral has ranked the two risks backwards: the sampling costs margin in a bounded, computable way, and the windup is unbounded.

(c) Remediation rule. If you cannot do (a)(iii) or (b), return to §L.4.1 and WE-L.4.b, then redo (b) by computing ω_c T_s for both rates before judging either proposal.

Part E — Failure Taxonomy, Lab, Cheat Sheet, Exercises, Exit Test

§L.5 — Failure taxonomy

§L.6 — The lab

§L.6.1 — Task

Implement a per-joint PID, a computed-torque controller, and a minimum-jerk trajectory for the running case’s 6-DOF arm; verify the PD’s closed-loop ω_n and ζ against the analytic prediction; demonstrate the integral windup, the derivative noise, and the gain-induced instability; and compare the PID’s and the computed torque’s tracking errors.

§L.6.2 — Conditions

Python 3.10+, PyTorch 2.2+, CPU. Seed 42, dtype = float64. Runtime under 5 minutes. The arm’s dynamics: a simplified M = diag([0.05, ...]) (6 joints, each with the same inertia for simplicity), C = 0 (small velocities), g = 0.5 sin(q) (each joint’s gravity). The trajectory: a minimum-jerk from q_0 = 0 to q_f = [0.5, 0.5, −0.5, 0.5, −0.5, 0.5] over T = 1 s. The sample rate: f_s = 1 kHz.

§L.6.3 — Controls held fixed

The seed, dtype, dynamics, trajectory, and sample rate are fixed. The gains are varied across the tests.

§L.6.4 — Metrics table

§L.6.5 — Pre-registered hypotheses

• H1. The PD’s pd_overshoot ≈ exp(−πζ/√(1−ζ²)) for ζ = 0.7 ≈ 4.6%, and pd_settling_time ≈ 4/(ζω_n) = 0.286 s for ω_n = 20. The prediction is the pair of values. Measure the overshoot against the reached steady state (0.979 rad, drooping under gravity), not against the setpoint 1.0; the lab prints both and they differ by a factor of two.
• H2. The pid_ss_error is ≈ 0 (the integral eliminates the steady-state error), while the PD’s is ≈ (d + g sin q_ss)/K_p. The prediction is the pair. The gravity term belongs in the prediction: the PD’s droop is g/K_p = 0.025 rad from gravity alone, and the disturbance adds another 0.025.
• H3. The ct_tracking_error is < 1e-3 (the computed torque’s exponential convergence with the exact model), while the pid_tracking_error is > 1e-2 (the PID’s bounded error). The prediction is the pair. The separation needs a plant the PID is actually bad at: at g = 0.5 and K_p = 400 the two agree to a factor of 5.8 and both sit below 1e-3 (9.5e-5 against 5.5e-4). The lab uses g = 3.0 and K_p = 100, where CT gives 2.8e-4 and PID gives 1.3e-2.
• H4. The windup_overshoot with K_i = 200 and no anti-windup is > 50%, while with anti-windup on it is < 50%. The prediction is the pair. K_i = 20 is too small to wind up meaningfully (10%); the effect needs a gain large enough that K_i ∫e outgrows the saturation limit within the rise time.
• H5. The discrete_instability at T_s = 0.1 is true, while at T_s = 0.001 it is false. The prediction is the pair — but the cause is the forward-Euler integrator, not the sample rate: exact ZOH sampling of the same loop at T_s = 0.1 has |eig| = 0.247 and is stable, while forward Euler has |eig| = 1.483. The lab’s job is to show that the two are distinguishable, and the discriminator is geometric growth (a factor of 2.6·10³¹ over the run) against the bounded oscillation a degraded phase margin produces.

§L.6.6 — The break-it sequence

1. Integral windup (silent). Set K_i = 200 with τ_max = 10 saturation and anti-windup off. Predicted symptom: no exception; the torque saturates, the integral grows unchecked, and the joint overshoots by > 50%. Actual: 61.6% — silent. With anti-windup on the same run gives 40.9%, so the gate is the difference between the two, not the absolute number.
2. Derivative noise (silent). Add 0.01 rad of Gaussian noise to the encoder, use the raw derivative with K_d = 5. Predicted symptom: the torque’s high-frequency std is K_d σ √2/T_s = 5·0.01·1.414/0.001 = 70.7 N·m — large, and the joint chatters. Actual: as predicted — silent. The √2 is not decoration: the difference e_k − e_{k−1} of two independent samples of variance σ² has variance 2σ². Dropping it gives 50, which is the same order and the wrong constant.
3. Gain-induced instability (silent). Set K_p = 1000 (50× the design). Predicted symptom: the closed-loop ω_n = √(1000/0.05) = 141 rad/s is 7× the design, and the sample rate 1 kHz gives ω_n T_s = 0.141 — still within the rule of thumb, but the unmodeled dynamics (if present) may destabilize. Actual: stable in the ideal case, but unstable with unmodeled dynamics — the lab tests the ideal case to show the stability’s dependence on the model.

Two of the three are silent (windup, noise); the third (gain) is silent in the ideal case but loud in the unmodeled case.

§L.6.7 — Acceptance criteria

§L.6.8 — What good looks like

A strong submission has five properties:

1. The PD’s step response matches the analytic prediction (the ω_n and ζ from the gains), with the overshoot and settling time measured and compared.
2. The PID’s steady-state error elimination is demonstrated, with the PD’s error shown for comparison.
3. The computed torque’s tracking error is an order of magnitude smaller than the PID’s, with both measured over the same trajectory.
4. The windup and the noise are shown with their symptoms (the overshoot with K_i = 100, the chatter with the raw derivative), and the anti-windup and the filter are implemented.
5. The discrete-time instability is shown at T_s = 0.1 (geometric divergence under forward Euler) and absent at T_s = 0.001, and the report distinguishes it from a phase-margin degradation rather than attributing both to the sample rate.

§L.6.9 — Runnable lab code

# lab_1_1_3.py — PID, computed torque, trajectory tracking, and the discrete-time constraint
import math
import numpy as np

J, G_AMP = 0.05, 0.5          # inertia, gravity amplitude

def plant_step(q, qdot, tau, dt=1e-3, dist=0.0, g=G_AMP):
    """One forward-Euler step of  J qddot + g sin(q) = tau - dist."""
    qddot = (tau - dist - g * math.sin(q)) / J
    return q + qdot * dt, qdot + qddot * dt

def pid_sim(q_d=1.0, Kp=20., Kd=1.4, Ki=0., dt=1e-3, T=3.0,
            sat=None, anti_windup=True, dist=0.0):
    q, qdot, integral, qs = 0.0, 0.0, 0.0, []
    for _ in range(int(round(T / dt))):
        e, e_dot = q_d - q, -qdot
        saturated = sat is not None and abs(Kp*e + Ki*integral + Kd*e_dot) >= sat
        if not (anti_windup and saturated):
            integral += e * dt
        tau = Kp*e + Ki*integral + Kd*e_dot
        if sat is not None:
            tau = max(-sat, min(sat, tau))
        q, qdot = plant_step(q, qdot, tau, dt, dist)
        qs.append(q)
    return np.array(qs)

def overshoot(qs):
    """Percent overshoot against the REACHED steady state, not the setpoint."""
    ss = qs[-1]
    return (qs.max() - ss) / abs(ss) * 100.0

def settling_time(qs, dt=1e-3, band=0.02):
    ss = qs[-1]
    outside = np.nonzero(np.abs(qs - ss) > band * abs(ss))[0]
    return 0.0 if len(outside) == 0 else (outside[-1] + 1) * dt

# --- H1: PD step ------------------------------------------------------
qs_pd = pid_sim(T=3.0)
print(f"H1: PD overshoot = {overshoot(qs_pd):.2f}%  (analytic exp(-pi*z/sqrt(1-z^2)) = 4.6%)")
print(f"H1: PD settling  = {settling_time(qs_pd):.3f} s (analytic 4/(z*wn) = 0.286 s)")
print(f"H1: steady-state droop from gravity = {1.0 - qs_pd[-1]:.3e} rad (g/Kp = {G_AMP/20:.3e})")

# --- H2: PD vs PID under a constant torque disturbance -----------------
pd_run  = pid_sim(Ki=0.,  T=10.0, dist=0.5)
pid_run = pid_sim(Ki=20., T=10.0, dist=0.5)
pd_ss, pid_ss = abs(1.0 - pd_run[-1]), abs(1.0 - pid_run[-1])
pred_pd = (0.5 + G_AMP*math.sin(pd_run[-1])) / 20.0
print(f"H2: PD  final error = {pd_ss:.3e}  (predicted (d + g sin q_ss)/Kp = {pred_pd:.3e})")
print(f"H2: PID final error = {pid_ss:.3e}  (predicted ~0)")

# --- H4: integral windup ----------------------------------------------
for Ki, aw in ((20., False), (200., False), (200., True)):
    qs = pid_sim(Ki=Ki, T=3.0, sat=10.0, anti_windup=aw)
    print(f"H4: Ki={Ki:3.0f}, anti-windup {'off' if not aw else 'on ':3s}: "
          f"overshoot = {overshoot(qs):5.1f}%")

# --- H5: forward Euler diverges at dt = 0.1; ZOH sampling would not ----
print("H5: growth of |q| between the first and last tenth of the run")
for dt in (1e-3, 1e-2, 5e-2, 1e-1):
    qs = pid_sim(dt=dt, T=1.0)
    n = len(qs); first = max(abs(qs[:max(1, n//10)]).max(), 1e-30)
    growth = abs(qs[-max(1, n//10):]).max() / first
    print(f"    dt={dt:.3f}: max|q| = {abs(qs).max():9.3e}  growth = {growth:9.2e}"
          f"  {'DIVERGED' if growth > 10 else 'bounded'}")

# --- H3: computed torque vs PID on a minimum-jerk trajectory -----------
def min_jerk(t, T):
    x = min(1.0, t / T); return 10*x**3 - 15*x**4 + 6*x**5
def min_jerk_vel(t, T):
    x = min(1.0, t / T); return (30*x**2 - 60*x**3 + 30*x**4) / T
def min_jerk_acc(t, T):
    x = min(1.0, t / T); return (60*x - 180*x**2 + 120*x**3) / (T*T)

def track(computed_torque, Kp=100., Kd=5., T=1.0, qf=0.5, dt=1e-3, g=3.0):
    q, qdot, errs = 0.0, 0.0, []
    for k in range(int(round(T / dt))):
        t = k * dt
        qd, qd_dot, qd_ddot = min_jerk(t,T)*qf, min_jerk_vel(t,T)*qf, min_jerk_acc(t,T)*qf
        e, e_dot = qd - q, qd_dot - qdot
        tau = J*(qd_ddot + Kd*e_dot + Kp*e) + g*math.sin(q) if computed_torque \
              else Kp*e + Kd*e_dot
        q, qdot = plant_step(q, qdot, tau, dt, g=g)
        errs.append(abs(e))
    return max(errs)

ct_err, pid_err = track(True), track(False)
print(f"H3: CT  max tracking error = {ct_err:.3e}  (predicted < 1e-3)")
print(f"H3: PID max tracking error = {pid_err:.3e}  (predicted > 1e-2)")

# --- Break-it: derivative noise ---------------------------------------
sigma, Kd_n, Ts = 0.01, 5.0, 1e-3
print(f"noise: raw-derivative torque std = Kd*sigma*sqrt(2)/Ts = "
      f"{Kd_n*sigma*math.sqrt(2)/Ts:.1f} N.m")

print("\n--- lab 1.1.3 summary ---")
print(f"H1 {overshoot(qs_pd):.2f}% / {settling_time(qs_pd):.3f} s  vs 4.6% / 0.286 s")
print(f"H2 PD {pd_ss:.2e}  vs PID {pid_ss:.2e}")
print(f"H3 CT {ct_err:.2e}  vs PID {pid_err:.2e}")
print(f"H4 windup {overshoot(pid_sim(Ki=200., T=3.0, sat=10.0, anti_windup=False)):.1f}% at Ki=200, no anti-windup")

§L.7 — The cheat sheet

§L.8 — Exercises with solutions

E 1.1.3.1 — The PD gains (numerical). A joint with J = 0.1, desired ω_n = 30, ζ = 0.6. Compute K_p and K_d.

S 1.1.3.1. K_p = 0.1·900 = 90; K_d = 2·0.6·0.1·30 = 3.6. The lesson: the gains scale with J and ω_n² (for K_p) and J ω_n (for K_d), and the ω_n is the bandwidth, which sets the speed.

E 1.1.3.2 — The overshoot (numerical). For ζ = 0.5, what is the overshoot?

S 1.1.3.2. exp(−π·0.5/√(0.75)) = exp(−π·0.5/0.866) = exp(−1.814) ≈ 0.163 = 16.3%. The lesson: the overshoot is 16.3% for ζ = 0.5 — a large overshoot, and the ζ = 0.7 (4.6%) is the common target.

E 1.1.3.3 — The steady-state error (numerical). With K_p = 20, a constant disturbance d = 0.5 N·m. What is e_ss?

S 1.1.3.3. e_ss = d/K_p = 0.5/20 = 0.025 rad. The lesson: the error is the disturbance divided by the proportional gain, and the integral term (if added) eliminates it.

E 1.1.3.4 — The windup (diagnostic). A joint’s setpoint is a step, the actuator saturates at 10 N·m, and the joint overshoots by 50%. What is the cause, and what is the fix?

S 1.1.3.4. The integral windup — the integral accumulates during the saturation, and when the joint reaches the setpoint, the integral’s stored value drives a large torque. The fix is anti-windup (clamping or back-calculation). The lesson: the integral’s accumulation during saturation is the windup, and the symptom is the large overshoot.

E 1.1.3.5 — The derivative noise (numerical). A noisy encoder σ = 0.01 rad at T_s = 0.001 s, K_d = 5. What is the torque’s noise std?

S 1.1.3.5. The derivative’s noise is K_d · (e_k − e_{k-1})/T_s, with e_k − e_{k-1} having std √2 · σ ≈ 0.0141. The torque’s noise std is 5 · 0.0141/0.001 ≈ 70.5 N·m — huge. The filter (ω_f = 100 rad/s) reduces it to ~70.5 · 100/1000 = 7 N·m — still large, but manageable. The lesson: the raw derivative’s noise is K_d σ √2/T_s, which is amplified by 1/T_s, and the filter is essential.

E 1.1.3.6 — The computed torque (diagnostic). A 6-DOF arm’s computed torque tracks a slow trajectory with 0.001 rad error, but a fast trajectory with 0.01 rad error. What is the cause?

S 1.1.3.6. The model mismatch’s error is (ΔM/M) q̈_d / K_p, which is proportional to the acceleration. The fast trajectory’s larger acceleration gives a larger error. The fix is a more accurate M (a better inertial model), or a higher K_p (at the cost of stability). The lesson: the acceleration is the source of the mismatch’s error, and the faster trajectory amplifies it.

E 1.1.3.7 — The sample rate (numerical). A loop with ω_n = 50 rad/s. What is the maximum T_s for the rule of thumb ω_n T_s ≤ 0.1? What is the corresponding f_s?

S 1.1.3.7. T_s ≤ 0.1/50 = 0.002 s, f_s ≥ 500 Hz. The lesson: these are two different rules and they do not agree unless you convert units. ω_n T_s ≤ 0.1 with ω_n = 50 rad/s gives f_s ≥ 500 Hz. The “10× the bandwidth in Hz” rule gives 10 · 50/(2π) ≈ 80 Hz. Both are individually defensible and they differ by the factor 2π, because one is stated in rad/s and the other in Hz. Quote the rule in the units it was written in — ω_n T_s ≤ 0.1 is unambiguous, and 10× is not.

E 1.1.3.8 — The trajectory design (constructive, transfer probe). A 2-DOF arm must move from q_0 = (0, 0) to q_f = (1, 1) in T = 0.5 s, with a peak torque of 10 N·m per joint. What is the minimum-jerk trajectory’s peak acceleration, and does it exceed the actuator’s limit?

S 1.1.3.8. Δq = 1 per joint, q̈_peak = 5.773 · 1/0.25 = 23.09 rad/s². The peak torque is M · q̈_peak ≈ 0.05 · 23.09 = 1.15 N·m — well below the 10 N·m limit. The trajectory is feasible. The faster T = 0.25 would give q̈_peak = 92.4 rad/s², τ_peak = 4.6 N·m — still feasible, but closer. The lesson: the actuator’s limit determines the minimum T, and the trajectory’s feasibility is the torque check.

§L.9 — Exit test

You are ready for Lesson 1.2.1 when you can, without notes:

1. State the PID control law and the role of each term.
2. Derive the error dynamics for a second-order plant under PD.
3. State the ω_n and ζ in terms of the gains, and the critically damped gains.
4. State the overshoot and settling time formulas.
5. State the steady-state error under PD and its elimination by PID.
6. State the computed torque law and why it linearizes the closed loop.
7. State the model mismatch’s effect and the inertia vs. gravity time signatures.
8. State the minimum-jerk trajectory and the 1/T² error scaling.
9. State the sample rate rule (ω_n T_s ≤ 0.1) and the Nyquist oscillation’s symptom.
10. State integral windup and the anti-windup fix.
11. State derivative noise and the filtered derivative fix.
12. Diagnose the overshoot (windup), the chatter (derivative noise), the growing oscillation (gain instability), and the steady-state sag (missing gravity feedforward) from their symptoms.

If any item is not answerable without notes, return to the section that introduced it: 1 to §L.1, 2 to §L.1, 3 to §L.1.1, 4 to §L.1.1, 5 to §L.1.2, 6 to §L.2, 7 to §L.2.1, 8 to §L.3, 9 to §L.4, 10 to §L.4, 11 to §L.4, 12 to §L.5.

End of Lesson 1.1.3. The learner has now completed the control foundation: the PID and its stability, the computed torque and its linearization, the trajectory generation and the speed–error trade-off, and the real-time discretization with its windup, noise, and sampling constraints. The next lesson, 1.1.4 — the full-stack integration — combines the FK, IK, PID, and computed torque into a complete pick-and-place controller for the running case, with the trajectory generation, the tracking, and the error recovery as the three phases.

§L.10 — Provenance and evidence tiers

What this lesson reproduces. Every hypothesis in §L.6.5 is executed by the lab in §L.6.9. These claims are tier A, status R.

What this lesson does not reproduce. H5’s cause claim — that the instability is the forward-Euler integrator, not the sample rate — is stated in the hypothesis and is not executed by the lab’s own code block, which only shows the forward-Euler divergence. It is checkable in four lines: the closed loop J q̈ + K_d q̇ + K_p q = 0 with the lab’s gains (K_p = 20, K_d = 1.4, J = 0.05) has forward-Euler |eig| = 1.4832 at T_s = 0.1 and exact-ZOH |eig| = 0.2466. The lesson’s 0.247 is correct; the figure is tier A, status S — reproduced, but by an auxiliary computation rather than by the shipped lab.

The 5.8× separation in H3 is a design choice, not a law. At g = 0.5, K_p = 400 the two controllers agree to a factor of 5.8 (9.5e-5 against 5.5e-4) and both sit under 1e-3; the separation to 1.34e-2 requires g = 3.0, K_p = 100. The hypothesis names its parameters for that reason. A reader who expects computed torque to beat PID by two orders of magnitude on any plant will be surprised by the first pair.

The PD overshoot prediction is approximate and the lab says so. 4.6% is the second-order underdamped formula; the measured 5.07% includes gravity and the finite step. The lesson measures overshoot against the reached steady state (0.979 rad), not against the setpoint, because the droop would otherwise be counted as overshoot — a measurement choice that changes the number by more than the modeling error.

No external sources are cited in this lesson. PID, computed torque, and the second-order step response are derived in Parts A–C and executed by the lab. No paper is named. Where the lesson quotes exp(−πζ/√(1−ζ²)), it is deriving it in §L.1.2, not citing a textbook for it.