"""Domain randomisation toy: tune a PD joint controller in simulation. Plant: J * theta'' + b * theta' = u, torque limited to +/- tau_max, with an actuation delay of a whole number of control periods (0 = applied at once). The controller is u = kp * (target - theta) - kd * omega, run at 1 / dt. Two ways to pick (kp, kd) from a grid: nominal: minimise the cost for the single nominal (J, b, delay); randomised: minimise the mean cost over many sampled (J, b, delay). All simulations are vectorised over parameter sets. """ from __future__ import annotations from dataclasses import dataclass import numpy as np # Example values for a small servo-driven joint; not a data-sheet model. J0 = 0.01 # kg m^2 B0 = 0.10 # N m s / rad DELAY0 = 1 # control periods DT = 0.01 # s (100 Hz control) TAU_MAX = 1.0 # N m TARGET = 0.5 # rad step HORIZON = 1.5 # s @dataclass class Params: j: np.ndarray b: np.ndarray delay: np.ndarray # integer control periods between computing and applying a command def __len__(self) -> int: return len(self.j) def nominal() -> Params: return Params(np.array([J0]), np.array([B0]), np.array([DELAY0])) def sample_params(rng: np.random.Generator, spread: float, n: int, max_extra_delay: int = 2) -> Params: """J and b uniform in nominal * (1 +/- spread); extra delay up to round(max_extra_delay * spread).""" if not 0.0 <= spread < 1.0: raise ValueError("spread must be in [0, 1)") j = J0 * (1.0 + spread * rng.uniform(-1.0, 1.0, n)) b = B0 * (1.0 + spread * rng.uniform(-1.0, 1.0, n)) extra = int(round(max_extra_delay * spread)) delay = DELAY0 + rng.integers(0, extra + 1, n) return Params(j, b, delay) def simulate(kp: float, kd: float, p: Params, dt: float = DT, horizon: float = HORIZON, target: float = TARGET, tau_max: float = TAU_MAX) -> np.ndarray: """Return the position error, shape (len(p), steps). Exact discretisation per step.""" n = int(round(horizon / dt)) m = len(p) a = np.exp(-p.b * dt / p.j) # velocity decay over one period c1 = (1.0 - a) / p.b # omega gain per unit torque c2 = (p.j / p.b) * (1.0 - a) # theta gain per unit omega c3 = (dt - c2) / p.b # theta gain per unit torque max_delay = int(p.delay.max()) queue = np.zeros((max_delay + 1, m)) # queue[d] is applied d periods from now theta = np.zeros(m) omega = np.zeros(m) err = np.empty((m, n)) idx = np.arange(m) for k in range(n): e = target - theta u = np.clip(kp * e - kd * omega, -tau_max, tau_max) queue[p.delay, idx] = u # command enters the delay line u_now = queue[0].copy() queue = np.roll(queue, -1, axis=0) queue[-1] = 0.0 theta, omega = theta + c2 * omega + c3 * u_now, a * omega + c1 * u_now theta = np.where(np.abs(theta) > 1e3, 1e3, theta) # keep unstable runs finite err[:, k] = target - theta return err def cost(err: np.ndarray, dt: float = DT, target: float = TARGET) -> np.ndarray: """Normalised integral of |error| plus a heavy penalty on overshoot (per parameter set).""" iae = np.sum(np.abs(err), axis=1) * dt / target overshoot = np.maximum(0.0, -err.min(axis=1)) / target return iae + 10.0 * overshoot def failed(err: np.ndarray, target: float = TARGET) -> np.ndarray: """A run fails if it overshoots by more than 10 % or ends more than 2 % from the target.""" overshoot = np.maximum(0.0, -err.min(axis=1)) / target final = np.abs(err[:, -1]) / target return (overshoot > 0.10) | (final > 0.02) KP_GRID = np.geomspace(0.5, 50.0, 30) KD_GRID = np.geomspace(0.01, 2.0, 30) def tune(p: Params, kp_grid: np.ndarray = KP_GRID, kd_grid: np.ndarray = KD_GRID) -> tuple[float, float, float]: """Grid search for the gains with the lowest mean cost over the parameter sets in p.""" best = (np.inf, 0.0, 0.0) for kp in kp_grid: for kd in kd_grid: c = float(np.mean(cost(simulate(kp, kd, p)))) if c < best[0]: best = (c, float(kp), float(kd)) return best[1], best[2], best[0] def evaluate(kp: float, kd: float, spread: float, n: int = 500, seed: int = 1) -> tuple[float, float]: """Mean cost and failure rate over n plants drawn with the given spread.""" p = sample_params(np.random.default_rng(seed), spread, n) err = simulate(kp, kd, p) return float(np.mean(cost(err))), float(np.mean(failed(err))) if __name__ == "__main__": kp_n, kd_n, _ = tune(nominal()) train = sample_params(np.random.default_rng(0), spread=0.6, n=64) kp_r, kd_r, _ = tune(train) print(f"nominal-tuned: kp = {kp_n:6.2f}, kd = {kd_n:.3f}") print(f"randomised-tuned: kp = {kp_r:6.2f}, kd = {kd_r:.3f}") print("spread nominal cost / fail randomised cost / fail") for s in (0.0, 0.2, 0.4, 0.6): cn, fn = evaluate(kp_n, kd_n, s) cr, fr = evaluate(kp_r, kd_r, s) print(f"{s:5.1f} {cn:6.3f} / {fn:5.1%} {cr:6.3f} / {fr:5.1%}")