"""Discrete PID controller with anti-windup, and a simulated robot joint. Contents -------- PID discrete PID: derivative on measurement, derivative filter, output limits, anti-windup by clamping or back-calculation JointPlant one joint as inertia + viscous damping + constant load torque: J * theta'' + b * theta' = tau - tau_load simulate closed-loop step response (controller at dt, plant sub-stepped) step_metrics overshoot, rise time, settling time, steady-state error Run this file to print the step metrics of a few gain sets: python pid.py """ from __future__ import annotations from dataclasses import dataclass, field from typing import Callable, Literal import numpy as np from numpy.typing import NDArray FloatArray = NDArray[np.float64] @dataclass class PID: """Discrete PID controller, evaluated once every dt seconds. u = kp*e + I + D, with e = setpoint - measurement I = running sum of ki*e*dt (stored in output units) D = -kd * d(measurement)/dt, low-pass filtered with time constant tau_d (derivative on measurement: no kick when the setpoint steps) The output is limited to [u_min, u_max]. anti_windup: "none" integrate always (shows windup) "clamp" conditional integration: stop integrating while the output is saturated and the error would drive it further "backcalc" back-calculation: bleed the integral with (u_sat - u_unsat) * dt / t_track """ kp: float ki: float kd: float dt: float u_min: float = -np.inf u_max: float = np.inf tau_d: float = 0.0 anti_windup: Literal["none", "clamp", "backcalc"] = "clamp" t_track: float | None = None derivative_on_measurement: bool = True integral: float = field(default=0.0, init=False) _prev_meas: float | None = field(default=None, init=False, repr=False) _prev_err: float | None = field(default=None, init=False, repr=False) _d_filt: float = field(default=0.0, init=False, repr=False) def reset(self) -> None: self.integral = 0.0 self._prev_meas = None self._prev_err = None self._d_filt = 0.0 def update(self, setpoint: float, measurement: float) -> float: err = setpoint - measurement # Derivative term (zero on the first call: no history yet) if self.derivative_on_measurement: raw_d = 0.0 if self._prev_meas is None else -(measurement - self._prev_meas) / self.dt else: raw_d = 0.0 if self._prev_err is None else (err - self._prev_err) / self.dt a = self.tau_d / (self.tau_d + self.dt) if self.tau_d > 0.0 else 0.0 self._d_filt = a * self._d_filt + (1.0 - a) * raw_d self._prev_meas, self._prev_err = measurement, err d_term = self.kd * self._d_filt # Integral term with anti-windup i_new = self.integral + self.ki * err * self.dt u_unsat = self.kp * err + i_new + d_term u = float(np.clip(u_unsat, self.u_min, self.u_max)) if self.anti_windup == "clamp": pushing_up = u_unsat > self.u_max and err > 0.0 pushing_down = u_unsat < self.u_min and err < 0.0 if not (pushing_up or pushing_down): self.integral = i_new elif self.anti_windup == "backcalc": t_track = self.t_track if self.t_track is not None else self._default_t_track() self.integral = i_new + (u - u_unsat) * self.dt / t_track elif self.anti_windup == "none": self.integral = i_new else: raise ValueError(f"unknown anti_windup {self.anti_windup!r}") return u def _default_t_track(self) -> float: # Common rule of thumb: tracking time between Td and Ti; use sqrt(Ti*Td) if self.ki <= 0.0: return 1.0 ti = self.kp / self.ki td = self.kd / self.kp if self.kp > 0.0 else 0.0 return float(np.sqrt(ti * td)) if td > 0.0 else ti @dataclass class JointPlant: """J * theta'' + b * theta' = tau - tau_load (SI units: kg m^2, N m s/rad, N m).""" inertia: float = 0.01 damping: float = 0.1 tau_load: float = 0.0 theta: float = 0.0 omega: float = 0.0 def step(self, tau: float, dt: float) -> None: """Advance by dt with semi-implicit Euler.""" alpha = (tau - self.damping * self.omega - self.tau_load) / self.inertia self.omega += alpha * dt self.theta += self.omega * dt @dataclass class SimResult: t: FloatArray theta: FloatArray u: FloatArray setpoint: FloatArray def simulate(controller: PID, plant: JointPlant, setpoint: Callable[[float], float] | float, t_end: float, substeps: int = 10, measurement_noise: float = 0.0, seed: int = 0, feedforward: Callable[[float], float] | None = None) -> SimResult: """Run the loop for t_end seconds. The controller output is held for one period dt (zero-order hold) while the plant is integrated in substeps. feedforward(t), if given, is a torque added to the PID output, for example J * qdd_ref(t) + b * qd_ref(t) computed from a trajectory.""" sp = setpoint if callable(setpoint) else (lambda _t, v=float(setpoint): v) rng = np.random.default_rng(seed) n = int(round(t_end / controller.dt)) t = np.arange(n + 1) * controller.dt theta = np.empty(n + 1) u = np.empty(n + 1) r = np.empty(n + 1) h = controller.dt / substeps for k in range(n + 1): y = plant.theta + (rng.normal(0.0, measurement_noise) if measurement_noise > 0 else 0.0) r[k] = sp(t[k]) u[k] = controller.update(r[k], y) if feedforward is not None: u[k] += feedforward(t[k]) theta[k] = plant.theta if k < n: for _ in range(substeps): plant.step(u[k], h) return SimResult(t=t, theta=theta, u=u, setpoint=r) def step_metrics(res: SimResult, band: float = 0.02) -> dict[str, float]: """Metrics of a step response from res.theta[0] to the final setpoint.""" y0, yf = res.theta[0], res.setpoint[-1] span = yf - y0 y = (res.theta - y0) / span overshoot = max(0.0, float(y.max()) - 1.0) * 100.0 t10 = res.t[np.argmax(y >= 0.1)] if np.any(y >= 0.1) else np.nan t90 = res.t[np.argmax(y >= 0.9)] if np.any(y >= 0.9) else np.nan outside = np.nonzero(np.abs(y - 1.0) > band)[0] if outside.size == 0: t_settle = 0.0 elif outside[-1] == y.size - 1: t_settle = np.nan else: t_settle = float(res.t[outside[-1] + 1]) return { "overshoot_pct": overshoot, "rise_time": float(t90 - t10), "settling_time": t_settle, "steady_state_error": float(yf - res.theta[-1]), } def pd_gains(inertia: float, damping: float, omega_n: float, zeta: float) -> tuple[float, float]: """kp, kd that place the closed-loop PD poles at natural frequency omega_n and damping ratio zeta for J s^2 + (b + kd) s + kp.""" kp = inertia * omega_n ** 2 kd = 2.0 * zeta * omega_n * inertia - damping return kp, kd def main() -> None: dt = 0.001 print("Joint: J = 0.01 kg m^2, b = 0.1 N m s/rad. Step 0 -> 0.5 rad, controller at 1 kHz.") cases = [ ("P kp=1, no load", dict(kp=1.0, ki=0.0, kd=0.0), 0.0), ("P kp=4, no load", dict(kp=4.0, ki=0.0, kd=0.0), 0.0), ("PD kp=4 kd=0.3, no load", dict(kp=4.0, ki=0.0, kd=0.3), 0.0), ("PD kp=4 kd=0.3, load", dict(kp=4.0, ki=0.0, kd=0.3), 0.05), ("PID kp=4 ki=20 kd=0.3, load", dict(kp=4.0, ki=20.0, kd=0.3), 0.05), ] for name, gains, load in cases: res = simulate(PID(dt=dt, **gains), JointPlant(tau_load=load), 0.5, 2.0) m = step_metrics(res) settle = "never" if np.isnan(m["settling_time"]) else f"{m['settling_time'] * 1e3:.0f} ms" print(f"{name:30s} overshoot {m['overshoot_pct']:5.1f} % rise {m['rise_time'] * 1e3:5.0f} ms " f"2 % settling {settle:>8s} final error {m['steady_state_error'] * 1e3:6.2f} mrad") if __name__ == "__main__": main()