PyDiffGame 1.0.0__py3-none-any.whl → 2.0.0__py3-none-any.whl

PyDiffGame/continuous.py

@@ -0,0 +1,223 @@
+r"""Continuous-time differential game solver.
+
+Solves the control design for
+
+.. math:: \dot{x}(t) = A x(t) + \sum_{i=1}^N B_i v_i(t)
+
+via the coupled differential / algebraic Riccati equations.  The feedback Nash
+gains are :math:`K_i = R_{ii}^{-1} B_i^\top P_i`, and the closed loop is
+:math:`A_{cl} = A - \sum_i B_i K_i`.
+"""
+
+from __future__ import annotations
+
+import warnings
+
+import numpy as np
+from scipy.integrate import ODEintWarning, odeint, solve_ivp
+from scipy.linalg import solve_continuous_are
+
+from PyDiffGame._typing import FloatArray
+from PyDiffGame.base import PyDiffGame
+
+
+class ContinuousPyDiffGame(PyDiffGame):
+    """Continuous-time differential game / LQR solver."""
+
+    def _solve_are(self, B: FloatArray, Q: FloatArray, R: FloatArray) -> FloatArray:
+        return solve_continuous_are(self._A, B, Q, R)
+
+    # ------------------------------------------------------------------ #
+    # Coupled Riccati dynamics
+    # ------------------------------------------------------------------ #
+    def _dP_dt(self, _t: float, P_flat: FloatArray) -> FloatArray:
+        r"""RHS of the coupled matrix Riccati ODE (forward-time derivative).
+
+        .. math:: \dot P_i = -\left(A_{cl}^\top P_i + P_i A_{cl}
+                  + Q_i + P_i S_i P_i\right),\qquad A_{cl}=A-\sum_j S_j P_j
+        """
+
+        Ps = self._unflatten(P_flat)
+        A_cl = self._A - sum(S_j @ P_j for S_j, P_j in zip(self._S, Ps))
+        derivatives = [
+            -(A_cl.T @ P_i + P_i @ A_cl + Q_i + P_i @ S_i @ P_i)
+            for P_i, S_i, Q_i in zip(Ps, self._S, self._Qs)
+        ]
+        return np.concatenate([d.ravel() for d in derivatives])
+
+    def _unflatten(self, P_flat: FloatArray) -> list[FloatArray]:
+        size = self._n * self._n
+        return [P_flat[i * size : (i + 1) * size].reshape(self._n, self._n) for i in range(self._N)]
+
+    # ------------------------------------------------------------------ #
+    # Solve
+    # ------------------------------------------------------------------ #
+    def solve(self) -> ContinuousPyDiffGame:
+        if self._infinite_horizon:
+            self._solve_infinite_horizon()
+        else:
+            self._solve_finite_horizon()
+        self._solved = True
+        return self
+
+    def _solve_infinite_horizon(self) -> None:
+        if self.is_lqr:
+            P = self._solve_are(self._Bs[0], self._Qs[0], self._Rs[0])
+            self._P = [P]
+        else:
+            self._P = self._converge_to_algebraic_solution()
+
+        self._K = [np.linalg.solve(R_i, B_i.T) @ P_i for R_i, B_i, P_i in zip(self._Rs, self._Bs, self._P)]
+        self._K_aggregate = self._aggregate_gain(self._K)
+        self._A_cl = self._closed_loop(self._K)
+
+    def _converge_to_algebraic_solution(self) -> list[FloatArray]:
+        """Integrate the coupled DREs backwards over repeated horizons until steady state.
+
+        Robust against games with no stabilising Nash equilibrium: each backward
+        integration uses ``odeint`` with a hard step-count cap (``mxstep``) so a
+        single stiff solve can never hang, and an exceeded cap, a non-finite norm
+        or a failure to converge within ``max_P_iterations`` all raise a clear
+        error pointing the user at the finite-horizon formulation.
+        """
+
+        no_equilibrium = (
+            "the coupled algebraic Riccati iteration did not reach a stabilising Nash "
+            "equilibrium ({reason}); this infinite-horizon game may not have one - "
+            "try a finite horizon by passing T_f."
+        )
+        Ps = list(self._P_f)
+        norms: list[float] = []
+        for _ in range(self.max_P_iterations):
+            y0 = np.concatenate([P.ravel() for P in Ps])
+            try:
+                with warnings.catch_warnings():
+                    warnings.simplefilter("error", category=ODEintWarning)
+                    ys = odeint(
+                        self._dP_dt,
+                        y0,
+                        [self._T_f, 0.0],
+                        tfirst=True,
+                        rtol=1e-8,
+                        atol=1e-10,
+                        mxstep=10000,
+                    )
+            except ODEintWarning as exc:
+                raise RuntimeError(
+                    no_equilibrium.format(reason="backward integration did not converge")
+                ) from exc
+            Ps = self._unflatten(ys[-1])
+            norm = sum(float(np.linalg.norm(P)) for P in Ps)
+            if not np.isfinite(norm):
+                raise RuntimeError(no_equilibrium.format(reason="non-finite Riccati norm"))
+            norms.append(norm)
+            if self._converged(norms):
+                return Ps
+        raise RuntimeError(
+            no_equilibrium.format(reason=f"no convergence in {self.max_P_iterations} iterations")
+        )
+
+    def _solve_finite_horizon(self) -> None:
+        backward_time = np.linspace(self._T_f, 0.0, self._L)
+        solution = solve_ivp(
+            fun=self._dP_dt,
+            t_span=(self._T_f, 0.0),
+            y0=np.concatenate([P.ravel() for P in self._P_f]),
+            method="LSODA",
+            t_eval=backward_time,
+            rtol=1e-8,
+            atol=1e-10,
+        )
+        # Reorder so index 0 corresponds to t = 0 (forward time).
+        Ps_t = solution.y[:, ::-1]
+        self._P = np.stack([self._unflatten(Ps_t[:, l]) for l in range(self._L)])  # (L, N, n, n)
+
+        gains_t, aggregate_t = [], []
+        for l in range(self._L):
+            player_gains = [
+                np.linalg.solve(R_i, B_i.T) @ self._P[l, i]
+                for i, (R_i, B_i) in enumerate(zip(self._Rs, self._Bs))
+            ]
+            gains_t.append(player_gains)
+            aggregate_t.append(self._aggregate_gain(player_gains))
+        self._K = gains_t
+        self._K_aggregate_t = aggregate_t
+        self._A_cl = self._closed_loop(gains_t[0])
+
+    # ------------------------------------------------------------------ #
+    # Gains
+    # ------------------------------------------------------------------ #
+    def _aggregate_gain(self, player_gains: list[FloatArray]) -> FloatArray:
+        """Physical-input gain ``K`` such that ``u = -K x``."""
+
+        if self.is_lqr:
+            return player_gains[0]
+        stacked = np.concatenate(player_gains, axis=0)
+        return self._M_inv @ stacked if self._M_inv is not None else stacked
+
+    def _gain_at(self, l: int) -> FloatArray:
+        return self._K_aggregate if self._infinite_horizon else self._K_aggregate_t[l]
+
+    def _closed_loop_at(self, l: int) -> FloatArray:
+        if self._infinite_horizon:
+            return self._A_cl
+        return self._closed_loop(self._K[l])
+
+    # ------------------------------------------------------------------ #
+    # Simulate
+    # ------------------------------------------------------------------ #
+    def simulate(self) -> ContinuousPyDiffGame:
+        self._require_solved()
+        if self._x_0 is None:
+            raise RuntimeError("simulate() requires x_0")
+        target = self._x_T if self._x_T is not None else np.zeros(self._n)
+
+        def dx_dt(t: float, x: FloatArray) -> FloatArray:
+            l = min(int(t / self._delta), self._L - 1)
+            return self._closed_loop_at(l) @ (x - target)
+
+        solution = solve_ivp(
+            fun=dx_dt,
+            t_span=(0.0, self._T_f),
+            y0=self._x_0,
+            method="LSODA",
+            t_eval=self._forward_time,
+            rtol=1e-8,
+            atol=1e-10,
+        )
+        self._x = solution.y.T
+        return self
+
+    # ------------------------------------------------------------------ #
+    # Stability
+    # ------------------------------------------------------------------ #
+    def is_closed_loop_stable(self) -> bool:
+        """Hurwitz test: all eigenvalues have non-positive real part, at most one at 0."""
+
+        self._require_solved()
+        eigenvalues = np.linalg.eigvals(self._A_cl)
+        real_parts = eigenvalues.real
+        zeros = int(np.sum(np.abs(real_parts) < self.eigenvalue_tolerance))
+        return bool(np.all(real_parts <= self.eigenvalue_tolerance) and zeros <= 1)
+
+    def algebraic_riccati_residuals(self) -> list[FloatArray]:
+        r"""Coupled-ARE residuals :math:`A_{cl}^\top P_i + P_i A_{cl} + Q_i + P_i S_i P_i`.
+
+        Only meaningful for the infinite-horizon case, where each residual should
+        be close to zero.
+        """
+
+        self._require_solved()
+        if not self._infinite_horizon:
+            raise RuntimeError(
+                "algebraic_riccati_residuals() is only defined for the infinite-horizon problem; "
+                "the finite-horizon solution solves the time-varying differential Riccati equation."
+            )
+        A_cl = self._closed_loop(self._K)
+        return [
+            A_cl.T @ P_i + P_i @ A_cl + Q_i + P_i @ S_i @ P_i
+            for P_i, S_i, Q_i in zip(self._P, self._S, self._Qs)
+        ]
+
+
+__all__ = ["ContinuousPyDiffGame"]

