"""Benchmark mode: synthesize several controllers and compare them fairly.
The whole point of this module is the *single* shared metric path:
``simulate_metrics`` owns the time grid, the integration method and the
units for every controller type. It simulates the closed loop once and reads
both y(t) and u(t) off that same trajectory -- u = -K x(t) for static state
feedback, controller-state propagation for dynamic output feedback -- so the
control-effort column of the comparison table is never computed on different
grids or methods per controller type.
"""
import multiprocessing
from typing import Dict, List, Optional
import numpy as np
from state_space_control.analysis import settling_time
from state_space_control.base import Plant, make_controller
SYNTH_TIMEOUT = 30.0 # seconds per synthesis (slycot can loop forever
# on plants with imaginary-axis poles)
[docs]
def design_controller(plant: Plant, name: str, params: Dict,
timeout: float = SYNTH_TIMEOUT):
"""Run ``make_controller(name, **params).design(plant)`` in a killable subprocess.
Parameter validation (make_controller/__init__) runs in-process so bad
params raise ValueError immediately; only .design() — which may call into
Fortran code that cannot be interrupted — runs in the child.
"""
design = make_controller(name, **params) # ValueError for bad params
ctx = multiprocessing.get_context('fork')
queue = ctx.Queue()
def worker():
try:
queue.put(('ok', design.design(plant)))
except Exception as exc:
queue.put(('err', type(exc).__name__, str(exc)))
proc = ctx.Process(target=worker, daemon=True)
proc.start()
try:
status, *rest = queue.get(timeout=timeout)
except Exception:
proc.terminate()
proc.join()
raise RuntimeError(
f'{name} synthesis did not finish within {timeout:.0f}s and was '
'aborted (H-infinity gamma iteration can fail to converge on '
'plants with undamped/imaginary-axis poles)')
proc.join()
if status == 'ok':
return rest[0]
exc_type, msg = rest
if exc_type == 'ValueError':
raise ValueError(msg)
raise RuntimeError(msg)
def _control_output_matrix(result) -> np.ndarray:
"""Cu such that u(t) = Cu @ x_cl(t) for the closed loop of ``result``.
Mirrors the state ordering of ControllerResult.closed_loop():
[x_plant] for static K, [x_plant; x_controller] for dynamic.
"""
plant = result.plant
if result.K is not None:
return -np.asarray(result.K)
k = result.controller
# u = k.D y + k.C xk, y = C x (plant strictly proper).
return np.hstack([np.asarray(k.D) @ plant.C, np.asarray(k.C)])
def _default_t_final(poles: np.ndarray) -> float:
stable = [p for p in poles if p.real < -1e-9]
return 8.0 / min(-p.real for p in stable) if stable else 10.0
[docs]
def simulate_metrics(
result,
input_index: int = 0,
t: Optional[np.ndarray] = None,
n_points: int = 600,
) -> Dict:
"""Step-response metrics for one designed controller.
A unit step enters at the plant input (disturbance rejection task, the
natural experiment for regulators about an equilibrium); y(t) and u(t)
are read off one simulation.
"""
from scipy.signal import StateSpace, step
cl = result.closed_loop()
Cu = _control_output_matrix(result)
m = Cu.shape[0]
Caug = np.vstack([cl.C, Cu])
Daug = np.zeros((Caug.shape[0], 1))
if t is None:
t_final = _default_t_final(cl.poles())
t = np.linspace(0.0, t_final, n_points)
lti = StateSpace(cl.A, cl.B[:, [input_index]], Caug, Daug)
t, ytot = step(lti, T=t)
ytot = np.atleast_2d(ytot)
if ytot.shape[0] != len(t):
ytot = ytot.T
ny = cl.C.shape[0]
y = ytot[:, :ny]
u = ytot[:, ny:ny + m]
stable = bool(result.is_stable())
y0 = y[:, 0]
yf = y0[-1]
if stable and abs(yf) > 1e-12:
peak = float(np.max(np.abs(y0)))
overshoot = max(0.0, (peak - abs(yf)) / abs(yf) * 100.0)
settling = settling_time(t, y0)
else:
overshoot = float('nan')
settling = float('inf')
trapezoid = getattr(np, 'trapezoid', np.trapz)
effort = float(trapezoid(np.sum(u * u, axis=1), t))
return {
'stable': stable,
'settling_time': float(settling),
'overshoot_pct': float(overshoot),
'control_effort': effort,
't': t,
'y': y,
'u': u,
}
def _loop_frequency_response(result, w: np.ndarray) -> np.ndarray:
"""Return ratio L(jw) with the loop broken at the plant input."""
plant = result.plant
A, B, C = plant.A, plant.B, plant.C
n, m = A.shape[0], B.shape[1]
L = np.empty((len(w), m, m), dtype=complex)
if result.K is not None:
K = np.asarray(result.K)
for i, wi in enumerate(w):
L[i] = K @ np.linalg.solve(1j * wi * np.eye(n) - A, B)
else:
k = result.controller
nk = k.A.shape[0]
for i, wi in enumerate(w):
G = C @ np.linalg.solve(1j * wi * np.eye(n) - A, B)
Kc = k.C @ np.linalg.solve(1j * wi * np.eye(nk) - k.A, k.B) + k.D
L[i] = -(Kc @ G)
return L
[docs]
def robustness_metrics(result) -> Dict:
"""Peak sensitivity Ms (any size) + gain/phase margins (SISO loops)."""
out = {'Ms': float('nan'), 'gain_margin_db': None, 'phase_margin_deg': None}
try:
poles = np.concatenate([result.plant.poles(),
result.closed_loop_poles()])
mags = np.abs(poles[np.abs(poles) > 1e-9])
wmin = 0.01 * (mags.min() if len(mags) else 1.0)
wmax = 100.0 * (mags.max() if len(mags) else 1.0)
w = np.logspace(np.log10(wmin), np.log10(wmax), 400)
L = _loop_frequency_response(result, w)
m = L.shape[1]
eye = np.eye(m)
ms = 0.0
for i in range(len(w)):
s = np.linalg.svd(np.linalg.inv(eye + L[i]), compute_uv=False)
ms = max(ms, float(s[0]))
out['Ms'] = ms
except Exception:
return out
if L.shape[1] == 1:
try:
import control
plant = result.plant
if result.K is not None:
lsys = control.ss(plant.A, plant.B,
np.asarray(result.K), 0.0)
else:
k = result.controller
G = control.ss(plant.A, plant.B, plant.C, plant.D)
Kc = control.ss(k.A, k.B, k.C, k.D)
lsys = control.series(G, control.negate(Kc)) \
if hasattr(control, 'negate') else -Kc * G
gm, pm, _, _ = control.margin(lsys)
if np.isfinite(gm) and gm > 0:
out['gain_margin_db'] = float(20 * np.log10(gm))
if np.isfinite(pm):
out['phase_margin_deg'] = float(pm)
except Exception:
pass
return out
[docs]
def run_benchmark(plant: Plant, entries: List[Dict], n_points: int = 600,
timeout: float = SYNTH_TIMEOUT) -> Dict:
"""Design + evaluate each entry {controller, params} on a shared grid.
Failed syntheses become rows with an 'error' field, never exceptions.
"""
# One shared time grid for every controller: base it on the slowest
# dynamics among all successfully designed closed loops.
designed = []
rows = []
for entry in entries:
name = entry['controller']
params = entry.get('params') or {}
try:
result = design_controller(plant, name, params, timeout=timeout)
designed.append((name, params, result))
except Exception as exc:
rows.append({'controller': name, 'params': params,
'error': str(exc)})
t = None
if designed:
t_final = max(_default_t_final(r.closed_loop().poles())
for _, _, r in designed)
t = np.linspace(0.0, t_final, n_points)
traces = {'t': t.tolist() if t is not None else [], 'y': {}, 'u': {}}
for name, params, result in designed:
try:
metrics = simulate_metrics(result, t=t)
rob = robustness_metrics(result)
rows.append({
'controller': name,
'params': params,
'stable': metrics['stable'],
'settling_time': metrics['settling_time'],
'overshoot_pct': metrics['overshoot_pct'],
'control_effort': metrics['control_effort'],
'Ms': rob['Ms'],
'gain_margin_db': rob['gain_margin_db'],
'phase_margin_deg': rob['phase_margin_deg'],
})
traces['y'][name] = metrics['y'][:, 0].tolist()
traces['u'][name] = metrics['u'].tolist()
except Exception as exc:
rows.append({'controller': name, 'params': params,
'error': str(exc)})
return {'table': rows, 'traces': traces}