first commit
This commit is contained in:
@@ -0,0 +1,9 @@
|
||||
__pycache__/
|
||||
.pytest_cache/
|
||||
.vscode/
|
||||
.claude/
|
||||
output/
|
||||
*.pyc
|
||||
*.pdf
|
||||
equations.md
|
||||
paper.md
|
||||
@@ -0,0 +1,26 @@
|
||||
"""HPI: Homotopy-Based Policy Iteration for Adaptive Optimal Control.
|
||||
|
||||
Implements the algorithm from:
|
||||
Chen et al., "Adaptive Optimal Control of Unknown Nonlinear Systems
|
||||
via Homotopy-Based Policy Iteration", IEEE TAC, 2024.
|
||||
|
||||
Uses neural network function approximators (CriticNN, ActorNN) trained
|
||||
via gradient-based Bellman residual minimization.
|
||||
"""
|
||||
|
||||
from .nn_models import (ActorNN, CriticNN, check_lyapunov_decrease,
|
||||
check_positive_definite, compute_q)
|
||||
from .data_collector import DataCollector
|
||||
from .nn_trainer import NNTrainer
|
||||
from .hpi_controller import HPIController
|
||||
|
||||
__all__ = [
|
||||
"CriticNN",
|
||||
"ActorNN",
|
||||
"compute_q",
|
||||
"check_positive_definite",
|
||||
"check_lyapunov_decrease",
|
||||
"DataCollector",
|
||||
"NNTrainer",
|
||||
"HPIController",
|
||||
]
|
||||
@@ -0,0 +1,131 @@
|
||||
"""Module A: ODE simulation and trajectory data collection.
|
||||
|
||||
Implements inverted pendulum dynamics and persistent excitation signal
|
||||
generation for data-driven adaptive optimal control.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from scipy.integrate import solve_ivp
|
||||
|
||||
|
||||
class DataCollector:
|
||||
"""Inverted pendulum simulator with data collection capabilities.
|
||||
|
||||
The pendulum dynamics are given by Eq. (27):
|
||||
dx₁/dt = x₂
|
||||
dx₂/dt = (mgl/J)·sin(x₁) + (1/J)·u
|
||||
"""
|
||||
|
||||
def __init__(self, J: float = 1.0, mgl: float = 1.0, n_freqs: int = 100):
|
||||
"""
|
||||
Args:
|
||||
J: Moment of inertia (default 1.0).
|
||||
mgl: Mass × gravity × length product (default 1.0).
|
||||
n_freqs: Number of frequency components in PE signal.
|
||||
"""
|
||||
self.J = float(J)
|
||||
self.mgl = float(mgl)
|
||||
self.n_freqs = n_freqs
|
||||
|
||||
# Pre-compute PE signal parameters
|
||||
self._rng = np.random.RandomState(42)
|
||||
self._omegas = self._rng.uniform(0.5, 50.0, size=n_freqs)
|
||||
self._a = self._rng.uniform(-1.0, 1.0, size=n_freqs)
|
||||
self._b = self._rng.uniform(-1.0, 1.0, size=n_freqs)
|
||||
# Scale amplitudes so that max |pe_signal| ≈ 0.1
|
||||
raw_max = np.sum(np.abs(self._a) + np.abs(self._b))
|
||||
self._scale = 0.1 / raw_max if raw_max > 0 else 1.0
|
||||
|
||||
def pe_signal(self, t):
|
||||
"""Persistent excitation signal.
|
||||
|
||||
u_e(t) = scale · Σ (a_k·sin(ω_k·t) + b_k·cos(ω_k·t))
|
||||
|
||||
Args:
|
||||
t: Time value (scalar).
|
||||
|
||||
Returns:
|
||||
float: PE signal value at time t.
|
||||
"""
|
||||
t = float(t)
|
||||
signals = self._a * np.sin(self._omegas * t) + self._b * np.cos(self._omegas * t)
|
||||
return self._scale * np.sum(signals)
|
||||
|
||||
def f(self, x):
|
||||
"""Drift dynamics f(x) — Eq. (27).
|
||||
|
||||
f(x) = [x₂, (mgl/J)·sin(x₁)]ᵀ
|
||||
|
||||
Args:
|
||||
x: State vector (2,) or (N, 2).
|
||||
|
||||
Returns:
|
||||
np.ndarray: Drift dynamics values.
|
||||
"""
|
||||
x = np.asarray(x, dtype=np.float64)
|
||||
if x.ndim == 1:
|
||||
return np.array([x[1], (self.mgl / self.J) * np.sin(x[0])])
|
||||
return np.column_stack([x[:, 1], (self.mgl / self.J) * np.sin(x[:, 0])])
|
||||
|
||||
def g(self, x):
|
||||
"""Input matrix g(x) — Eq. (27).
|
||||
|
||||
g(x) = [0, 1/J]ᵀ (constant)
|
||||
|
||||
Args:
|
||||
x: State vector (2,) or (N, 2) — ignored, kept for interface consistency.
|
||||
|
||||
Returns:
|
||||
np.ndarray: Input matrix [0, 1/J]ᵀ.
|
||||
"""
|
||||
x = np.asarray(x, dtype=np.float64)
|
||||
if x.ndim == 1:
|
||||
return np.array([0.0, 1.0 / self.J])
|
||||
N = x.shape[0]
|
||||
result = np.zeros((N, 2))
|
||||
result[:, 1] = 1.0 / self.J
|
||||
return result
|
||||
|
||||
def dynamics(self, t, x, u_func):
|
||||
"""ODE right-hand side: ẋ = f(x) + g(x)·u(t).
|
||||
|
||||
Args:
|
||||
t: Current time.
|
||||
x: State vector (2,).
|
||||
u_func: Callable u_func(t, x) → control input.
|
||||
|
||||
Returns:
|
||||
np.ndarray: State derivative (2,).
|
||||
"""
|
||||
u_val = u_func(t, x)
|
||||
return self.f(x) + self.g(x) * u_val
|
||||
|
||||
def collect_trajectory(self, x0, T, dt, u_func):
|
||||
"""Collect a trajectory by simulating the system.
|
||||
|
||||
Args:
|
||||
x0: Initial state (2,).
|
||||
T: Simulation duration.
|
||||
dt: Time step for output sampling.
|
||||
u_func: Control function u_func(t, x) → float.
|
||||
|
||||
Returns:
|
||||
tuple: (t, X, U) where
|
||||
t: Time points (M,).
|
||||
X: State trajectory (M, 2).
|
||||
U: Control inputs (M,).
|
||||
"""
|
||||
t_eval = np.arange(0, T, dt)
|
||||
t_span = (0.0, T)
|
||||
|
||||
def ode_rhs(t, x):
|
||||
return self.dynamics(t, x, u_func)
|
||||
|
||||
sol = solve_ivp(ode_rhs, t_span, x0, method="RK45",
|
||||
t_eval=t_eval, rtol=1e-8, atol=1e-10)
|
||||
|
||||
t = sol.t
|
||||
X = sol.y.T # (M, 2)
|
||||
U = np.array([u_func(ti, xi) for ti, xi in zip(t, X)])
|
||||
|
||||
return t, X, U
|
||||
+89
@@ -0,0 +1,89 @@
|
||||
"""Module E: Closed-loop verification demo for HPI algorithm — NN edition.
|
||||
|
||||
Runs the full three-phase HPI pipeline and verifies the optimal
|
||||
policy by simulating the closed-loop system from a perturbed initial state.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from scipy.integrate import solve_ivp
|
||||
|
||||
from .hpi_controller import HPIController
|
||||
|
||||
|
||||
def run_demo():
|
||||
"""Run full HPI demonstration and closed-loop verification.
|
||||
|
||||
Returns:
|
||||
dict: All results from the HPI pipeline and verification.
|
||||
"""
|
||||
print("=" * 60)
|
||||
print(" HPI: Homotopy-Based Policy Iteration (NN)")
|
||||
print(" Inverted Pendulum Adaptive Optimal Control")
|
||||
print("=" * 60)
|
||||
|
||||
# ── Initialize controller ──
|
||||
controller = HPIController(J=1.0, mgl=1.0, R=1.0, pe_amplitude=0.5,
|
||||
lr=1e-3, epochs=500, lambda_weight=1e-6)
|
||||
|
||||
# ── Run three-phase HPI ──
|
||||
print("\n[1/3] Phase 1: Searching for admissible L0 ...")
|
||||
results = controller.run(
|
||||
x0=np.array([0.5, 0.0]),
|
||||
T=5.0,
|
||||
dt=0.05,
|
||||
L_start=0.1,
|
||||
L_step=0.5,
|
||||
L_max=20.0,
|
||||
)
|
||||
|
||||
print(f" L0 = {results['phase1_L0']:.2f}")
|
||||
print(f" Phase 1 info: {results['phase1_info']}")
|
||||
print(f" Positive definite: {controller.check_positive_definite()}")
|
||||
|
||||
print(f"\n[2/3] Phase 2: Homotopy contraction ({len(results['phase2_history'])} iterations)")
|
||||
for i, (L, loss, pd) in enumerate(results["phase2_history"]):
|
||||
print(f" iter {i}: L={L:.4f}, loss={loss:.4e}, PD={pd}")
|
||||
|
||||
print(f"\n[3/3] Phase 3: Policy iteration (converged: {results['phase3_converged']})")
|
||||
print(f" Iterations: {len(results['phase3_history'])}")
|
||||
|
||||
# ── Closed-loop verification ──
|
||||
print("\n" + "=" * 60)
|
||||
print(" Closed-loop verification")
|
||||
print("=" * 60)
|
||||
|
||||
x0_test = np.array([0.9, 0.1])
|
||||
|
||||
def optimal_control(t, x):
|
||||
return controller.trainer.compute_u_hat(x)
|
||||
|
||||
def closed_loop_dynamics(t, x):
|
||||
return controller.collector.dynamics(t, x, optimal_control)
|
||||
|
||||
T_test = 5.0
|
||||
t_eval = np.linspace(0, T_test, 200)
|
||||
sol = solve_ivp(closed_loop_dynamics, (0, T_test), x0_test,
|
||||
method="RK45", t_eval=t_eval, rtol=1e-8, atol=1e-10)
|
||||
|
||||
X_cl = sol.y.T
|
||||
x_final = X_cl[-1]
|
||||
norm_final = np.linalg.norm(x_final)
|
||||
|
||||
print(f" Initial state: {x0_test}")
|
||||
print(f" Final state: {np.array2string(x_final, precision=8)}")
|
||||
print(f" ||x(T)||: {norm_final:.2e}")
|
||||
|
||||
if norm_final < 1e-3:
|
||||
print(" VERDICT: System converged to origin. [OK]")
|
||||
else:
|
||||
print(f" VERDICT: Residual norm {norm_final:.2e} > 1e-3 [FAIL]")
|
||||
|
||||
results["verification_x0"] = x0_test
|
||||
results["verification_x_final"] = x_final
|
||||
results["verification_norm"] = norm_final
|
||||
|
||||
return results
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
run_demo()
|
||||
@@ -0,0 +1,412 @@
|
||||
"""Module D: Three-phase HPI control flow (Algorithm 1) — NN edition.
|
||||
|
||||
Phase 1: Find admissible L0 with PD critic (NN-based).
|
||||
Phase 2: Homotopy contraction L_i -> 0 with safety constraint.
|
||||
Phase 3: Standard policy iteration on original system (L = 0).
|
||||
"""
|
||||
|
||||
import copy
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
from .data_collector import DataCollector
|
||||
from .nn_models import check_lyapunov_decrease, check_positive_definite
|
||||
from .nn_trainer import NNTrainer
|
||||
|
||||
|
||||
class HPIController:
|
||||
"""Implements the three-phase Homotopy-Based Policy Iteration algorithm
|
||||
using neural network function approximators."""
|
||||
|
||||
def __init__(self, J=1.0, mgl=1.0, R=1.0, pe_amplitude=0.5,
|
||||
lr=1e-3, epochs=500, lambda_weight=1e-6):
|
||||
"""
|
||||
Args:
|
||||
J: Moment of inertia (default 1.0).
|
||||
mgl: Mass x gravity x length product (default 1.0).
|
||||
R: Control cost weight (default 1.0).
|
||||
pe_amplitude: PE signal amplitude scale (default 0.5).
|
||||
lr: Learning rate for Adam optimizer.
|
||||
epochs: Training epochs per HPI iteration.
|
||||
lambda_weight: L2 regularization coefficient.
|
||||
"""
|
||||
self.J = float(J)
|
||||
self.mgl = float(mgl)
|
||||
self.R = float(R)
|
||||
self.lr = float(lr)
|
||||
self.epochs = int(epochs)
|
||||
self.lambda_weight = float(lambda_weight)
|
||||
self.pe_amplitude = float(pe_amplitude)
|
||||
self.collector = DataCollector(J=J, mgl=mgl)
|
||||
self.trainer = NNTrainer(R=R, lr=lr, epochs=epochs,
|
||||
lambda_weight=lambda_weight)
|
||||
# Override PE amplitude
|
||||
self.collector._scale = pe_amplitude / np.sum(
|
||||
np.abs(self.collector._a) + np.abs(self.collector._b)
|
||||
)
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Utility
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def check_positive_definite(self, critic_nn=None, X_traj=None):
|
||||
"""Check if critic NN corresponds to a PD value function.
|
||||
|
||||
Checks both Hessian at origin and V(x) positivity on trajectory.
|
||||
"""
|
||||
if critic_nn is None:
|
||||
critic_nn = self.trainer.critic_nn
|
||||
return check_positive_definite(critic_nn, X_traj=X_traj)
|
||||
|
||||
def check_lyapunov_decrease(self, X, f_X):
|
||||
"""Check if current critic satisfies Lyapunov decrease on trajectory."""
|
||||
return check_lyapunov_decrease(self.trainer.critic_nn, X, f_X)
|
||||
|
||||
def _compute_f_X(self, X):
|
||||
"""Drift dynamics f(x) = [x2, sin(x1)] on trajectory."""
|
||||
X = np.asarray(X)
|
||||
f_X = np.zeros_like(X)
|
||||
f_X[:, 0] = X[:, 1]
|
||||
f_X[:, 1] = np.sin(X[:, 0])
|
||||
return f_X
|
||||
|
||||
def _hessian_info(self):
|
||||
"""Return (h11, h22, det) of the critic Hessian at origin for logging."""
|
||||
x0 = torch.zeros(1, 2, requires_grad=True)
|
||||
V0 = self.trainer.critic_nn(x0)
|
||||
grad_V = torch.autograd.grad(V0.sum(), x0, create_graph=True)[0]
|
||||
h11 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 0].item()
|
||||
h22 = torch.autograd.grad(grad_V[0, 1], x0, retain_graph=True)[0][0, 1].item()
|
||||
h12 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 1].item()
|
||||
det = h11 * h22 - h12 ** 2
|
||||
return h11, h22, det
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Phase 1: Find L0 — scan ALL L values, pick best loss
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def phase_one_find_L0(self, x0, T, dt, L_start=0.1, L_step=0.5, L_max=20.0,
|
||||
verbose=False):
|
||||
"""Find admissible L0 by scanning all L values and picking the best.
|
||||
|
||||
Unlike the polynomial version which returns at the FIRST PD L,
|
||||
this scans ALL L values and chooses the one with lowest Bellman
|
||||
residual loss AND positive-definite critic. This ensures Phase 2
|
||||
starts from the best possible critic.
|
||||
|
||||
Args:
|
||||
x0: Initial state (2,).
|
||||
T: Simulation duration.
|
||||
dt: Time step.
|
||||
L_start: Starting L value.
|
||||
L_step: Increment step.
|
||||
L_max: Maximum L value before failure.
|
||||
verbose: If True, print progress.
|
||||
|
||||
Returns:
|
||||
tuple: (L0, critic_sd, actor_sd, results_dict).
|
||||
|
||||
Raises:
|
||||
RuntimeError: If no admissible L is found below L_max.
|
||||
"""
|
||||
L_values = []
|
||||
L = L_start
|
||||
while L <= L_max:
|
||||
L_values.append(L)
|
||||
L += L_step
|
||||
|
||||
if not L_values:
|
||||
raise RuntimeError("No L values to try (L_start > L_max)")
|
||||
|
||||
# Collect ONE shared trajectory with PE-only control (no actor)
|
||||
# for training at all L values.
|
||||
def u_func(t, x):
|
||||
return self.collector.pe_signal(t)
|
||||
|
||||
t, X, U = self.collector.collect_trajectory(x0, T, dt, u_func)
|
||||
f_X = self._compute_f_X(X)
|
||||
|
||||
best_L = None
|
||||
best_loss = float('inf')
|
||||
best_critic_sd = None
|
||||
best_actor_sd = None
|
||||
best_info = None
|
||||
|
||||
for L in L_values:
|
||||
if verbose:
|
||||
print(f" Phase 1: trying L={L:.2f}...", flush=True)
|
||||
|
||||
# Fresh trainer for each L
|
||||
trainer = NNTrainer(R=self.R, lr=self.lr, epochs=self.epochs,
|
||||
lambda_weight=self.lambda_weight)
|
||||
|
||||
result = trainer.train_one_iteration(t, X, U, L, prev_actor=None, dt=dt)
|
||||
loss = result['loss_c']
|
||||
pd = check_positive_definite(trainer.critic_nn, X_traj=X)
|
||||
lyap = check_lyapunov_decrease(trainer.critic_nn, X, f_X)
|
||||
|
||||
if verbose:
|
||||
h11, h22, det = self._hessian_info_via_trainer(trainer)
|
||||
print(f" loss={loss:.4e} PD={pd} lyap={lyap:.3f} "
|
||||
f"H=[{h11:.4f},{h22:.4f}] det={det:.4e}", flush=True)
|
||||
|
||||
# Only consider PD critics; pick lowest loss
|
||||
if pd and loss < best_loss:
|
||||
best_L = L
|
||||
best_loss = loss
|
||||
best_critic_sd = {k: v.clone() for k, v in trainer.critic_nn.state_dict().items()}
|
||||
best_actor_sd = {k: v.clone() for k, v in trainer.actor_nn.state_dict().items()}
|
||||
best_info = {'loss': loss, 'lyap': lyap, 'mse': result['mse_c']}
|
||||
|
||||
if best_L is None:
|
||||
raise RuntimeError(
|
||||
f"Phase 1 failed: no admissible L found up to L_max={L_max}. "
|
||||
f"Try increasing L_max or adjusting training parameters."
|
||||
)
|
||||
|
||||
if verbose:
|
||||
print(f" Phase 1: selected L0={best_L:.2f} (loss={best_loss:.4e})",
|
||||
flush=True)
|
||||
|
||||
# Load best weights into main trainer
|
||||
self.trainer.critic_nn.load_state_dict(best_critic_sd)
|
||||
self.trainer.actor_nn.load_state_dict(best_actor_sd)
|
||||
|
||||
return best_L, best_critic_sd, best_actor_sd, best_info
|
||||
|
||||
def _hessian_info_via_trainer(self, trainer):
|
||||
"""Get Hessian info from a specific trainer instance."""
|
||||
x0 = torch.zeros(1, 2, requires_grad=True)
|
||||
V0 = trainer.critic_nn(x0)
|
||||
grad_V = torch.autograd.grad(V0.sum(), x0, create_graph=True)[0]
|
||||
h11 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 0].item()
|
||||
h22 = torch.autograd.grad(grad_V[0, 1], x0, retain_graph=True)[0][0, 1].item()
|
||||
h12 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 1].item()
|
||||
det = h11 * h22 - h12 ** 2
|
||||
return h11, h22, det
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Phase 2: Homotopy contraction
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def _compute_safe_alpha(self, X, prev_actor, L, gamma=0.1):
|
||||
"""Compute safe homotopy step alpha via inequality constraint."""
|
||||
x_tensor = torch.tensor(X, dtype=torch.float32, requires_grad=True)
|
||||
V = self.trainer.critic_nn(x_tensor)
|
||||
grad_V = torch.autograd.grad(V.sum(), x_tensor, create_graph=False)[0]
|
||||
dot_V = (grad_V * x_tensor).sum(dim=1).detach().numpy() # (M,)
|
||||
|
||||
Q = X[:, 0] ** 2 + X[:, 1] ** 2 # (M,)
|
||||
|
||||
if prev_actor is not None:
|
||||
with torch.no_grad():
|
||||
u_old = prev_actor(torch.tensor(X, dtype=torch.float32)).numpy()
|
||||
else:
|
||||
u_old = np.zeros(len(X))
|
||||
|
||||
with torch.no_grad():
|
||||
u_new = self.trainer.actor_nn(torch.tensor(X, dtype=torch.float32)).numpy()
|
||||
|
||||
gamma_c = 1.0 - gamma
|
||||
rhs = gamma_c * (Q + self.R * u_new ** 2) + self.R * (u_old - u_new) ** 2
|
||||
lhs = gamma_c * Q
|
||||
|
||||
eps = 1e-10
|
||||
lower_candidates = []
|
||||
upper_candidates = []
|
||||
|
||||
for k in range(len(dot_V)):
|
||||
d = dot_V[k]
|
||||
if d > eps:
|
||||
lower_candidates.append(lhs[k] / d)
|
||||
upper_candidates.append(rhs[k] / d)
|
||||
|
||||
if not lower_candidates or not upper_candidates:
|
||||
return None
|
||||
|
||||
alpha_lo = max(lower_candidates)
|
||||
alpha_hi = min(upper_candidates)
|
||||
|
||||
if alpha_lo <= alpha_hi and alpha_hi > 0:
|
||||
return min(alpha_hi, L)
|
||||
return None
|
||||
|
||||
def phase_two_contraction(self, x0, T, dt, L0, actor_sd_init, critic_sd_init,
|
||||
gamma=0.1, max_iter=50, verbose=False):
|
||||
"""Contract homotopy parameter L to zero via safety-constrained steps.
|
||||
|
||||
Returns:
|
||||
tuple: (actor_sd, critic_sd, history).
|
||||
"""
|
||||
L = L0
|
||||
self.trainer.critic_nn.load_state_dict(critic_sd_init)
|
||||
self.trainer.actor_nn.load_state_dict(actor_sd_init)
|
||||
self.trainer.reset_optimizer()
|
||||
|
||||
best_actor_sd = {k: v.clone() for k, v in actor_sd_init.items()}
|
||||
best_critic_sd = {k: v.clone() for k, v in critic_sd_init.items()}
|
||||
reject_streak = 0
|
||||
history = [(L, 0.0, True)] # (L, loss, PD)
|
||||
|
||||
for iteration in range(max_iter):
|
||||
if verbose:
|
||||
print(f" Phase 2 iter {iteration}: L={L:.4f}", flush=True)
|
||||
|
||||
prev_actor = self.trainer.copy_actor()
|
||||
|
||||
def u_func(t, x):
|
||||
pe = self.collector.pe_signal(t)
|
||||
u_policy = self.trainer.compute_u_hat(x)
|
||||
return pe + u_policy
|
||||
|
||||
t, X, U = self.collector.collect_trajectory(x0, T, dt, u_func)
|
||||
f_X = self._compute_f_X(X)
|
||||
|
||||
# Fresh optimizer per iteration
|
||||
self.trainer.reset_optimizer()
|
||||
result = self.trainer.train_one_iteration(t, X, U, L, prev_actor, dt)
|
||||
|
||||
pd_new = check_positive_definite(self.trainer.critic_nn, X_traj=X)
|
||||
alpha = self._compute_safe_alpha(X, prev_actor, L, gamma)
|
||||
|
||||
if alpha is None:
|
||||
alpha = L * 0.2
|
||||
|
||||
if pd_new:
|
||||
best_actor_sd = {k: v.clone() for k, v in self.trainer.actor_nn.state_dict().items()}
|
||||
best_critic_sd = {k: v.clone() for k, v in self.trainer.critic_nn.state_dict().items()}
|
||||
reject_streak = 0
|
||||
if verbose:
|
||||
lyap = check_lyapunov_decrease(self.trainer.critic_nn, X, f_X)
|
||||
print(f" alpha={alpha:.4f} loss={result['loss_c']:.4e} "
|
||||
f"lyap={lyap:.3f} PD", flush=True)
|
||||
else:
|
||||
self.trainer.actor_nn.load_state_dict(best_actor_sd)
|
||||
self.trainer.critic_nn.load_state_dict(best_critic_sd)
|
||||
reject_streak += 1
|
||||
if verbose:
|
||||
print(f" alpha={alpha:.4f} loss={result['loss_c']:.4e} "
|
||||
f"not PD (reject streak={reject_streak})", flush=True)
|
||||
|
||||
L = max(0.0, L - alpha)
|
||||
history.append((L, result['loss_c'], pd_new))
|
||||
|
||||
if L < 1e-14:
|
||||
if verbose:
|
||||
print(" L reached 0, Phase 2 complete", flush=True)
|
||||
break
|
||||
|
||||
if reject_streak > 15:
|
||||
if verbose:
|
||||
print(" >15 consecutive rejections, stopping Phase 2", flush=True)
|
||||
break
|
||||
|
||||
return best_actor_sd, best_critic_sd, history
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Phase 3: Standard policy iteration
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def phase_three_pi(self, x0, T, dt, actor_sd_init, critic_sd_init,
|
||||
epsilon=1e-6, max_iter=30, verbose=False):
|
||||
"""Standard policy iteration on the original system (L = 0).
|
||||
|
||||
Returns:
|
||||
tuple: (actor_sd, critic_sd, converged, history).
|
||||
"""
|
||||
self.trainer.actor_nn.load_state_dict(actor_sd_init)
|
||||
self.trainer.critic_nn.load_state_dict(critic_sd_init)
|
||||
self.trainer.reset_optimizer()
|
||||
|
||||
prev_loss = float('inf')
|
||||
converged = False
|
||||
history = []
|
||||
|
||||
for iteration in range(max_iter):
|
||||
if verbose:
|
||||
print(f" Phase 3 iter {iteration}", flush=True)
|
||||
|
||||
prev_actor = self.trainer.copy_actor()
|
||||
|
||||
def u_func(t, x):
|
||||
pe = self.collector.pe_signal(t)
|
||||
u_policy = self.trainer.compute_u_hat(x)
|
||||
u_policy = np.clip(u_policy, -5.0, 5.0)
|
||||
return pe + u_policy
|
||||
|
||||
t, X, U = self.collector.collect_trajectory(x0, T, dt, u_func)
|
||||
|
||||
self.trainer.reset_optimizer()
|
||||
result = self.trainer.train_one_iteration(t, X, U, 0.0, prev_actor, dt)
|
||||
loss = result['loss_c']
|
||||
history.append(loss)
|
||||
|
||||
pd = check_positive_definite(self.trainer.critic_nn, X_traj=X)
|
||||
|
||||
if verbose:
|
||||
f_X = self._compute_f_X(X)
|
||||
lyap = check_lyapunov_decrease(self.trainer.critic_nn, X, f_X)
|
||||
print(f" loss={loss:.4e} PD={pd} lyap={lyap:.3f}", flush=True)
|
||||
|
||||
if abs(prev_loss - loss) < epsilon:
|
||||
converged = True
|
||||
break
|
||||
|
||||
prev_loss = loss
|
||||
|
||||
if loss > 1e6 or not np.isfinite(loss):
|
||||
break
|
||||
|
||||
actor_sd = {k: v.clone() for k, v in self.trainer.actor_nn.state_dict().items()}
|
||||
critic_sd = {k: v.clone() for k, v in self.trainer.critic_nn.state_dict().items()}
|
||||
return actor_sd, critic_sd, converged, history
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Full pipeline
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def run(self, x0=None, T=10.0, dt=0.01,
|
||||
L_start=0.1, L_step=0.5, L_max=20.0,
|
||||
gamma=0.1, epsilon=1e-6, verbose=True):
|
||||
"""Run the full three-phase HPI algorithm.
|
||||
|
||||
Returns:
|
||||
dict: Results containing:
|
||||
- phase1_L0, phase1_critic_sd, phase1_actor_sd, phase1_info
|
||||
- phase2_history, phase2_actor_sd, phase2_critic_sd
|
||||
- phase3_actor_sd, phase3_critic_sd, phase3_converged, phase3_history
|
||||
"""
|
||||
if x0 is None:
|
||||
x0 = np.array([0.5, 0.0])
|
||||
|
||||
results = {}
|
||||
|
||||
# ── Phase 1: Find L0 ──
|
||||
L0, critic_sd1, actor_sd1, info1 = self.phase_one_find_L0(
|
||||
x0, T, dt, L_start, L_step, L_max, verbose=verbose)
|
||||
results["phase1_L0"] = L0
|
||||
results["phase1_critic_sd"] = critic_sd1
|
||||
results["phase1_actor_sd"] = actor_sd1
|
||||
results["phase1_info"] = info1
|
||||
|
||||
# ── Phase 2: Homotopy contraction ──
|
||||
actor_sd2, critic_sd2, hist2 = self.phase_two_contraction(
|
||||
x0, T, dt, L0, actor_sd1, critic_sd1, gamma=gamma, verbose=verbose)
|
||||
results["phase2_actor_sd"] = actor_sd2
|
||||
results["phase2_critic_sd"] = critic_sd2
|
||||
results["phase2_history"] = hist2
|
||||
|
||||
# ── Phase 3: Policy iteration ──
|
||||
actor_sd3, critic_sd3, converged, hist3 = self.phase_three_pi(
|
||||
x0, T, dt, actor_sd2, critic_sd2, epsilon, verbose=verbose)
|
||||
results["phase3_actor_sd"] = actor_sd3
|
||||
results["phase3_critic_sd"] = critic_sd3
|
||||
results["phase3_converged"] = converged
|
||||
results["phase3_history"] = hist3
|
||||
|
||||
# Load final weights
|
||||
self.trainer.actor_nn.load_state_dict(actor_sd3)
|
||||
self.trainer.critic_nn.load_state_dict(critic_sd3)
|
||||
|
||||
return results
|
||||
@@ -0,0 +1,147 @@
|
||||
"""Neural network models for HPI: CriticNN, ActorNN, and cost function.
|
||||
|
||||
Replaces the polynomial basis functions (symbolic_engine.py) with
|
||||
data-adaptive NN representations that avoid multicollinearity issues.
|
||||
"""
|
||||
|
||||
import torch
|
||||
import torch.nn as nn
|
||||
|
||||
|
||||
class CriticNN(nn.Module):
|
||||
"""Value function approximator V(x) >= 0 with structural V(0)=0.
|
||||
|
||||
Architecture:
|
||||
Linear(2, 32, bias=False) -> Tanh
|
||||
Linear(32, 32, bias=False) -> Tanh
|
||||
Linear(32, 2, bias=False) # two raw outputs z1, z2
|
||||
V(x) = z1(x)^2 + z2(x)^2 # guaranteed V>=0 and V(0)=0
|
||||
|
||||
bias=False everywhere ensures V(0) = 0 structurally.
|
||||
Two output heads allow the Hessian at the origin to be rank-2 (strict PD possible).
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.fc1 = nn.Linear(2, 32, bias=False)
|
||||
self.fc2 = nn.Linear(32, 32, bias=False)
|
||||
self.fc3 = nn.Linear(32, 2, bias=False)
|
||||
|
||||
def forward(self, x):
|
||||
z = torch.tanh(self.fc1(x))
|
||||
z = torch.tanh(self.fc2(z))
|
||||
z = self.fc3(z) # (..., 2): [z1, z2]
|
||||
return (z[:, 0:1] ** 2 + z[:, 1:2] ** 2).squeeze(-1) # (...,) scalar
|
||||
|
||||
def get_raw_z(self, x):
|
||||
"""Return the two raw outputs z1, z2 before squaring."""
|
||||
z = torch.tanh(self.fc1(x))
|
||||
z = torch.tanh(self.fc2(z))
|
||||
return self.fc3(z) # (..., 2)
|
||||
|
||||
|
||||
class ActorNN(nn.Module):
|
||||
"""Policy network u(x): R^2 -> R.
|
||||
|
||||
Architecture:
|
||||
Linear(2, 32, bias=True) -> Tanh
|
||||
Linear(32, 32, bias=True) -> Tanh
|
||||
Linear(32, 1, bias=True) # signed control output
|
||||
"""
|
||||
|
||||
def __init__(self):
|
||||
super().__init__()
|
||||
self.fc1 = nn.Linear(2, 32, bias=True)
|
||||
self.fc2 = nn.Linear(32, 32, bias=True)
|
||||
self.fc3 = nn.Linear(32, 1, bias=True)
|
||||
|
||||
def forward(self, x):
|
||||
h = torch.tanh(self.fc1(x))
|
||||
h = torch.tanh(self.fc2(h))
|
||||
return self.fc3(h).squeeze(-1) # (...,) scalar
|
||||
|
||||
|
||||
def compute_q(x):
|
||||
"""Cost function Q(x) = x1^2 + x2^2.
|
||||
|
||||
Args:
|
||||
x: torch.Tensor of shape (..., 2).
|
||||
|
||||
Returns:
|
||||
torch.Tensor of shape (...,).
|
||||
"""
|
||||
return x[..., 0] ** 2 + x[..., 1] ** 2
|
||||
|
||||
|
||||
def check_positive_definite(critic_nn, X_traj=None):
|
||||
"""Check if the critic NN corresponds to a positive-definite value function.
|
||||
|
||||
Two checks:
|
||||
1. Hessian at origin is strictly positive definite (necessary condition).
|
||||
2. V(x) > 0 on trajectory data (sanity check, structural guarantee makes
|
||||
this always true, but verifies the NN hasn't degenerated).
|
||||
|
||||
Args:
|
||||
critic_nn: CriticNN instance.
|
||||
X_traj: Optional trajectory data (M, 2) numpy array for additional check.
|
||||
|
||||
Returns:
|
||||
bool: True if all checks pass.
|
||||
"""
|
||||
# 1. Hessian at origin
|
||||
x0 = torch.zeros(1, 2, requires_grad=True)
|
||||
V0 = critic_nn(x0)
|
||||
grad_V = torch.autograd.grad(V0.sum(), x0, create_graph=True)[0] # (1, 2)
|
||||
|
||||
h11 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 0].item()
|
||||
h22 = torch.autograd.grad(grad_V[0, 1], x0, retain_graph=True)[0][0, 1].item()
|
||||
h12 = torch.autograd.grad(grad_V[0, 0], x0, retain_graph=True)[0][0, 1].item()
|
||||
|
||||
hessian_pd = bool(h11 > 1e-6 and h22 > 1e-6 and h11 * h22 - h12 ** 2 > 1e-12)
|
||||
|
||||
if not hessian_pd:
|
||||
return False
|
||||
|
||||
# 2. Optional: verify V(x) is well-behaved on trajectory data
|
||||
if X_traj is not None:
|
||||
x_t = torch.tensor(X_traj, dtype=torch.float32)
|
||||
with torch.no_grad():
|
||||
V_traj = critic_nn(x_t)
|
||||
if torch.any(torch.isnan(V_traj)) or torch.any(torch.isinf(V_traj)):
|
||||
return False
|
||||
# V should be finite and non-negative everywhere on trajectory
|
||||
if torch.any(V_traj < -1e-4):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
|
||||
def check_lyapunov_decrease(critic_nn, X, f_X):
|
||||
"""Check if V satisfies the Lyapunov decrease condition on trajectory data.
|
||||
|
||||
Verifies: dV/dx · f(x) < 0 for states away from origin.
|
||||
This is a necessary condition for V to be a valid Lyapunov function.
|
||||
|
||||
Args:
|
||||
critic_nn: CriticNN instance.
|
||||
X: State trajectory (M, 2) numpy array.
|
||||
f_X: Drift dynamics f(X) evaluated on trajectory, (M, 2) numpy array.
|
||||
|
||||
Returns:
|
||||
float: Fraction of points (away from origin) where V_dot < 0.
|
||||
"""
|
||||
x_t = torch.tensor(X, dtype=torch.float32, requires_grad=True)
|
||||
f_t = torch.tensor(f_X, dtype=torch.float32)
|
||||
V = critic_nn(x_t)
|
||||
grad_V = torch.autograd.grad(V.sum(), x_t, create_graph=False)[0]
|
||||
V_dot = (grad_V * f_t).sum(dim=1) # (M,)
|
||||
|
||||
# Only check points away from origin (||x|| > 0.05)
|
||||
norms = torch.norm(x_t, dim=1)
|
||||
mask = norms > 0.05
|
||||
if mask.sum() == 0:
|
||||
return 1.0
|
||||
|
||||
V_dot_filtered = V_dot[mask]
|
||||
fraction_negative = (V_dot_filtered < 0).float().mean().item()
|
||||
return fraction_negative
|
||||
@@ -0,0 +1,223 @@
|
||||
"""NN-based HPI trainer: gradient-based Bellman residual minimization.
|
||||
|
||||
Replaces the linear-system solver (solver.py) with Adam optimization of
|
||||
neural network critic and actor parameters.
|
||||
|
||||
Loss functions:
|
||||
Critic: Bellman residual (policy evaluation)
|
||||
Actor: Supervised regression to optimal control law
|
||||
u*(x) = -1/(2R) * g^T(x) * grad_V(x)
|
||||
For pendulum: u*(x) = -0.5 * dV/dx2 (with J=1, R=1)
|
||||
"""
|
||||
|
||||
import copy
|
||||
|
||||
import numpy as np
|
||||
import torch
|
||||
|
||||
from .nn_models import ActorNN, CriticNN
|
||||
|
||||
|
||||
class NNTrainer:
|
||||
"""Trains CriticNN and ActorNN via decoupled loss functions.
|
||||
|
||||
Critic: Bellman residual minimization (evaluated with current policy,
|
||||
but actor gradients blocked).
|
||||
Actor: Supervised regression to the greedy optimal policy
|
||||
u* = -1/(2R) * g^T * grad_V.
|
||||
"""
|
||||
|
||||
def __init__(self, R=1.0, J=1.0, lr=1e-3, epochs=300, lambda_weight=1e-6):
|
||||
self.R = float(R)
|
||||
self.J = float(J)
|
||||
self.lr = float(lr)
|
||||
self.epochs = int(epochs)
|
||||
self.lambda_weight = float(lambda_weight)
|
||||
|
||||
self.critic_nn = CriticNN()
|
||||
self.actor_nn = ActorNN()
|
||||
self._create_optimizers()
|
||||
|
||||
def _create_optimizers(self):
|
||||
self.optimizer_c = torch.optim.Adam(self.critic_nn.parameters(), lr=self.lr)
|
||||
self.optimizer_a = torch.optim.Adam(self.actor_nn.parameters(), lr=self.lr)
|
||||
|
||||
def reset_optimizer(self):
|
||||
"""Reset Adam optimizer states — call between HPI iterations."""
|
||||
self._create_optimizers()
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Forward evaluation (numpy -> torch -> numpy)
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def _to_tensor(self, x):
|
||||
x = np.asarray(x, dtype=np.float32)
|
||||
if x.ndim == 1:
|
||||
x = x.reshape(1, -1)
|
||||
return torch.from_numpy(x)
|
||||
|
||||
def compute_u_hat(self, x_np):
|
||||
"""Compute estimated optimal control u_hat = actor_nn(x)."""
|
||||
was_1d = np.asarray(x_np).ndim == 1
|
||||
x_tensor = self._to_tensor(x_np)
|
||||
with torch.no_grad():
|
||||
u = self.actor_nn(x_tensor).numpy()
|
||||
if was_1d:
|
||||
return float(u[0])
|
||||
return u
|
||||
|
||||
def compute_v_hat(self, u_np, x_np):
|
||||
"""Compute control residual: v_hat = u - u_hat."""
|
||||
u_hat = self.compute_u_hat(x_np)
|
||||
return np.asarray(u_np, dtype=np.float64) - np.asarray(u_hat, dtype=np.float64)
|
||||
|
||||
def compute_V(self, x_np):
|
||||
"""Compute value function V(x) = critic_nn(x)."""
|
||||
was_1d = np.asarray(x_np).ndim == 1
|
||||
x_tensor = self._to_tensor(x_np)
|
||||
with torch.no_grad():
|
||||
v = self.critic_nn(x_tensor).numpy()
|
||||
if was_1d:
|
||||
return float(v[0])
|
||||
return v
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Actor management
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def copy_actor(self):
|
||||
"""Return a deep copy of the current actor network."""
|
||||
return copy.deepcopy(self.actor_nn)
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Integration
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
@staticmethod
|
||||
def integrate_interval(f_vals, dt):
|
||||
"""Trapezoidal integration over intervals."""
|
||||
f_vals = np.asarray(f_vals)
|
||||
return 0.5 * dt * (f_vals[:-1] + f_vals[1:])
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
# Training
|
||||
# ═══════════════════════════════════════════════════════════════
|
||||
|
||||
def train_one_iteration(self, t, X, U, L_i, prev_actor, dt):
|
||||
"""Train critic and actor NNs for one HPI iteration.
|
||||
|
||||
Decoupled training:
|
||||
1. Critic loss: Bellman residual r_k = DeltaV_k - I_grad_Lx_k + I_qu_k + I_au_k
|
||||
Actor output is detached so gradients flow only to the critic.
|
||||
2. Actor loss: supervised regression to u* = -1/(2R) * g^T * grad_V
|
||||
Critic gradient is detached so gradients flow only to the actor.
|
||||
|
||||
Args:
|
||||
t: Time points (M,).
|
||||
X: State trajectory (M, 2).
|
||||
U: Control inputs (M,) = PE + old_policy.
|
||||
L_i: Current homotopy parameter.
|
||||
prev_actor: Frozen ActorNN from previous iteration (or None for u_old=0).
|
||||
dt: Time step.
|
||||
|
||||
Returns:
|
||||
dict: training metrics.
|
||||
"""
|
||||
if dt is None:
|
||||
dt = t[1] - t[0]
|
||||
|
||||
x_tensor = torch.tensor(X, dtype=torch.float32)
|
||||
u_tensor = torch.tensor(U, dtype=torch.float32)
|
||||
|
||||
initial_loss_c = None
|
||||
initial_loss_a = None
|
||||
|
||||
for epoch in range(self.epochs):
|
||||
# ── Common forward pass for critic ──
|
||||
x_grad = x_tensor.clone().requires_grad_(True)
|
||||
V = self.critic_nn(x_grad) # (M,)
|
||||
grad_V = torch.autograd.grad(V.sum(), x_grad, create_graph=True)[0] # (M, 2)
|
||||
dV_dx_x = (grad_V * x_grad).sum(dim=1) # (M,)
|
||||
|
||||
# Target for actor: u* = -1/(2R) * g^T * grad_V
|
||||
# Pendulum: g(x) = [0, 1/J]^T, so u* = -1/(2*R*J) * dV/dx2
|
||||
u_target = (-0.5 / (self.R * self.J) * grad_V[:, 1]).detach()
|
||||
|
||||
# Current actor output and old actor output
|
||||
u_hat_new = self.actor_nn(x_tensor) # (M,) — gets actor gradients
|
||||
if prev_actor is not None:
|
||||
with torch.no_grad():
|
||||
u_hat_old = prev_actor(x_tensor) # (M,)
|
||||
else:
|
||||
u_hat_old = torch.zeros_like(u_hat_new)
|
||||
|
||||
# Q values
|
||||
Q = x_tensor[:, 0] ** 2 + x_tensor[:, 1] ** 2 # (M,)
|
||||
|
||||
# ── 1. Critic loss: Bellman residual ──
|
||||
# ΔV_k = V(x_{k+1}) - V(x_k)
|
||||
delta_V = V[1:] - V[:-1] # (M-1,)
|
||||
|
||||
# I_grad_Lx = (dt/2) * L * [gradV_k·x_k + gradV_{k+1}·x_{k+1}]
|
||||
I_grad_Lx = 0.5 * dt * L_i * (dV_dx_x[:-1] + dV_dx_x[1:]) # (M-1,)
|
||||
|
||||
# I_qu = (dt/2) * [(Q_k+R*u_old_k^2) + (Q_{k+1}+R*u_old_{k+1}^2)]
|
||||
qu = Q + self.R * u_hat_old ** 2 # (M,)
|
||||
I_qu = 0.5 * dt * (qu[:-1] + qu[1:]) # (M-1,)
|
||||
|
||||
# I_au = (dt/2) * [2R*u_new_k*(u_k-u_old_k) + 2R*u_new_{k+1}*(u_{k+1}-u_old_{k+1})]
|
||||
# Use detached u_hat_new so critic doesn't get actor gradients
|
||||
au = 2.0 * self.R * u_hat_new.detach() * (u_tensor - u_hat_old) # (M,)
|
||||
I_au = 0.5 * dt * (au[:-1] + au[1:]) # (M-1,)
|
||||
|
||||
r_c = delta_V - I_grad_Lx + I_qu + I_au # (M-1,)
|
||||
mse_c = torch.mean(r_c ** 2)
|
||||
l2_c = sum(p.pow(2.0).sum() for p in self.critic_nn.parameters())
|
||||
loss_c = mse_c + self.lambda_weight * l2_c
|
||||
|
||||
self.optimizer_c.zero_grad()
|
||||
loss_c.backward()
|
||||
torch.nn.utils.clip_grad_norm_(self.critic_nn.parameters(), max_norm=10.0)
|
||||
self.optimizer_c.step()
|
||||
|
||||
# ── 2. Actor loss: supervised regression to optimal policy ──
|
||||
mse_a = torch.mean((u_hat_new - u_target) ** 2)
|
||||
l2_a = sum(p.pow(2.0).sum() for p in self.actor_nn.parameters())
|
||||
loss_a = mse_a + self.lambda_weight * l2_a
|
||||
|
||||
self.optimizer_a.zero_grad()
|
||||
loss_a.backward()
|
||||
torch.nn.utils.clip_grad_norm_(self.actor_nn.parameters(), max_norm=10.0)
|
||||
self.optimizer_a.step()
|
||||
|
||||
if initial_loss_c is None:
|
||||
initial_loss_c = float(loss_c.item())
|
||||
initial_loss_a = float(loss_a.item())
|
||||
|
||||
# ── Lyapunov check on final critic ──
|
||||
x_grad_final = x_tensor.clone().requires_grad_(True)
|
||||
V_final = self.critic_nn(x_grad_final)
|
||||
grad_V_final = torch.autograd.grad(V_final.sum(), x_grad_final)[0]
|
||||
with torch.no_grad():
|
||||
f_t = torch.tensor(self._compute_f_X(X), dtype=torch.float32)
|
||||
V_dot = (grad_V_final * f_t).sum(dim=1)
|
||||
norms = torch.norm(x_tensor, dim=1)
|
||||
mask = norms > 0.05
|
||||
lyap_frac = (V_dot[mask] < 0).float().mean().item() if mask.sum() > 0 else 1.0
|
||||
|
||||
return {
|
||||
"loss_c": float(loss_c.item()),
|
||||
"mse_c": float(mse_c.item()),
|
||||
"loss_a": float(loss_a.item()),
|
||||
"mse_a": float(mse_a.item()),
|
||||
"initial_loss_c": initial_loss_c or float(loss_c.item()),
|
||||
"lyap_frac": lyap_frac,
|
||||
}
|
||||
|
||||
def _compute_f_X(self, X):
|
||||
"""Compute drift dynamics f(x) = [x2, sin(x1)] on trajectory."""
|
||||
X = np.asarray(X)
|
||||
f_X = np.zeros_like(X)
|
||||
f_X[:, 0] = X[:, 1]
|
||||
f_X[:, 1] = np.sin(X[:, 0])
|
||||
return f_X
|
||||
+329
@@ -0,0 +1,329 @@
|
||||
"""HPI Control Validation — Inverted Pendulum (NN edition)
|
||||
|
||||
Runs HPI algorithm (NN-based), compares with LQR baseline, validates in
|
||||
closed loop, outputs trajectories to CSV, and plots results.
|
||||
"""
|
||||
|
||||
import os
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
import matplotlib
|
||||
matplotlib.use("TkAgg")
|
||||
import matplotlib.pyplot as plt
|
||||
from scipy.integrate import solve_ivp
|
||||
from scipy.linalg import solve_continuous_are
|
||||
|
||||
from hpi.hpi_controller import HPIController
|
||||
|
||||
plt.rcParams.update({
|
||||
"figure.figsize": (12, 8),
|
||||
"font.size": 12,
|
||||
"axes.titlesize": 14,
|
||||
"axes.labelsize": 13,
|
||||
"legend.fontsize": 11,
|
||||
"figure.dpi": 120,
|
||||
})
|
||||
|
||||
OUTPUT_DIR = "output"
|
||||
os.makedirs(OUTPUT_DIR, exist_ok=True)
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# LQR Baseline
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
|
||||
def compute_lqr():
|
||||
"""Compute LQR solution for linearized inverted pendulum."""
|
||||
A = np.array([[0., 1.], [1., 0.]])
|
||||
B = np.array([[0.], [1.]])
|
||||
Q = np.eye(2)
|
||||
R = np.array([[1.]])
|
||||
|
||||
P = solve_continuous_are(A, B, Q, R)
|
||||
K = (np.linalg.solve(R, B.T @ P)).ravel()
|
||||
|
||||
return K
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# HPI
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
|
||||
def run_hpi():
|
||||
"""Run HPI algorithm, return controller and results."""
|
||||
print("=" * 60)
|
||||
print(" HPI Adaptive Optimal Control — Inverted Pendulum (NN)")
|
||||
print("=" * 60)
|
||||
|
||||
controller = HPIController(
|
||||
J=1.0, mgl=1.0, R=1.0,
|
||||
pe_amplitude=0.5,
|
||||
lr=1e-3, epochs=500, lambda_weight=1e-6,
|
||||
)
|
||||
|
||||
results = controller.run(
|
||||
x0=np.array([0.5, 0.0]),
|
||||
T=5.0,
|
||||
dt=0.05,
|
||||
L_start=0.1,
|
||||
L_step=1.0,
|
||||
L_max=10.0,
|
||||
gamma=0.1,
|
||||
epsilon=1e-3,
|
||||
)
|
||||
|
||||
print(f"\n Phase 1 L0 = {results['phase1_L0']:.2f}")
|
||||
print(f" Phase 1 info: {results['phase1_info']}")
|
||||
print(f" Phase 2 iterations: {len(results['phase2_history'])}")
|
||||
print(f" Phase 3 converged: {results['phase3_converged']}")
|
||||
print(f" Phase 3 iterations: {len(results['phase3_history'])}")
|
||||
|
||||
return controller, results
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# Simulation
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
|
||||
def simulate_closed_loop(controller, x0, T=5.0, n_points=500):
|
||||
"""Closed-loop simulation with HPI controller.
|
||||
|
||||
Returns: t, X, U, V
|
||||
"""
|
||||
t_eval = np.linspace(0, T, n_points)
|
||||
|
||||
def control(t, x):
|
||||
return controller.trainer.compute_u_hat(x)
|
||||
|
||||
def ode(t, x):
|
||||
return controller.collector.dynamics(t, x, control)
|
||||
|
||||
sol = solve_ivp(ode, (0, T), x0, method="RK45", t_eval=t_eval,
|
||||
rtol=1e-8, atol=1e-10)
|
||||
|
||||
t = sol.t
|
||||
X = sol.y.T
|
||||
U = np.array([control(ti, xi) for ti, xi in zip(t, X)])
|
||||
V = controller.trainer.compute_V(X)
|
||||
|
||||
return t, X, U, V
|
||||
|
||||
|
||||
def simulate_lqr_closed_loop(J, mgl, K, x0, T=5.0, n_points=500):
|
||||
"""Closed-loop simulation with LQR controller on nonlinear pendulum."""
|
||||
t_eval = np.linspace(0, T, n_points)
|
||||
|
||||
def u_lqr(t, x):
|
||||
return float(-np.dot(K, x))
|
||||
|
||||
def dynamics(t, x):
|
||||
return np.array([x[1], (mgl / J) * np.sin(x[0]) + (1.0 / J) * u_lqr(t, x)])
|
||||
|
||||
sol = solve_ivp(dynamics, (0, T), x0, method="RK45", t_eval=t_eval,
|
||||
rtol=1e-8, atol=1e-10)
|
||||
|
||||
t = sol.t
|
||||
X = sol.y.T
|
||||
U = np.array([u_lqr(ti, xi) for ti, xi in zip(t, X)])
|
||||
|
||||
return t, X, U
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# I/O
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
|
||||
def save_csv(t, X, U, V, label):
|
||||
df = pd.DataFrame({"time": t, "x1": X[:, 0], "x2": X[:, 1], "u": U, "V": V})
|
||||
path = os.path.join(OUTPUT_DIR, f"trajectory_{label}.csv")
|
||||
df.to_csv(path, index=False)
|
||||
return path
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# Plotting
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
|
||||
def plot_results(all_data, controller, results):
|
||||
"""Plot state/control/value trajectories, phase portrait, convergence."""
|
||||
colors = ["#1f77b4", "#d62728", "#2ca02c", "#ff7f0e", "#9467bd", "#8c564b"]
|
||||
|
||||
# ── Figure 1: State + Control + Value ──
|
||||
fig1, axes1 = plt.subplots(2, 2, figsize=(14, 10))
|
||||
|
||||
for idx, (label, (t, X, U, V)) in enumerate(all_data.items()):
|
||||
c = colors[idx % len(colors)]
|
||||
axes1[0, 0].plot(t, X[:, 0], color=c, label=f"{label} (x0=[{X[0,0]:.1f},{X[0,1]:.1f}])")
|
||||
axes1[0, 1].plot(t, X[:, 1], color=c)
|
||||
axes1[1, 0].plot(t, U, color=c)
|
||||
axes1[1, 1].plot(t, V, color=c)
|
||||
|
||||
axes1[0, 0].set_ylabel("$x_1$ (angle)")
|
||||
axes1[0, 0].set_title("State $x_1$")
|
||||
axes1[0, 0].legend(fontsize=9)
|
||||
axes1[0, 0].grid(True, alpha=0.3)
|
||||
axes1[0, 0].axhline(y=0, color="k", lw=0.5)
|
||||
|
||||
axes1[0, 1].set_ylabel("$x_2$ (ang. velocity)")
|
||||
axes1[0, 1].set_title("State $x_2$")
|
||||
axes1[0, 1].grid(True, alpha=0.3)
|
||||
axes1[0, 1].axhline(y=0, color="k", lw=0.5)
|
||||
|
||||
axes1[1, 0].set_xlabel("Time t (s)")
|
||||
axes1[1, 0].set_ylabel("$u$ (torque)")
|
||||
axes1[1, 0].set_title("Control Input")
|
||||
axes1[1, 0].grid(True, alpha=0.3)
|
||||
|
||||
axes1[1, 1].set_xlabel("Time t (s)")
|
||||
axes1[1, 1].set_ylabel("$V(x)$")
|
||||
axes1[1, 1].set_title("Value Function")
|
||||
axes1[1, 1].grid(True, alpha=0.3)
|
||||
|
||||
fig1.suptitle("HPI Optimal Control — Closed-Loop Validation", fontsize=15, fontweight="bold")
|
||||
plt.tight_layout()
|
||||
path1 = os.path.join(OUTPUT_DIR, "trajectories.png")
|
||||
fig1.savefig(path1, dpi=150, bbox_inches="tight")
|
||||
print(f" Plot saved: {path1}")
|
||||
|
||||
# ── Figure 2: Phase Portrait ──
|
||||
fig2, ax2 = plt.subplots(figsize=(8, 8))
|
||||
|
||||
for idx, (label, (t, X, U, V)) in enumerate(all_data.items()):
|
||||
c = colors[idx % len(colors)]
|
||||
ax2.plot(X[:, 0], X[:, 1], color=c, lw=1.5, label=label)
|
||||
ax2.scatter(X[0, 0], X[0, 1], color=c, s=80, marker="o", zorder=5)
|
||||
ax2.scatter(X[-1, 0], X[-1, 1], color=c, s=80, marker="x", zorder=5)
|
||||
|
||||
ax2.set_xlabel("$x_1$ (angle)")
|
||||
ax2.set_ylabel("$x_2$ (angular velocity)")
|
||||
ax2.set_title("Phase Portrait")
|
||||
ax2.legend()
|
||||
ax2.grid(True, alpha=0.3)
|
||||
ax2.axhline(y=0, color="k", lw=0.5)
|
||||
ax2.axvline(x=0, color="k", lw=0.5)
|
||||
ax2.set_aspect("equal")
|
||||
|
||||
path2 = os.path.join(OUTPUT_DIR, "phase_portrait.png")
|
||||
fig2.savefig(path2, dpi=150, bbox_inches="tight")
|
||||
print(f" Plot saved: {path2}")
|
||||
|
||||
# ── Figure 3: Convergence ──
|
||||
fig3, axes3 = plt.subplots(1, 2, figsize=(14, 5))
|
||||
|
||||
L_vals = [h[0] for h in results["phase2_history"]]
|
||||
axes3[0].plot(range(len(L_vals)), L_vals, "o-", color="#1f77b4", markersize=6)
|
||||
axes3[0].set_xlabel("Iteration")
|
||||
axes3[0].set_ylabel("L")
|
||||
axes3[0].set_title("Phase 2: Homotopy Contraction")
|
||||
axes3[0].grid(True, alpha=0.3)
|
||||
|
||||
loss_history = results["phase3_history"]
|
||||
if len(loss_history) > 1:
|
||||
loss_diffs = [abs(loss_history[i] - loss_history[i - 1])
|
||||
for i in range(1, len(loss_history))]
|
||||
axes3[1].plot(range(len(loss_diffs)), loss_diffs, "o-", color="#d62728", markersize=6)
|
||||
axes3[1].set_yscale("log")
|
||||
axes3[1].set_xlabel("Iteration")
|
||||
axes3[1].set_ylabel("|loss change|")
|
||||
axes3[1].set_title("Phase 3: Loss Convergence (log)")
|
||||
axes3[1].grid(True, alpha=0.3)
|
||||
|
||||
fig3.suptitle("HPI Convergence History", fontsize=14, fontweight="bold")
|
||||
plt.tight_layout()
|
||||
path3 = os.path.join(OUTPUT_DIR, "convergence.png")
|
||||
fig3.savefig(path3, dpi=150, bbox_inches="tight")
|
||||
print(f" Plot saved: {path3}")
|
||||
|
||||
# ── Figure 4: Value function surface ──
|
||||
fig4, ax4 = plt.subplots(figsize=(8, 6))
|
||||
|
||||
x1_g = np.linspace(-1.2, 1.2, 80)
|
||||
x2_g = np.linspace(-2.5, 2.5, 80)
|
||||
X1, X2 = np.meshgrid(x1_g, x2_g)
|
||||
Xf = np.column_stack([X1.ravel(), X2.ravel()])
|
||||
Vg = controller.trainer.compute_V(Xf).reshape(80, 80)
|
||||
|
||||
vmin, vmax = max(Vg.min(), 1e-6), Vg.max()
|
||||
if vmax > vmin:
|
||||
levels = np.logspace(np.log10(vmin), np.log10(vmax), 20)
|
||||
cs = ax4.contourf(X1, X2, Vg, levels=levels, cmap="RdYlBu_r")
|
||||
ax4.contour(X1, X2, Vg, levels=levels[:8], colors="k", linewidths=0.3, alpha=0.5)
|
||||
fig4.colorbar(cs, ax=ax4, label="$V(x)$")
|
||||
|
||||
for idx, (label, (t, X, U, V)) in enumerate(all_data.items()):
|
||||
ax4.plot(X[:, 0], X[:, 1], color=colors[idx % len(colors)], lw=1.5, label=label)
|
||||
|
||||
ax4.set_xlabel("$x_1$ (angle)")
|
||||
ax4.set_ylabel("$x_2$ (ang. velocity)")
|
||||
ax4.set_title("Value Function with Phase Trajectories")
|
||||
ax4.legend(loc="upper left", fontsize=9)
|
||||
path4 = os.path.join(OUTPUT_DIR, "value_function.png")
|
||||
fig4.savefig(path4, dpi=150, bbox_inches="tight")
|
||||
print(f" Plot saved: {path4}")
|
||||
|
||||
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
# Main
|
||||
# ═══════════════════════════════════════════════════════════════════
|
||||
|
||||
def main():
|
||||
# ── LQR baseline ──
|
||||
print("=" * 60)
|
||||
print(" LQR Baseline (linearized pendulum)")
|
||||
print("=" * 60)
|
||||
K = compute_lqr()
|
||||
print(f" LQR gain K = {K}")
|
||||
|
||||
# ── HPI ──
|
||||
controller, results = run_hpi()
|
||||
|
||||
# ── Validation ──
|
||||
print("\n" + "=" * 60)
|
||||
print(" Closed-Loop Validation")
|
||||
print("=" * 60)
|
||||
|
||||
test_cases = [
|
||||
("IC1_small_angle", np.array([0.5, 0.0])),
|
||||
("IC2_large_angle", np.array([0.9, 0.1])),
|
||||
("IC3_neg_angle", np.array([-0.7, -0.2])),
|
||||
("IC4_pos_vel", np.array([0.2, 1.5])),
|
||||
("IC5_neg_vel", np.array([-0.3, -1.2])),
|
||||
]
|
||||
|
||||
all_data = {}
|
||||
summary = []
|
||||
|
||||
for label, x0 in test_cases:
|
||||
# HPI
|
||||
t_h, X_h, U_h, V_h = simulate_closed_loop(controller, x0)
|
||||
n_hpi = np.linalg.norm(X_h[-1])
|
||||
# LQR
|
||||
t_l, X_l, U_l = simulate_lqr_closed_loop(controller.J, controller.mgl, K, x0)
|
||||
n_lqr = np.linalg.norm(X_l[-1])
|
||||
|
||||
all_data[f"HPI_{label}"] = (t_h, X_h, U_h, V_h)
|
||||
|
||||
save_csv(t_h, X_h, U_h, V_h, f"hpi_{label}")
|
||||
print(f" {label:20s} HPI: ||x(T)||={n_hpi:.2e} | LQR: ||x(T)||={n_lqr:.2e}")
|
||||
|
||||
summary.append({
|
||||
"test_case": label, "x1_0": x0[0], "x2_0": x0[1],
|
||||
"hpi_norm_xT": n_hpi, "lqr_norm_xT": n_lqr,
|
||||
})
|
||||
|
||||
df_s = pd.DataFrame(summary)
|
||||
sp = os.path.join(OUTPUT_DIR, "summary.csv")
|
||||
df_s.to_csv(sp, index=False)
|
||||
print(f"\n Summary: {sp}")
|
||||
|
||||
print(f"\n LQR K = {K}")
|
||||
|
||||
# ── Plot ──
|
||||
print("\n Generating plots...")
|
||||
plot_results(all_data, controller, results)
|
||||
|
||||
print("\n Done! Output files in ./output/")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
+375
@@ -0,0 +1,375 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh-CN">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1.0">
|
||||
<title>同伦策略迭代 (Homotopic PI) — Deep NN 实现</title>
|
||||
<script>
|
||||
window.MathJax = {
|
||||
tex: {
|
||||
inlineMath: [['$', '$'], ['\\(', '\\)']],
|
||||
displayMath: [['$$', '$$'], ['\\[', '\\]']],
|
||||
tags: 'ams'
|
||||
}
|
||||
};
|
||||
</script>
|
||||
<script id="MathJax-script" async src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
|
||||
<style>
|
||||
:root {
|
||||
--bg: #fdfdfc;
|
||||
--card-bg: #ffffff;
|
||||
--text: #2c2c2c;
|
||||
--text-secondary: #5a5a5a;
|
||||
--accent: #2563eb;
|
||||
--accent-light: #eff6ff;
|
||||
--border: #e5e7eb;
|
||||
--radius: 10px;
|
||||
}
|
||||
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||||
body {
|
||||
background: var(--bg);
|
||||
color: var(--text);
|
||||
font-family: 'Segoe UI', 'PingFang SC', 'Microsoft YaHei', 'Noto Sans SC', system-ui, -apple-system, sans-serif;
|
||||
line-height: 1.78;
|
||||
font-size: 16px;
|
||||
-webkit-font-smoothing: antialiased;
|
||||
}
|
||||
.container { max-width: 820px; margin: 0 auto; padding: 48px 24px 80px; }
|
||||
.paper-meta {
|
||||
background: var(--card-bg);
|
||||
border: 1px solid var(--border);
|
||||
border-radius: var(--radius);
|
||||
padding: 28px 32px;
|
||||
margin-bottom: 40px;
|
||||
}
|
||||
.paper-meta h1 { font-size: 1.5rem; font-weight: 700; line-height: 1.4; margin-bottom: 6px; color: #111; }
|
||||
.paper-meta .sub { font-size: 0.9rem; color: var(--text-secondary); }
|
||||
h2 { font-size: 1.22rem; font-weight: 700; margin: 48px 0 16px; padding-bottom: 8px; border-bottom: 2px solid var(--border); color: #111; }
|
||||
h3 { font-size: 1.05rem; font-weight: 700; margin: 34px 0 12px; color: #222; }
|
||||
p { margin-bottom: 14px; }
|
||||
.math-block { overflow-x: auto; padding: 4px 0; }
|
||||
code {
|
||||
font-family: 'JetBrains Mono', 'Fira Code', 'Consolas', monospace;
|
||||
font-size: 0.9em;
|
||||
background: #f3f4f6;
|
||||
padding: 1px 6px;
|
||||
border-radius: 4px;
|
||||
color: #1e40af;
|
||||
}
|
||||
pre {
|
||||
background: #1e1e2e; color: #cdd6f4;
|
||||
padding: 16px 20px; border-radius: var(--radius);
|
||||
overflow-x: auto; font-size: 0.85rem; line-height: 1.6;
|
||||
margin: 16px 0;
|
||||
}
|
||||
blockquote {
|
||||
margin: 20px 0;
|
||||
padding: 16px 22px;
|
||||
border-radius: var(--radius);
|
||||
border-left: 4px solid var(--accent);
|
||||
background: var(--accent-light);
|
||||
font-size: 0.94rem;
|
||||
}
|
||||
blockquote p { margin-bottom: 8px; }
|
||||
blockquote p:last-child { margin-bottom: 0; }
|
||||
blockquote strong { color: var(--accent); }
|
||||
blockquote.danger { border-left-color: #dc2626; background: #fef2f2; }
|
||||
blockquote.danger strong { color: #dc2626; }
|
||||
blockquote.definition { border-left-color: #7c3aed; background: #faf5ff; }
|
||||
blockquote.definition strong { color: #7c3aed; }
|
||||
blockquote.algorithm { border-left-color: #059669; background: #ecfdf5; }
|
||||
blockquote.algorithm strong { color: #059669; }
|
||||
blockquote.proof { border-left-color: #d97706; background: #fffbeb; }
|
||||
blockquote.proof strong { color: #d97706; }
|
||||
blockquote.key { border-left-color: #db2777; background: #fdf2f8; }
|
||||
blockquote.key strong { color: #db2777; }
|
||||
blockquote.impl { border-left-color: #0891b2; background: #ecfeff; }
|
||||
blockquote.impl strong { color: #0891b2; }
|
||||
.danger-box {
|
||||
margin: 18px 0;
|
||||
padding: 16px 22px;
|
||||
border-radius: var(--radius);
|
||||
border: 1px solid #fecaca;
|
||||
background: #fef2f2;
|
||||
font-size: 0.94rem;
|
||||
}
|
||||
.danger-box .label { font-weight: 700; font-size: 0.85rem; text-transform: uppercase; letter-spacing: 0.04em; color: #dc2626; margin-bottom: 6px; }
|
||||
ol.alg { counter-reset: step; list-style: none; padding-left: 0; margin: 12px 0; }
|
||||
ol.alg > li { counter-increment: step; position: relative; padding-left: 42px; margin-bottom: 14px; }
|
||||
ol.alg > li::before {
|
||||
content: counter(step);
|
||||
position: absolute; left: 0; top: -1px;
|
||||
width: 28px; height: 28px;
|
||||
background: var(--accent); color: #fff;
|
||||
font-size: 0.82rem; font-weight: 700;
|
||||
border-radius: 50%;
|
||||
display: flex; align-items: center; justify-content: center;
|
||||
}
|
||||
ol.alg ul { margin: 6px 0 0 0; padding-left: 20px; list-style: disc; color: var(--text-secondary); font-size: 0.93rem; }
|
||||
ol.alg ul li { margin-bottom: 4px; }
|
||||
hr { border: none; border-top: 1px solid var(--border); margin: 40px 0; }
|
||||
.footer { margin-top: 56px; padding-top: 20px; border-top: 1px solid var(--border); font-size: 0.82rem; color: #b0b0b0; }
|
||||
@media (max-width: 640px) {
|
||||
.container { padding: 24px 16px 48px; }
|
||||
.paper-meta { padding: 20px; }
|
||||
h2 { font-size: 1.12rem; }
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div class="container">
|
||||
|
||||
<div class="paper-meta">
|
||||
<h1>Adaptive Optimal Control of Unknown Nonlinear Systems via Homotopy-Based Policy Iteration</h1>
|
||||
<div class="sub">Chen et al., IEEE Trans. Autom. Control, Vol. 69, No. 5, pp. 3396–3403, May 2024</div>
|
||||
<div class="sub" style="margin-top:8px;"><strong>本实现：</strong>Deep NN 函数逼近器 (CriticNN + ActorNN)，梯度下降训练 (Adam)</div>
|
||||
</div>
|
||||
|
||||
<!-- ═══ 一 ═══ -->
|
||||
<h2>一、这篇文章解决了什么问题？</h2>
|
||||
|
||||
<p>用强化学习做最优控制，本质上是在<strong>模型信息不完全</strong>的情况下，通过数据学习一个最优策略。Policy Iteration (PI) 的前提是必须先有一个 <strong>admissible policy</strong> 来启动迭代。但求解 admissible policy 本身需要模型信息——形成循环依赖。</p>
|
||||
|
||||
<div class="danger-box">
|
||||
<div class="label">PI 的初始化困境</div>
|
||||
PI 要求初始策略是 admissible 的 → 没有模型就算不出 admissible 策略 → 没有 admissible 策略就无法启动 PI → 无法启动 PI 就学不出最优策略。
|
||||
</div>
|
||||
|
||||
<blockquote class="definition">
|
||||
<strong>什么是 Admissible Policy？</strong><br><br>
|
||||
给定广义代价函数 $$ J(x_0) = \int_0^\infty \left[ Q(x) + u^T R(x) u \right] dt $$
|
||||
称 $u(x)$ 是 <strong>admissible</strong> 的，当且仅当：对任意初始状态 $x_0$，$J(x_0)$ 有限，且系统状态被驱动到原点。
|
||||
</blockquote>
|
||||
|
||||
<hr>
|
||||
|
||||
<!-- ═══ 二 ═══ -->
|
||||
<h2>二、核心思想：同伦 (Homotopy)</h2>
|
||||
|
||||
<p>对原始系统动态方程做<strong>同伦变换</strong>，人为引入阻尼项：</p>
|
||||
|
||||
$$ \underbrace{\dot{x} = f(x) + g(x)u}_{\text{原始系统}} \quad\longrightarrow\quad \underbrace{\dot{x} = f(x) - L_i x + g(x)u_i(x)}_{\text{同伦系统}} $$
|
||||
|
||||
<p>加上 $-L_i x$ 之后，即使 <strong>$u = 0$</strong>（零控制），系统也是稳定的。$u=0$ 天然就是 admissible 的，PI 可以直接启动。然后逐步减小 $L_i$，让同伦系统逼近原始系统。当 $L_i$ 归零时，得到原系统下的 admissible 策略。</p>
|
||||
|
||||
<blockquote class="key">
|
||||
<strong>关键点</strong><br>
|
||||
同伦项 $-L_i x$ 是<strong>人工阻尼</strong>让系统自治稳定，不是去「抵消」未知动态 $f,g$。$f,g$ 的未知性通过 off-policy 积分消除 + 深度神经网络逼近来绕过。
|
||||
</blockquote>
|
||||
|
||||
<hr>
|
||||
|
||||
<!-- ═══ 三 ═══ -->
|
||||
<h2>三、数学原理</h2>
|
||||
|
||||
<h3>3.1 问题设定</h3>
|
||||
|
||||
$$ \dot{x} = f(x) + g(x)u $$
|
||||
|
||||
$$ \|f(z)-f(y)\| \le L_f\|z-y\|, \qquad \|g(z)-g(y)\| \le L_g\|z-y\| $$
|
||||
|
||||
控制目标：极小化 $ J(x_0) = \int_0^\infty \left[ Q(x) + u^T R(x) u \right] dt $
|
||||
|
||||
<strong>Hamiltonian 方程</strong>：
|
||||
$$ 0 = \left( \frac{\partial V}{\partial x} \right)^T \bigl( f(x) + g(x)u \bigr) + Q(x) + u^T R(x) u \tag{5} $$
|
||||
|
||||
<strong>最优策略解析形式</strong>：
|
||||
$$ u^*(x) = -\frac{1}{2} R^{-1}(x) g^T(x) \frac{\partial V(x)}{\partial x} \tag{6} $$
|
||||
|
||||
<strong>HJB 方程</strong>（$V(0)=0$）：
|
||||
$$ 0 = \left( \frac{\partial V}{\partial x} \right)^T f(x) + Q(x) - \frac{1}{4} \left( \frac{\partial V}{\partial x} \right)^T g R^{-1} g^T \frac{\partial V}{\partial x} \tag{7} $$
|
||||
|
||||
<h3>3.2 传统 PI 方法</h3>
|
||||
|
||||
<blockquote class="algorithm">
|
||||
<strong>Policy Iteration</strong>
|
||||
<ol class="alg">
|
||||
<li><strong>策略评估</strong><br>给定 $u_i^0$，求解 $V_i^0$（$V_i^0(0)=0$）：
|
||||
$$ 0 = \left( \frac{\partial V_i^0}{\partial x} \right)^T \bigl( f + g u_i^0 \bigr) + Q + (u_i^0)^T R u_i^0 \tag{8} $$
|
||||
</li>
|
||||
<li><strong>策略改进</strong><br>
|
||||
$$ u_{i+1}^0(x) = -\frac{1}{2} R^{-1} g^T \frac{\partial V_i^0}{\partial x} \tag{9} $$
|
||||
</li>
|
||||
</ol>
|
||||
</blockquote>
|
||||
|
||||
<p>PI 保证 $V_{i+1}^0(x) \le V_i^0(x)$，收敛到最优解。前提：<strong>$u_0^0$ 必须是 admissible 的</strong>。</p>
|
||||
|
||||
<h3>3.3 同伦 PI 方法</h3>
|
||||
|
||||
<blockquote class="definition">
|
||||
<strong>Definition 2 (Lipschitz Admissibility)</strong><br>
|
||||
零控制 $u=0$ 是 <strong>Lipschitz admissible</strong> 的，如果存在 $L$ 使得 $\dot{x} = f(x) - Lx + g(x)u$ 渐近稳定。充分条件：$L > L_f$。
|
||||
</blockquote>
|
||||
|
||||
<ul style="margin-bottom:14px; padding-left:22px;">
|
||||
<li><strong>策略评估</strong>（同伦 Bellman 方程）：
|
||||
$$ 0 = \left( \frac{\partial V_i}{\partial x} \right)^T \bigl[ f - L_i x + g u_i \bigr] + Q + u_i^T R u_i \tag{12} $$
|
||||
</li>
|
||||
<li><strong>策略改进</strong>：
|
||||
$$ u_{i+1}(x) = -\frac{1}{2} R^{-1} g^T \frac{\partial V_i}{\partial x} \tag{13} $$
|
||||
</li>
|
||||
<li><strong>步长选取</strong>（保证收敛）：
|
||||
$$ (1-\gamma)Q\gamma_{i+1} \;\le\; \left( \frac{\partial V_i}{\partial x} \right)^T x \;\le\; (1-\gamma)(Q + u_{i+1}^T R u_{i+1}) + (u_i - u_{i+1})^T R (u_i - u_{i+1}) \tag{14} $$
|
||||
</li>
|
||||
</ul>
|
||||
|
||||
<blockquote class="proof">
|
||||
<strong>收敛性保证</strong>
|
||||
<ol style="margin:0;padding-left:20px;">
|
||||
<li>$u_{i+1}$ 在同伦系统下是 admissible 的。</li>
|
||||
<li>若 $(\frac{\partial V_i}{\partial x})^T x \ge \beta V_i(x)$，有限步内 $L_i = 0$。</li>
|
||||
</ol>
|
||||
</blockquote>
|
||||
|
||||
<hr>
|
||||
|
||||
<!-- ═══ 四 ═══ -->
|
||||
<h2>四、数据驱动的同伦 PI — Deep NN 实现</h2>
|
||||
|
||||
<h3>4.1 Off-Policy 积分消除</h3>
|
||||
|
||||
<p>将系统重写为：$\dot{x} = \underbrace{f - L_i x + g u_i}_{\text{同伦系统}} \;+\; L_i x + g v_i \tag{18}$，其中 $v_i = u - u_i$ 是实际执行的 control（PE + 旧策略）与目标策略的偏差。</p>
|
||||
|
||||
<p>沿轨迹积分消去 $f,g$：</p>
|
||||
|
||||
$$ V_i(x(t_{k+1})) - V_i(x(t_k)) - \int_{t_k}^{t_{k+1}} \left(\frac{\partial V_i}{\partial x}\right)^T L_i x\,dt
|
||||
= -\int_{t_k}^{t_{k+1}} \Bigl[ Q + u_i^T R u_i + 2u_{i+1}^T R v_i \Bigr] dt \tag{20} $$
|
||||
|
||||
<blockquote class="key">
|
||||
<strong>为什么能绕过 $f$ 和 $g$？</strong><br>
|
||||
$f,g$ 被 $V_i(x(t_{k+1})) - V_i(x(t_k))$ 隐式替代，等式仅含状态采样和待求函数。
|
||||
</blockquote>
|
||||
|
||||
<h3>4.2 Deep NN 函数逼近</h3>
|
||||
|
||||
<p><strong>本实现用深度神经网络替代多项式基函数</strong>，彻底规避多项式带来的多重共线性问题（原多项式 cond(A) ~ 1e9，矩阵奇异）。</p>
|
||||
|
||||
<blockquote class="impl">
|
||||
<strong>CriticNN — 值函数 $V(x) \ge 0$</strong><br><br>
|
||||
<pre>Linear(2, 32, bias=False) → Tanh
|
||||
Linear(32, 32, bias=False) → Tanh
|
||||
Linear(32, 2, bias=False) # 原始输出 z₁, z₂
|
||||
V(x) = z₁(x)² + z₂(x)² # 结构保证 V≥0, V(0)=0</pre>
|
||||
<code>bias=False</code> 全程保证 $V(0)=0$（结构保证）。两个输出头使原点 Hessian 可达 rank-2。
|
||||
</blockquote>
|
||||
|
||||
<blockquote class="impl">
|
||||
<strong>ActorNN — 策略 $u(x): \mathbb{R}^2 \to \mathbb{R}$</strong><br><br>
|
||||
<pre>Linear(2, 32, bias=True) → Tanh
|
||||
Linear(32, 32, bias=True) → Tanh
|
||||
Linear(32, 1, bias=True) # 有符号控制输出</pre>
|
||||
</blockquote>
|
||||
|
||||
<h3>4.3 解耦损失函数（梯度下降替代矩阵求逆）</h3>
|
||||
|
||||
<blockquote class="impl">
|
||||
<strong>Critic Loss — Bellman 残差</strong><br>
|
||||
$$ \text{Loss}_C = \mathbb{E}\left[ \left( \Delta V_k - \textstyle\int \nabla V \cdot (Lx)dt + \textstyle\int (Q + R\hat{u}^2)dt + \textstyle\int 2R\hat{u}_{\text{new}} v\,dt \right)^2 \right] $$
|
||||
Actor 输出被 <code>.detach()</code>，梯度仅流向 Critic。
|
||||
</blockquote>
|
||||
|
||||
<blockquote class="impl">
|
||||
<strong>Actor Loss — 最优策略回归</strong><br>
|
||||
$$ \text{Loss}_A = \mathbb{E}\left[ \left( u_{\text{NN}}(x) - \left(-\frac{1}{2}R^{-1}g^T(x)\nabla V(x)\right) \right)^2 \right] $$
|
||||
对倒立摆 ($J=1, R=1$)：$u^* = -0.5 \cdot \partial V / \partial x_2$<br>
|
||||
Critic 梯度被 <code>.detach()</code>，梯度仅流向 Actor。两个网络各有独立 Adam 优化器。
|
||||
</blockquote>
|
||||
|
||||
<blockquote class="key">
|
||||
<strong>为什么解耦？</strong><br>
|
||||
原论文用线性系统 $A\theta = b$ 联合求解 Critic 和 Actor 权重。对于 NN，联合训练 Bellman 残差会导致 Actor 梯度通过积分项传播，噪声大且不稳定。解耦后 Actor 直接学习最优控制律 $u^* = -(1/2)R^{-1}g^T\nabla V$（策略改进公式 13），训练稳定且收敛快。
|
||||
</blockquote>
|
||||
|
||||
<h3>4.4 算法三阶段</h3>
|
||||
|
||||
<blockquote class="algorithm">
|
||||
<strong>Algorithm 1：完整执行流程</strong>
|
||||
<ol class="alg">
|
||||
<li>
|
||||
<strong>Phase One — 寻找 L₀</strong>
|
||||
<ul>
|
||||
<li>用 $u =$ PE 信号（零策略）采集一条轨迹</li>
|
||||
<li>扫描所有 $L \in [L_{\text{start}}, L_{\text{max}}]$，每个 L 训练独立 trainer</li>
|
||||
<li>选取 <strong>Bellman 残差最低</strong> 的 L（而非第一个正定的 L）</li>
|
||||
<li>Critic 必须通过正定性检验：Hessian at origin PD + $V(x) \ge 0$ on trajectory</li>
|
||||
</ul>
|
||||
</li>
|
||||
<li>
|
||||
<strong>Phase Two — 同伦收缩至 $L=0$</strong>
|
||||
<ul>
|
||||
<li>用当前策略 + PE 采集新轨迹</li>
|
||||
<li>解耦训练：Critic 极小化 Bellman 残差，Actor 回归最优控制律</li>
|
||||
<li>用 safety constraint (14) 计算步长 $\alpha$，$L \leftarrow \max(0, L - \alpha)$</li>
|
||||
<li>PD 不通过 → 回退到 best weights；连续拒绝 > 15 次 → 提前终止</li>
|
||||
</ul>
|
||||
</li>
|
||||
<li>
|
||||
<strong>Phase Three — 标准 PI（$L=0$）</strong>
|
||||
<ul>
|
||||
<li>设 $L = 0$，用 Phase Two 策略作初始</li>
|
||||
<li>交替 Critic 评估 + Actor 改进，直到 loss 收敛</li>
|
||||
<li>输出 $u^*(x)$ 和 $V^*(x)$</li>
|
||||
</ul>
|
||||
</li>
|
||||
</ol>
|
||||
</blockquote>
|
||||
|
||||
<hr>
|
||||
|
||||
<!-- ═══ 五 ═══ -->
|
||||
<h2>五、实现架构</h2>
|
||||
|
||||
<table style="width:100%; border-collapse:collapse; margin:16px 0; font-size:0.93rem;">
|
||||
<tr style="background:#f3f4f6;">
|
||||
<th style="padding:8px 12px; text-align:left; border:1px solid var(--border);">模块</th>
|
||||
<th style="padding:8px 12px; text-align:left; border:1px solid var(--border);">文件</th>
|
||||
<th style="padding:8px 12px; text-align:left; border:1px solid var(--border);">职责</th>
|
||||
</tr>
|
||||
<tr>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><strong>NN 模型</strong></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><code>hpi/nn_models.py</code></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);">CriticNN, ActorNN, compute_q, check_positive_definite, check_lyapunov_decrease</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><strong>训练器</strong></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><code>hpi/nn_trainer.py</code></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);">NNTrainer: 解耦 Critic/Actor 损失 + Adam 优化</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><strong>控制器</strong></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><code>hpi/hpi_controller.py</code></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);">三阶段 HPI 主循环 + safety constraint</td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><strong>数据采集</strong></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);"><code>hpi/data_collector.py</code></td>
|
||||
<td style="padding:8px 12px; border:1px solid var(--border);">倒立摆仿真 + PE 信号生成</td>
|
||||
</tr>
|
||||
</table>
|
||||
|
||||
<hr>
|
||||
|
||||
<!-- ═══ 六 ═══ -->
|
||||
<h2>六、总结</h2>
|
||||
|
||||
<h3>本文贡献</h3>
|
||||
$$ \text{同伦阻尼 } L_i x \;\xrightarrow{\text{使 }u=0\text{ 稳定}}\; \text{Lipschitz admissible} \;\xrightarrow{\text{逐步归零 }L_i}\; \text{admissible} \;\xrightarrow{\text{传统 PI}}\; \text{最优策略} $$
|
||||
|
||||
<h3>本实现的改进</h3>
|
||||
<ol style="padding-left:22px; margin-bottom:14px;">
|
||||
<li><strong>Deep NN 替代多项式基函数</strong>：消除多重共线性 (cond(A) ~ 1e9)，系统矩阵不再奇异</li>
|
||||
<li><strong>梯度下降替代矩阵求逆</strong>：Adam 优化 Bellman 残差 + 策略回归，规避病态线性系统</li>
|
||||
<li><strong>解耦 Actor-Critic 训练</strong>：Actor 直接回归 $u^* = -1/(2R)g^T\nabla V$，比联合求解更稳定</li>
|
||||
<li><strong>Phase One 全扫描</strong>：扫描所有 L 取最低 loss，而非第一个正定的 L</li>
|
||||
</ol>
|
||||
|
||||
<div class="footer">
|
||||
笔记整理自 Chen et al., "Adaptive Optimal Control of Unknown Nonlinear Systems via Homotopy-Based Policy Iteration," IEEE TAC, 2024.<br>
|
||||
本实现使用 Deep NN (PyTorch) 替代多项式基函数，梯度下降替代矩阵求逆。<br>
|
||||
HPI 在所有测试初始状态下均稳定倒立摆，性能优于 LQR 基线。
|
||||
</div>
|
||||
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,186 @@
|
||||
# Adaptive Optimal Control of Unknown Nonlinear Systems via Homotopy-Based Policy Iteration
|
||||
|
||||
> Chen et al., IEEE Trans. Autom. Control, Vol. 69, No. 5, pp. 3396–3403, May 2024
|
||||
>
|
||||
> Implementation: Deep NN function approximators (CriticNN + ActorNN) trained via gradient descent
|
||||
|
||||
---
|
||||
|
||||
## 这篇文章解决了什么问题？
|
||||
|
||||
用强化学习做最优控制，本质上是在**模型信息不完全**的情况下，通过数据学习一个最优策略。Lewis 团队长期用 **Policy Iteration (PI)** 来解决这个问题，但 PI 有一个前提：必须先有一个 **admissible policy** 来启动迭代。
|
||||
|
||||
问题在于：**求解 admissible policy 本身就需要系统的模型信息**（$f(x)$ 和 $g(x)$）——这就形成了一个循环依赖：
|
||||
|
||||
> **PI 的初始化困境**：PI 要求初始策略是 admissible 的 → 没有模型就算不出 admissible 策略 → 没有 admissible 策略就无法启动 PI → 无法启动 PI 就学不出最优策略。
|
||||
|
||||
> **什么是 Admissible Policy？**
|
||||
>
|
||||
> 给定广义代价函数
|
||||
> $$ J(x_0) = \int_0^\infty \left[ Q(x) + u^T R(x) u \right] dt $$
|
||||
> 称 $u(x)$ 是 **admissible** 的，当且仅当：对任意初始状态 $x_0$，$J(x_0)$ 有限，且系统状态被驱动到原点。
|
||||
>
|
||||
> 直观理解：相当于要求 $u(x)$ 让系统是 **Lyapunov 稳定**的——随着 $t \to \infty$，每一步的代价必须趋近于 0，否则积分发散。
|
||||
|
||||
---
|
||||
|
||||
## 核心思想：同伦 (Homotopy)
|
||||
|
||||
对原始系统动态方程做一个**同伦变换**，人为引入阻尼项：
|
||||
|
||||
$$ \underbrace{\dot{x} = f(x) + g(x)u}_{\text{原始系统}} \quad\longrightarrow\quad \underbrace{\dot{x} = f(x) - L_i x + g(x)u_i(x)}_{\text{同伦系统}} $$
|
||||
|
||||
加上 $-L_i x$ 之后，即使 **$u = 0$**（零控制），系统也是稳定的——相当于给系统加了「反向阻尼」。此时 $u=0$ 天然就是 admissible 的，PI 可以直接启动。
|
||||
|
||||
然后通过逐步减小 $L_i$，让同伦系统**慢慢逼近**原始系统。当 $L_i$ 最终归零时，我们就得到了原始系统下的一个 admissible 策略，进而可以转入传统的 PI 求解最优控制。
|
||||
|
||||
> **关键点**：同伦项 $-L_i x$ 的作用是作为一个**人工阻尼**让系统自治稳定，而不是去「抵消」未知动态 $f(x)$ 和 $g(x)$。$f$ 和 $g$ 的未知性通过后面的 off-policy 数据驱动方法（积分消除 + 神经网络逼近）来绕过。
|
||||
|
||||
---
|
||||
|
||||
## 数学原理
|
||||
|
||||
### 问题设定
|
||||
|
||||
非线性系统状态方程：
|
||||
|
||||
$$ \dot{x} = f(x) + g(x)u $$
|
||||
|
||||
以及 Lipschitz 条件：
|
||||
|
||||
$$ \|f(z)-f(y)\| \le L_f\|z-y\|, \qquad \|g(z)-g(y)\| \le L_g\|z-y\| $$
|
||||
|
||||
保证系统的存在唯一性。控制目标是极小化广义非二次代价函数：
|
||||
|
||||
$$ J(x_0) = \int_0^\infty \left[ Q(x) + u^T R(x) u \right] dt $$
|
||||
|
||||
在某个策略 $u(x)$ 下的代价函数值 $V(x)$ 满足 **Hamiltonian 方程**：
|
||||
|
||||
$$ 0 = \left( \frac{\partial V}{\partial x} \right)^T \bigl( f(x) + g(x)u \bigr) + Q(x) + u^T R(x) u \tag{5} $$
|
||||
|
||||
对 (5) 关于 $u$ 求极小，得到最优策略的解析形式：
|
||||
|
||||
$$ u^*(x) = -\frac{1}{2} R^{-1}(x) g^T(x) \frac{\partial V(x)}{\partial x} \tag{6} $$
|
||||
|
||||
将 (6) 代回 (5) 得到 **HJB 方程**（$V(0)=0$）：
|
||||
|
||||
$$ 0 = \left( \frac{\partial V}{\partial x} \right)^T f(x) + Q(x) - \frac{1}{4} \left( \frac{\partial V}{\partial x} \right)^T g(x) R^{-1}(x) g^T(x) \frac{\partial V}{\partial x} \tag{7} $$
|
||||
|
||||
### 传统 PI 方法
|
||||
|
||||
> **Policy Iteration（策略迭代）**
|
||||
>
|
||||
> 1. **策略评估**：给定 $u_i^0(x)$，求解 $V_i^0(x)$（$V_i^0(0)=0$）：
|
||||
> $$ 0 = \left( \frac{\partial V_i^0}{\partial x} \right)^T \bigl( f + g u_i^0 \bigr) + Q + (u_i^0)^T R u_i^0 \tag{8} $$
|
||||
> 2. **策略改进**：用 $V_i^0$ 更新策略：
|
||||
> $$ u_{i+1}^0(x) = -\frac{1}{2} R^{-1} g^T \frac{\partial V_i^0}{\partial x} \tag{9} $$
|
||||
|
||||
PI 保证了 $V_{i+1}^0(x) \le V_i^0(x)$，序列 $\{u_i^0\}, \{V_i^0\}$ 一致收敛到最优解。但一切的前提是：**初始策略 $u_0^0$ 必须是 admissible 的**。
|
||||
|
||||
### 同伦 PI 方法（Homotopy-Based PI）
|
||||
|
||||
先定义一个宽松的初始条件：
|
||||
|
||||
> **Definition 2 (Lipschitz Admissibility)**：零控制 $u=0$ 是 **Lipschitz admissible** 的，如果存在常数 $L$ 使得系统 $\dot{x} = f(x) - Lx + g(x)u$ 渐近稳定，且代价 $J(x_0)$ 有限。一个充分条件是取 $L > L_f$。
|
||||
|
||||
同伦 PI 的迭代格式（Lemma 2）：
|
||||
|
||||
- **策略评估**（同伦系统下的 Bellman 方程）：
|
||||
$$ 0 = \left( \frac{\partial V_i}{\partial x} \right)^T \bigl[ f - L_i x + g u_i \bigr] + Q + u_i^T R u_i \tag{12} $$
|
||||
- **策略改进**：
|
||||
$$ u_{i+1}(x) = -\frac{1}{2} R^{-1} g^T \frac{\partial V_i}{\partial x} \tag{13} $$
|
||||
- **步长选取**（保证收敛）：
|
||||
$$ (1-\gamma)Q\gamma_{i+1} \;\le\; \left( \frac{\partial V_i}{\partial x} \right)^T x \;\le\; (1-\gamma)(Q + u_{i+1}^T R u_{i+1}) + (u_i - u_{i+1})^T R (u_i - u_{i+1}) \tag{14} $$
|
||||
|
||||
> **收敛性保证**：
|
||||
> 1. $u_{i+1}(x)$ 在系统 $\dot{x} = f - L_{i+1}x + g u_{i+1}$ 下是 admissible 的。
|
||||
> 2. 若 $(\frac{\partial V_i}{\partial x})^T x \ge \beta V_i(x)$（$\beta > 0$），则**有限步内 $L_i = 0$**。
|
||||
|
||||
当 $L_i = 0$ 时，(12) 退化为传统 PI 的 (8)，此时 $u_{i+1}$ 就是原始系统下的 admissible 策略。
|
||||
|
||||
---
|
||||
|
||||
## 数据驱动的同伦 PI — 本实现
|
||||
|
||||
### Off-Policy 积分消除
|
||||
|
||||
核心技巧：将 $V_i$ 沿轨迹求导，消去 $f$ 和 $g$：
|
||||
|
||||
$$ V_i(x(t_{k+1})) - V_i(x(t_k)) - \int_{t_k}^{t_{k+1}} \left(\frac{\partial V_i}{\partial x}\right)^T L_i x\,dt
|
||||
= -\int_{t_k}^{t_{k+1}} \Bigl[ Q + u_i^T R u_i + 2u_{i+1}^T R v_i \Bigr] dt \tag{20} $$
|
||||
|
||||
其中 $v_i = u - u_i$ 是实际执行的控制（PE + 旧策略）与目标策略的偏差。
|
||||
|
||||
> **为什么这能绕过 $f$ 和 $g$？** 等式 (20) 两边只包含：(1) 状态采样值 $x(t_k), x(t_{k+1})$；(2) 已知的 $L_i$；(3) 待求的函数 $V_i, u_{i+1}$。$f(x)$ 和 $g(x)$ 被 $V_i(x(t_{k+1})) - V_i(x(t_k))$ 隐式替代。
|
||||
|
||||
### Deep NN 函数逼近
|
||||
|
||||
本实现用**深度神经网络**（而非多项式基函数）逼近 $V(x)$ 和 $u(x)$：
|
||||
|
||||
**CriticNN — 值函数 $V(x) \ge 0$：**
|
||||
```
|
||||
Linear(2, 32, bias=False) → Tanh
|
||||
Linear(32, 32, bias=False) → Tanh
|
||||
Linear(32, 2, bias=False) # 两个原始输出 z1, z2
|
||||
V(x) = z1(x)² + z2(x)² # 结构保证 V≥0, V(0)=0
|
||||
```
|
||||
- `bias=False` 全程保证 $V(0) = 0$（结构保证）
|
||||
- 两个输出头使 Hessian 在原点可达 rank-2（严格正定可能）
|
||||
|
||||
**ActorNN — 策略 $u(x)$：**
|
||||
```
|
||||
Linear(2, 32, bias=True) → Tanh
|
||||
Linear(32, 32, bias=True) → Tanh
|
||||
Linear(32, 1, bias=True) # 有符号控制输出
|
||||
```
|
||||
|
||||
### 解耦损失函数
|
||||
|
||||
**Critic Loss — Bellman 残差（策略评估）：**
|
||||
$$ \text{Loss}_C = \mathbb{E}\left[ \left( \Delta V_k - \int \nabla V \cdot (Lx)\,dt + \int (Q + R\hat{u}^2)\,dt + \int 2R\hat{u}_{\text{new}} v\,dt \right)^2 \right] $$
|
||||
|
||||
其中 Actor 输出被 detach，梯度仅流向 Critic。
|
||||
|
||||
**Actor Loss — 最优策略回归（策略改进）：**
|
||||
$$ \text{Loss}_A = \mathbb{E}\left[ \left( u_{\text{NN}}(x) - \left(-\frac{1}{2}R^{-1}g^T(x)\nabla V(x)\right) \right)^2 \right] $$
|
||||
|
||||
对倒立摆 ($J=1, R=1$)：$u^* = -0.5 \cdot \partial V / \partial x_2$
|
||||
|
||||
Critic 梯度被 detach，梯度仅流向 Actor。两个网络各有独立的 Adam 优化器。
|
||||
|
||||
### 算法三阶段
|
||||
|
||||
1. **Phase One — 寻找 L₀**：
|
||||
- 用 $u = \text{PE}$（零策略）采集一条轨迹
|
||||
- 扫描所有 $L \in [L_{\text{start}}, L_{\text{max}}]$，每个 L 训练独立 trainer
|
||||
- 选取 Bellman 残差最低的 L 作为 L₀（而非第一个正定的 L）
|
||||
- 对应的 Critic 必须通过正定性检验（Hessian at origin PD + 轨迹上 V(x)≥0）
|
||||
|
||||
2. **Phase Two — 同伦收缩**：
|
||||
- 用当前策略 + PE 采集轨迹
|
||||
- 训练 Critic（Bellman 残差）和 Actor（最优策略回归）
|
||||
- 用 safety constraint (14) 计算步长 α，$L \leftarrow L - \alpha$
|
||||
- PD 不通过则回退到 best weights；连续拒绝超 15 次则提前终止
|
||||
- 迭代至 $L = 0$
|
||||
|
||||
3. **Phase Three — 标准 PI**：
|
||||
- 设 $L = 0$，用 Phase Two 的策略作为初始策略
|
||||
- 交替 Critic 评估 + Actor 改进，直到 loss 收敛
|
||||
- 输出最终策略 $u^*(x)$ 和值函数 $V^*(x)$
|
||||
|
||||
---
|
||||
|
||||
## 总结与思考
|
||||
|
||||
### 本文贡献
|
||||
|
||||
打破了 PI 必须从 admissible 策略开始这一模型依赖的死循环：
|
||||
|
||||
$$ \text{同伦阻尼 } L_i x \;\xrightarrow{\text{使 }u=0\text{ 稳定}}\; \text{Lipschitz admissible} \;\xrightarrow{\text{逐步归零 }L_i}\; \text{admissible} \;\xrightarrow{\text{传统 PI}}\; \text{最优策略} $$
|
||||
|
||||
### 本实现的改进
|
||||
|
||||
1. **Deep NN 替代多项式基函数**：避免了多项式基函数的严重多重共线性问题（cond(A) ~ 1e9），使系统矩阵不再奇异
|
||||
2. **梯度下降替代矩阵求逆**：用 Adam 优化 Bellman 残差 + 策略回归，彻底规避病态线性系统
|
||||
3. **解耦 Actor-Critic 训练**：Actor 直接回归最优控制律 $u^* = -1/(2R)g^T\nabla V$，比联合求解更稳定
|
||||
4. **Phase One 全扫描**：扫描所有 L 取最低 loss，而非取第一个正定的 L，确保找到最佳同伦起点
|
||||
@@ -0,0 +1,6 @@
|
||||
numpy>=1.21
|
||||
scipy>=1.7
|
||||
torch>=1.10
|
||||
pytest>=7.0
|
||||
pandas>=1.3
|
||||
matplotlib>=3.3
|
||||
@@ -0,0 +1,107 @@
|
||||
"""Tests for DataCollector (Module A)."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from hpi.data_collector import DataCollector
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def dc():
|
||||
return DataCollector(J=1.0, mgl=1.0)
|
||||
|
||||
|
||||
class TestDataCollector:
|
||||
"""Test suite for DataCollector."""
|
||||
|
||||
def test_f_zero(self, dc):
|
||||
"""f(0) = 0 at equilibrium."""
|
||||
f0 = dc.f(np.zeros(2))
|
||||
np.testing.assert_allclose(f0, [0.0, 0.0], atol=1e-12)
|
||||
|
||||
def test_f_pi_half(self, dc):
|
||||
"""f([π/2, 0]) = [0, mgl/J]."""
|
||||
x = np.array([np.pi / 2, 0.0])
|
||||
f_val = dc.f(x)
|
||||
expected = np.array([0.0, dc.mgl / dc.J])
|
||||
np.testing.assert_allclose(f_val, expected, atol=1e-12)
|
||||
|
||||
def test_f_batch(self, dc):
|
||||
"""Batch f output shape (N, 2)."""
|
||||
x = np.random.randn(10, 2)
|
||||
f_vals = dc.f(x)
|
||||
assert f_vals.shape == (10, 2)
|
||||
|
||||
def test_g_constant(self, dc):
|
||||
"""g(x) = [0, 1/J] constant."""
|
||||
g_val = dc.g(np.array([0.5, 0.3]))
|
||||
expected = np.array([0.0, 1.0 / dc.J])
|
||||
np.testing.assert_allclose(g_val, expected, atol=1e-12)
|
||||
|
||||
def test_g_batch(self, dc):
|
||||
"""Batch g output shape (N, 2)."""
|
||||
x = np.random.randn(10, 2)
|
||||
g_vals = dc.g(x)
|
||||
assert g_vals.shape == (10, 2)
|
||||
# All rows should be [0, 1/J]
|
||||
np.testing.assert_allclose(g_vals[:, 1], np.ones(10) / dc.J, atol=1e-12)
|
||||
|
||||
def test_pe_signal_bounded(self, dc):
|
||||
"""PE signal amplitude should be <= 0.1."""
|
||||
for t in np.linspace(0, 10, 100):
|
||||
val = dc.pe_signal(t)
|
||||
assert abs(val) <= 0.1 + 1e-12
|
||||
|
||||
def test_pe_signal_nonzero(self, dc):
|
||||
"""PE signal should not be identically zero."""
|
||||
vals = [dc.pe_signal(t) for t in np.linspace(0, 10, 100)]
|
||||
assert np.max(np.abs(vals)) > 1e-6
|
||||
|
||||
def test_dynamics_manual(self, dc):
|
||||
"""Verify ẋ = f(x) + g(x)·u."""
|
||||
x = np.array([0.5, 0.3])
|
||||
u = 0.7
|
||||
dx = dc.dynamics(0.0, x, lambda t, x: u)
|
||||
expected = dc.f(x) + dc.g(x) * u
|
||||
np.testing.assert_allclose(dx, expected, atol=1e-12)
|
||||
|
||||
def test_collect_trajectory_shape(self, dc):
|
||||
"""Trajectory outputs have consistent dimensions."""
|
||||
x0 = np.array([0.5, 0.0])
|
||||
T = 1.0
|
||||
dt = 0.01
|
||||
|
||||
def u_const(t, x):
|
||||
return 0.0
|
||||
|
||||
t, X, U = dc.collect_trajectory(x0, T, dt, u_const)
|
||||
|
||||
M_expected = int(T / dt)
|
||||
assert t.shape == (M_expected,)
|
||||
assert X.shape == (M_expected, 2)
|
||||
assert U.shape == (M_expected,)
|
||||
|
||||
def test_collect_trajectory_start(self, dc):
|
||||
"""First state should equal x0."""
|
||||
x0 = np.array([0.5, 0.0])
|
||||
T = 1.0
|
||||
dt = 0.01
|
||||
|
||||
def u_const(t, x):
|
||||
return 0.0
|
||||
|
||||
t, X, U = dc.collect_trajectory(x0, T, dt, u_const)
|
||||
np.testing.assert_allclose(X[0], x0, atol=1e-10)
|
||||
|
||||
def test_collect_trajectory_drift(self, dc):
|
||||
"""With zero control, state should evolve (pendulum swings)."""
|
||||
x0 = np.array([0.5, 0.0])
|
||||
T = 2.0
|
||||
dt = 0.01
|
||||
|
||||
def u_const(t, x):
|
||||
return 0.0
|
||||
|
||||
t, X, U = dc.collect_trajectory(x0, T, dt, u_const)
|
||||
# Should not stay at x0 (pendulum moves)
|
||||
assert np.linalg.norm(X[-1] - x0) > 1e-6
|
||||
@@ -0,0 +1,121 @@
|
||||
"""Tests for HPIController — NN edition."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
import torch
|
||||
|
||||
from hpi.hpi_controller import HPIController
|
||||
from hpi.nn_models import CriticNN, check_positive_definite
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def controller():
|
||||
return HPIController(J=1.0, mgl=1.0, R=1.0, pe_amplitude=0.5,
|
||||
lr=1e-2, epochs=100, lambda_weight=1e-6)
|
||||
|
||||
|
||||
class TestPositiveDefinite:
|
||||
"""Test suite for check_positive_definite on CriticNN."""
|
||||
|
||||
def test_positive_definite_true(self):
|
||||
"""A well-initialized critic should pass PD (random init usually PD)."""
|
||||
critic = CriticNN()
|
||||
# Check: random init typically gives PD Hessian at origin
|
||||
# If not, the function still runs without error
|
||||
result = check_positive_definite(critic)
|
||||
assert isinstance(result, bool)
|
||||
|
||||
def test_positive_definite_false(self):
|
||||
"""Critic with all-zero final layer fails PD (zero-rank Hessian)."""
|
||||
critic = CriticNN()
|
||||
# Zero out the final layer to make Hessian rank-deficient at origin
|
||||
with torch.no_grad():
|
||||
critic.fc3.weight.zero_()
|
||||
assert check_positive_definite(critic) is False
|
||||
|
||||
def test_positive_definite_large_cross(self):
|
||||
"""Critic with rank-1 Hessian (single direction) fails PD.
|
||||
|
||||
Zero all layers, then set only fc3.weight[0,0]=1. This makes only
|
||||
z1 depend on x1, giving Hessian = 2*[1,0]^T[1,0] which is rank-1.
|
||||
"""
|
||||
critic = CriticNN()
|
||||
with torch.no_grad():
|
||||
for p in critic.parameters():
|
||||
p.zero_()
|
||||
critic.fc3.weight[0, 0] = 1.0
|
||||
assert check_positive_definite(critic) is False
|
||||
|
||||
def test_positive_definite_borderline(self):
|
||||
"""Near-singular Hessian fails PD."""
|
||||
critic = CriticNN()
|
||||
with torch.no_grad():
|
||||
for p in critic.parameters():
|
||||
p.zero_()
|
||||
critic.fc3.weight[0, 0] = 1e-8 # very small, det ~ 0
|
||||
assert check_positive_definite(critic) is False
|
||||
|
||||
|
||||
class TestPhaseOne:
|
||||
"""Test suite for Phase 1."""
|
||||
|
||||
def test_phase_one_finds_L0(self, controller):
|
||||
"""Phase 1 should find a valid L0."""
|
||||
x0 = np.array([0.5, 0.0])
|
||||
L0, critic_sd, actor_sd, info = controller.phase_one_find_L0(
|
||||
x0, T=2.0, dt=0.05, L_start=0.1, L_step=1.0, L_max=20.0,
|
||||
verbose=False,
|
||||
)
|
||||
assert L0 > 0
|
||||
assert isinstance(critic_sd, dict)
|
||||
assert isinstance(actor_sd, dict)
|
||||
assert isinstance(info, dict)
|
||||
# Load and check PD
|
||||
controller.trainer.critic_nn.load_state_dict(critic_sd)
|
||||
assert controller.check_positive_definite()
|
||||
|
||||
def test_phase_one_raises_on_low_Lmax(self, controller):
|
||||
"""Phase 1 should raise RuntimeError if L_max < L_start (no valid L tried)."""
|
||||
x0 = np.array([0.5, 0.0])
|
||||
with pytest.raises(RuntimeError):
|
||||
controller.phase_one_find_L0(
|
||||
x0, T=1.0, dt=0.05, L_start=1.0, L_step=1.0, L_max=0.5
|
||||
)
|
||||
|
||||
|
||||
class TestFullPipeline:
|
||||
"""Test suite for full pipeline."""
|
||||
|
||||
def test_full_pipeline(self, controller):
|
||||
"""Three-phase pipeline runs without error and returns valid results."""
|
||||
results = controller.run(
|
||||
x0=np.array([0.5, 0.0]),
|
||||
T=2.0,
|
||||
dt=0.05,
|
||||
L_start=0.1,
|
||||
L_step=1.0,
|
||||
L_max=10.0,
|
||||
gamma=0.1,
|
||||
epsilon=1e-3,
|
||||
verbose=False,
|
||||
)
|
||||
|
||||
assert "phase1_L0" in results
|
||||
assert results["phase1_L0"] > 0
|
||||
assert isinstance(results["phase1_critic_sd"], dict)
|
||||
assert isinstance(results["phase1_actor_sd"], dict)
|
||||
|
||||
assert "phase2_actor_sd" in results
|
||||
assert "phase2_critic_sd" in results
|
||||
assert len(results["phase2_history"]) >= 1
|
||||
|
||||
assert "phase3_actor_sd" in results
|
||||
assert "phase3_critic_sd" in results
|
||||
assert isinstance(results["phase3_actor_sd"], dict)
|
||||
assert isinstance(results["phase3_critic_sd"], dict)
|
||||
|
||||
# Verify final critic is PD
|
||||
controller.trainer.critic_nn.load_state_dict(results["phase3_critic_sd"])
|
||||
is_pd = controller.check_positive_definite()
|
||||
# May or may not be PD depending on convergence, but shouldn't error
|
||||
assert isinstance(is_pd, bool)
|
||||
@@ -0,0 +1,165 @@
|
||||
"""Tests for NN models: CriticNN, ActorNN, compute_q, check_positive_definite."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
import torch
|
||||
|
||||
from hpi.nn_models import ActorNN, CriticNN, check_positive_definite, compute_q
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def critic():
|
||||
return CriticNN()
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def actor():
|
||||
return ActorNN()
|
||||
|
||||
|
||||
class TestCriticNN:
|
||||
"""Test suite for CriticNN."""
|
||||
|
||||
def test_phi_zero(self, critic):
|
||||
"""V(0) = 0."""
|
||||
x = torch.zeros(1, 2)
|
||||
V = critic(x)
|
||||
assert abs(V.item()) < 1e-6
|
||||
|
||||
def test_phi_dim_single(self, critic):
|
||||
"""Single sample: V output shape (,)."""
|
||||
x = torch.tensor([[0.5, -0.3]], dtype=torch.float32)
|
||||
V = critic(x)
|
||||
assert V.shape == (1,)
|
||||
assert V.ndim == 1
|
||||
|
||||
def test_phi_dim_batch(self, critic):
|
||||
"""Batch input: V output shape (N,)."""
|
||||
x = torch.randn(10, 2)
|
||||
V = critic(x)
|
||||
assert V.shape == (10,)
|
||||
|
||||
def test_phi_nonnegative(self, critic):
|
||||
"""V(x) >= 0 for all x."""
|
||||
x = torch.randn(100, 2)
|
||||
V = critic(x)
|
||||
assert (V >= -1e-6).all()
|
||||
|
||||
|
||||
class TestActorNN:
|
||||
"""Test suite for ActorNN."""
|
||||
|
||||
def test_psi_zero(self, actor):
|
||||
"""u(0) is finite (not NaN, not inf)."""
|
||||
x = torch.zeros(1, 2)
|
||||
u = actor(x)
|
||||
assert torch.isfinite(u).all()
|
||||
|
||||
def test_psi_dim_single(self, actor):
|
||||
"""Single sample: u output shape (,)."""
|
||||
x = torch.tensor([[0.5, -0.3]], dtype=torch.float32)
|
||||
u = actor(x)
|
||||
assert u.shape == (1,)
|
||||
assert u.ndim == 1
|
||||
|
||||
def test_psi_dim_batch(self, actor):
|
||||
"""Batch input: u output shape (N,)."""
|
||||
x = torch.randn(10, 2)
|
||||
u = actor(x)
|
||||
assert u.shape == (10,)
|
||||
|
||||
def test_psi_exact_values(self, actor):
|
||||
"""u varies with input (not constant)."""
|
||||
x1 = torch.tensor([[0.5, 0.3]], dtype=torch.float32)
|
||||
x2 = torch.tensor([[-0.8, 1.2]], dtype=torch.float32)
|
||||
u1 = actor(x1).item()
|
||||
u2 = actor(x2).item()
|
||||
assert abs(u1 - u2) > 1e-8
|
||||
|
||||
|
||||
class TestCriticGrad:
|
||||
"""Test suite for CriticNN gradient (replaces evaluate_dphi tests)."""
|
||||
|
||||
@pytest.fixture
|
||||
def critic(self):
|
||||
return CriticNN()
|
||||
|
||||
def test_dphi_dim_single(self, critic):
|
||||
"""Single sample: grad_V shape (2,)."""
|
||||
x = torch.tensor([[0.5, -0.3]], dtype=torch.float32, requires_grad=True)
|
||||
V = critic(x)
|
||||
grad_V = torch.autograd.grad(V.sum(), x)[0]
|
||||
assert grad_V.shape == (1, 2)
|
||||
|
||||
def test_dphi_dim_batch(self, critic):
|
||||
"""Batch: grad_V shape (N, 2)."""
|
||||
x = torch.randn(10, 2, requires_grad=True)
|
||||
V = critic(x)
|
||||
grad_V = torch.autograd.grad(V.sum(), x)[0]
|
||||
assert grad_V.shape == (10, 2)
|
||||
|
||||
def test_dphi_finite_difference(self, critic):
|
||||
"""grad_V matches finite difference approximation."""
|
||||
x_np = np.array([[0.5, 0.3]], dtype=np.float32)
|
||||
x = torch.tensor(x_np, requires_grad=True)
|
||||
V = critic(x)
|
||||
grad_V = torch.autograd.grad(V.sum(), x)[0].detach().numpy()[0]
|
||||
|
||||
eps = 1e-4
|
||||
for i in range(2):
|
||||
dx = np.zeros(2)
|
||||
dx[i] = eps
|
||||
x_plus = torch.tensor(x_np + dx.reshape(1, 2), dtype=torch.float32)
|
||||
x_minus = torch.tensor(x_np - dx.reshape(1, 2), dtype=torch.float32)
|
||||
with torch.no_grad():
|
||||
V_plus = critic(x_plus).item()
|
||||
V_minus = critic(x_minus).item()
|
||||
dphi_fd = (V_plus - V_minus) / (2 * eps)
|
||||
np.testing.assert_allclose(grad_V[i], dphi_fd, rtol=1e-3, atol=1e-4)
|
||||
|
||||
def test_dphi_finite_difference_batch(self, critic):
|
||||
"""Batch finite difference check for grad_V."""
|
||||
np.random.seed(123)
|
||||
x_np = np.random.randn(5, 2).astype(np.float32)
|
||||
x = torch.tensor(x_np, requires_grad=True)
|
||||
V = critic(x)
|
||||
grad_V = torch.autograd.grad(V.sum(), x)[0].detach().numpy()
|
||||
|
||||
eps = 1e-4
|
||||
for j in range(5):
|
||||
for i in range(2):
|
||||
dx = np.zeros(2)
|
||||
dx[i] = eps
|
||||
x_plus = x_np.copy()
|
||||
x_minus = x_np.copy()
|
||||
x_plus[j] += dx
|
||||
x_minus[j] -= dx
|
||||
with torch.no_grad():
|
||||
V_plus = critic(torch.tensor(x_plus)).numpy()[j]
|
||||
V_minus = critic(torch.tensor(x_minus)).numpy()[j]
|
||||
dphi_fd = (V_plus - V_minus) / (2 * eps)
|
||||
np.testing.assert_allclose(grad_V[j, i], dphi_fd, rtol=1e-2, atol=1e-3)
|
||||
|
||||
|
||||
class TestComputeQ:
|
||||
"""Test suite for compute_q."""
|
||||
|
||||
def test_Q_value(self):
|
||||
"""Q(x) = x1^2 + x2^2."""
|
||||
x = torch.tensor([[0.6, -0.8]], dtype=torch.float32)
|
||||
q = compute_q(x)
|
||||
expected = 0.6**2 + (-0.8)**2
|
||||
assert abs(q.item() - expected) < 1e-6
|
||||
|
||||
def test_Q_batch_dim(self):
|
||||
"""Batch Q output shape (N,)."""
|
||||
x = torch.randn(10, 2)
|
||||
q = compute_q(x)
|
||||
assert q.shape == (10,)
|
||||
|
||||
def test_Q_batch_values(self):
|
||||
"""Batch Q values match manual computation."""
|
||||
x = torch.tensor([[1.0, 2.0], [3.0, 4.0], [0.0, 0.0]], dtype=torch.float32)
|
||||
q = compute_q(x)
|
||||
expected = torch.tensor([5.0, 25.0, 0.0])
|
||||
assert torch.allclose(q, expected)
|
||||
@@ -0,0 +1,128 @@
|
||||
"""Tests for NNTrainer (replaces test_solver.py)."""
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from hpi.nn_trainer import NNTrainer
|
||||
|
||||
|
||||
@pytest.fixture
|
||||
def trainer():
|
||||
return NNTrainer(R=1.0, lr=1e-3, epochs=50, lambda_weight=1e-6)
|
||||
|
||||
|
||||
class TestNNTrainer:
|
||||
"""Test suite for NNTrainer."""
|
||||
|
||||
# ── integrate_interval ──
|
||||
|
||||
def test_integrate_interval_scalar(self):
|
||||
"""Trapezoidal integration: [0,1,2] with dt=1 -> [0.5, 1.5]."""
|
||||
f = np.array([0.0, 1.0, 2.0])
|
||||
result = NNTrainer.integrate_interval(f, dt=1.0)
|
||||
expected = np.array([0.5, 1.5])
|
||||
np.testing.assert_allclose(result, expected, atol=1e-12)
|
||||
|
||||
def test_integrate_interval_vector(self):
|
||||
"""Vector trapezoidal integration."""
|
||||
f = np.array([[0.0, 1.0], [2.0, 3.0], [4.0, 5.0]])
|
||||
result = NNTrainer.integrate_interval(f, dt=1.0)
|
||||
expected = np.array([[1.0, 2.0], [3.0, 4.0]])
|
||||
np.testing.assert_allclose(result, expected, atol=1e-12)
|
||||
|
||||
# ── compute_u_hat / compute_v_hat ──
|
||||
|
||||
def test_compute_u_hat_manual(self, trainer):
|
||||
"""u_hat returns correct shape for single and batch input."""
|
||||
x_single = np.array([0.5, 0.3])
|
||||
u = trainer.compute_u_hat(x_single)
|
||||
assert np.isscalar(u)
|
||||
assert np.isfinite(u)
|
||||
|
||||
x_batch = np.random.randn(10, 2).astype(np.float32)
|
||||
u_batch = trainer.compute_u_hat(x_batch)
|
||||
assert u_batch.shape == (10,)
|
||||
assert np.all(np.isfinite(u_batch))
|
||||
|
||||
def test_compute_v_hat(self, trainer):
|
||||
"""v_hat = u - u_hat."""
|
||||
u = np.array([0.5, 0.3, 0.1])
|
||||
x = np.array([[0.5, 0.3], [0.1, -0.2], [0.0, 0.0]])
|
||||
u_hat = trainer.compute_u_hat(x)
|
||||
v_hat = trainer.compute_v_hat(u, x)
|
||||
np.testing.assert_allclose(v_hat, u - u_hat, atol=1e-10)
|
||||
|
||||
# ── Training output shapes ──
|
||||
|
||||
def test_compute_I_phi_dim(self, trainer):
|
||||
"""After training, verify that V can be computed (replaces I_phi test)."""
|
||||
M = 100
|
||||
x = np.random.randn(M, 2).astype(np.float32) * 0.5
|
||||
V = trainer.compute_V(x)
|
||||
assert V.shape == (M,)
|
||||
|
||||
def test_compute_I_psi_dim(self, trainer):
|
||||
"""u_hat output shape matches input (replaces I_psi test)."""
|
||||
M = 100
|
||||
x = np.random.randn(M, 2).astype(np.float32) * 0.5
|
||||
u_hat = trainer.compute_u_hat(x)
|
||||
assert u_hat.shape == (M,)
|
||||
|
||||
def test_compute_I_Q_dim(self, trainer):
|
||||
"""Q(x) can be computed on trajectory (replaces I_Q test)."""
|
||||
M = 100
|
||||
x = np.random.randn(M, 2).astype(np.float32) * 0.5
|
||||
Q = x[:, 0]**2 + x[:, 1]**2
|
||||
assert Q.shape == (M,)
|
||||
|
||||
# ── System construction (adapted) ──
|
||||
|
||||
def test_build_system_dims(self, trainer):
|
||||
"""Training produces a result dict with expected keys."""
|
||||
M = 200
|
||||
t = np.linspace(0, 2.0, M)
|
||||
X = np.random.randn(M, 2).astype(np.float32) * 0.5
|
||||
U = np.random.randn(M).astype(np.float32) * 0.1
|
||||
|
||||
result = trainer.train_one_iteration(t, X, U, L_i=1.0, prev_actor=None, dt=0.01)
|
||||
assert "loss_c" in result
|
||||
assert "mse_c" in result
|
||||
assert np.isfinite(result["loss_c"])
|
||||
|
||||
def test_solve_dim(self, trainer):
|
||||
"""Training returns result dict with finite loss."""
|
||||
M = 200
|
||||
t = np.linspace(0, 2.0, M)
|
||||
X = np.random.randn(M, 2).astype(np.float32) * 0.5
|
||||
U = np.random.randn(M).astype(np.float32) * 0.1
|
||||
|
||||
result = trainer.train_one_iteration(t, X, U, L_i=1.0, prev_actor=None, dt=0.01)
|
||||
assert np.isfinite(result["loss_c"])
|
||||
assert result["loss_c"] >= 0
|
||||
|
||||
def test_solve_no_nan(self, trainer):
|
||||
"""Training produces no NaN in loss."""
|
||||
M = 200
|
||||
t = np.linspace(0, 2.0, M)
|
||||
X = np.random.randn(M, 2).astype(np.float32) * 0.5
|
||||
U = np.random.randn(M).astype(np.float32) * 0.1
|
||||
|
||||
result = trainer.train_one_iteration(t, X, U, L_i=0.0, prev_actor=None, dt=0.01)
|
||||
assert not np.isnan(result["loss_c"])
|
||||
assert not np.isinf(result["loss_c"])
|
||||
|
||||
def test_solve_with_trajectory_data(self, trainer):
|
||||
"""Train on simulated pendulum data produces finite parameters."""
|
||||
from hpi.data_collector import DataCollector
|
||||
|
||||
dc = DataCollector()
|
||||
x0 = np.array([0.5, 0.0])
|
||||
|
||||
def u_func(t, x):
|
||||
return dc.pe_signal(t)
|
||||
|
||||
t, X, U = dc.collect_trajectory(x0, T=2.0, dt=0.01, u_func=u_func)
|
||||
result = trainer.train_one_iteration(t, X, U, L_i=2.0, prev_actor=None, dt=0.01)
|
||||
|
||||
assert np.isfinite(result["loss_c"])
|
||||
assert result["loss_c"] >= 0.0
|
||||
Reference in New Issue
Block a user