PyDiffGame/discrete.py

@@ -0,0 +1,211 @@
+r"""Discrete-time differential game solver.
+
+Considers control design for
+
+.. math:: x[k+1] = \tilde A x[k] + \sum_{i=1}^N \tilde B_i v_i[k]
+
+If the matrices are supplied in continuous form (the default) they are first
+discretised with a zero-order hold using the Van Loan matrix-exponential
+identity.  The coupled feedback gains at each step are obtained by solving a
+*linear* block system (the discrete coupled-Riccati gain equations are linear in
+the gains given the next-step cost matrices), which is both faster and far more
+robust than the root-finding the previous implementation attempted.
+"""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+
+import numpy as np
+from scipy.linalg import expm, solve_discrete_are
+
+from PyDiffGame._typing import ArrayLike, FloatArray
+from PyDiffGame.base import PyDiffGame
+from PyDiffGame.objective import Objective
+
+
+class DiscretePyDiffGame(PyDiffGame):
+    """Discrete-time differential game / LQR solver.
+
+    Parameters
+    ----------
+    is_input_discrete:
+        When ``True`` the matrices ``A``, ``B`` and the objectives are taken to
+        be already in discrete form.  When ``False`` (default) the continuous
+        data are discretised with sampling period ``delta = T_f / L``.
+    """
+
+    def __init__(
+        self,
+        A: ArrayLike,
+        objectives: Sequence[Objective],
+        *,
+        is_input_discrete: bool = False,
+        **kwargs,
+    ) -> None:
+        super().__init__(A, objectives, **kwargs)
+        if not is_input_discrete:
+            self._discretize()
+        # Riccati seeds must be computed from the (possibly discretised) data.
+        self._P_f = self._uncoupled_are_solutions()
+
+    def _solve_are(self, B: FloatArray, Q: FloatArray, R: FloatArray) -> FloatArray:
+        return solve_discrete_are(self._A, B, Q, R)
+
+    def _discretize(self) -> None:
+        r"""Zero-order-hold discretisation via the Van Loan identity.
+
+        .. math:: \exp\!\left(\begin{bmatrix}A & I\\0 & 0\end{bmatrix}\delta\right)
+                  = \begin{bmatrix}\tilde A & \Gamma\\0 & I\end{bmatrix},
+                  \qquad \tilde B_i = \Gamma B_i
+
+        The quadratic cost weights are scaled by the sampling period so the
+        discrete sum approximates the continuous integral.
+        """
+
+        n, delta = self._n, self._delta
+        augmented = np.zeros((2 * n, 2 * n))
+        augmented[:n, :n] = self._A
+        augmented[:n, n:] = np.eye(n)
+        exponential = expm(augmented * delta)
+        A_d = exponential[:n, :n]
+        gamma = exponential[:n, n:]
+
+        self._A = A_d
+        self._Bs = [gamma @ B_i for B_i in self._Bs]
+        self._Qs = [Q_i * delta for Q_i in self._Qs]
+        self._Rs = [R_i * delta for R_i in self._Rs]
+
+    # ------------------------------------------------------------------ #
+    # Coupled gain solve (one backward step)
+    # ------------------------------------------------------------------ #
+    def _step_gains(self, Ps_next: list[FloatArray]) -> list[FloatArray]:
+        r"""Feedback gains for one backward step given next-step matrices ``P[k+1]``.
+
+        Solves the linear block system arising from
+
+        .. math:: K_i = (R_{ii} + B_i^\top P_i B_i)^{-1} B_i^\top P_i
+                        \Big(A - \sum_{j\neq i} B_j K_j\Big)
+        """
+
+        G = [
+            np.linalg.solve(R_i + B_i.T @ P_i @ B_i, B_i.T @ P_i)
+            for B_i, R_i, P_i in zip(self._Bs, self._Rs, Ps_next)
+        ]
+        offsets = np.cumsum([0] + [B_i.shape[1] for B_i in self._Bs])
+        m = int(offsets[-1])
+        C = np.zeros((m, m))
+        D = np.zeros((m, self._n))
+        for i in range(self._N):
+            rows = slice(offsets[i], offsets[i + 1])
+            D[rows] = G[i] @ self._A
+            for j in range(self._N):
+                cols = slice(offsets[j], offsets[j + 1])
+                if i == j:
+                    C[rows, cols] = np.eye(self._Bs[i].shape[1])
+                else:
+                    C[rows, cols] = G[i] @ self._Bs[j]
+        K_stacked = np.linalg.solve(C, D)
+        return [K_stacked[offsets[i] : offsets[i + 1]] for i in range(self._N)]
+
+    def _step_P(self, Ps_next: list[FloatArray], gains: list[FloatArray]) -> list[FloatArray]:
+        A_cl = self._closed_loop(gains)
+        return [
+            A_cl.T @ P_i @ A_cl + K_i.T @ R_i @ K_i + Q_i
+            for P_i, K_i, R_i, Q_i in zip(Ps_next, gains, self._Rs, self._Qs)
+        ]
+
+    # ------------------------------------------------------------------ #
+    # Solve
+    # ------------------------------------------------------------------ #
+    def solve(self) -> DiscretePyDiffGame:
+        if self._infinite_horizon:
+            self._solve_infinite_horizon()
+        else:
+            self._solve_finite_horizon()
+        self._solved = True
+        return self
+
+    def _solve_infinite_horizon(self) -> None:
+        if self.is_lqr:
+            P = self._solve_are(self._Bs[0], self._Qs[0], self._Rs[0])
+            Ps = [P]
+            gains = self._step_gains(Ps)
+        else:
+            Ps = list(self._P_f)
+            norms: list[float] = []
+            for _ in range(self.max_P_iterations):
+                gains = self._step_gains(Ps)
+                Ps = self._step_P(Ps, gains)
+                norms.append(sum(float(np.linalg.norm(P)) for P in Ps))
+                if self._converged(norms):
+                    break
+            gains = self._step_gains(Ps)
+
+        self._P = Ps
+        self._K = gains
+        self._K_aggregate = self._aggregate_gain(gains)
+        self._A_cl = self._closed_loop(gains)
+
+    def _solve_finite_horizon(self) -> None:
+        Ps_t: list[list[FloatArray]] = [list(self._P_f)]
+        gains_t: list[list[FloatArray]] = []
+        for _ in range(self._L - 1):
+            gains = self._step_gains(Ps_t[-1])
+            Ps_t.append(self._step_P(Ps_t[-1], gains))
+            gains_t.append(gains)
+        gains_t.append(self._step_gains(Ps_t[-1]))
+
+        # Index 0 == t = 0 (forward time): reverse the backward sweep.
+        Ps_t.reverse()
+        gains_t.reverse()
+        self._P = np.stack([np.stack(step) for step in Ps_t])  # (L, N, n, n)
+        self._K = gains_t
+        self._K_aggregate_t = [self._aggregate_gain(g) for g in gains_t]
+        self._A_cl = self._closed_loop(gains_t[0])
+
+    # ------------------------------------------------------------------ #
+    # Gains
+    # ------------------------------------------------------------------ #
+    def _aggregate_gain(self, player_gains: list[FloatArray]) -> FloatArray:
+        if self.is_lqr:
+            return player_gains[0]
+        stacked = np.concatenate(player_gains, axis=0)
+        return self._M_inv @ stacked if self._M_inv is not None else stacked
+
+    def _gain_at(self, l: int) -> FloatArray:
+        return self._K_aggregate if self._infinite_horizon else self._K_aggregate_t[l]
+
+    def _closed_loop_at(self, l: int) -> FloatArray:
+        if self._infinite_horizon:
+            return self._A_cl
+        return self._closed_loop(self._K[l])
+
+    # ------------------------------------------------------------------ #
+    # Simulate
+    # ------------------------------------------------------------------ #
+    def simulate(self) -> DiscretePyDiffGame:
+        self._require_solved()
+        if self._x_0 is None:
+            raise RuntimeError("simulate() requires x_0")
+        target = self._x_T if self._x_T is not None else np.zeros(self._n)
+
+        x = np.zeros((self._L, self._n))
+        x[0] = self._x_0
+        for k in range(self._L - 1):
+            x[k + 1] = self._closed_loop_at(k) @ (x[k] - target) + target
+        self._x = x
+        return self
+
+    # ------------------------------------------------------------------ #
+    # Stability
+    # ------------------------------------------------------------------ #
+    def is_closed_loop_stable(self) -> bool:
+        """Schur test: the closed-loop spectral radius is strictly below one."""
+
+        self._require_solved()
+        spectral_radius = float(np.max(np.abs(np.linalg.eigvals(self._A_cl))))
+        return spectral_radius < 1.0
+
+
+__all__ = ["DiscretePyDiffGame"]