adipu/guadaloop_lev_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

rename to levSim, requirements, readme

Browse Source
This commit is contained in:
pulipakaa24
2026-03-20 19:05:50 -05:00
parent a7ff7ae0ca
commit a098ec5462
46 changed files with 1208 additions and 1004 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4472
lev_sim/Ansys Results 12-9.csv Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
lev_sim/Ansys Results 12-9.xlsx Executable file
View File

Binary file not shown.

2306
lev_sim/Function Fitting.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

164
lev_sim/PWM_Circuit_Model.py Normal file
View File

@@ -0,0 +1,164 @@
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import TransferFunction, lsim
# ============================================================
# Circuit Parameters
# ============================================================
R = 1.5 # Resistance [Ω]
L = 0.0025 # Inductance [H] (2.5 mH)
V_SUPPLY = 12.0 # Supply rail [V]
tau = L / R # RL time constant ≈ 1.667 ms
# ============================================================
# PWM Parameters
# ============================================================
F_PWM = 16e3 # PWM frequency [Hz]
T_PWM = 1.0 / F_PWM # PWM period [s] (62.5 µs)
# ============================================================
# Simulation Window
# ============================================================
T_END = 1e-3 # 1 ms → 16 full PWM cycles
DT = 1e-7 # 100 ns resolution (625 pts / PWM cycle)
t = np.arange(0, T_END + DT / 2, DT)
# ============================================================
# Duty-Cycle Command D(t)
# ============================================================
# Ramp from 20 % → 80 % over the window so every PWM cycle
# has a visibly different pulse width.
def duty_command(t_val):
"""Continuous duty-cycle setpoint (from a controller)."""
return np.clip(0.2 + 0.6 * (np.asarray(t_val) / T_END), 0.0, 1.0)
D_continuous = duty_command(t)
# ============================================================
# MODEL 1 — Abstracted (Average-Voltage) Approximation
# ============================================================
# Treats the coil voltage as the smooth signal D(t)·V.
# Transfer function: I(s)/D(s) = V / (Ls + R)
G = TransferFunction([V_SUPPLY], [L, R])
_, i_avg, _ = lsim(G, U=D_continuous, T=t)
# ============================================================
# MODEL 2 — True PWM Waveform (exact analytical solution)
# ============================================================
# Between every switching edge the RL circuit obeys:
#
# di/dt = (V_seg − R·i) / L (V_seg = V_SUPPLY or 0)
#
# Closed-form from initial condition i₀ at time t₀:
#
# i(t) = V_seg/R + (i₀ − V_seg/R) · exp(−R·(t − t₀) / L)
#
# We propagate i analytically from edge to edge — zero
# numerical-integration error. The only error source is
# IEEE-754 floating-point arithmetic (~1e-15 relative).
# --- Step 1: build segment table and propagate boundary currents ---
seg_t_start = [] # start time of each constant-V segment
seg_V = [] # voltage applied during segment
seg_i0 = [] # current at segment start
i_boundary = 0.0 # coil starts de-energised
cycle = 0
while cycle * T_PWM < T_END:
t_cycle = cycle * T_PWM
D = float(duty_command(t_cycle))
# ---- ON phase (V_SUPPLY applied) ----
t_on_start = t_cycle
t_on_end = min(t_cycle + D * T_PWM, T_END)
if t_on_end > t_on_start:
seg_t_start.append(t_on_start)
seg_V.append(V_SUPPLY)
seg_i0.append(i_boundary)
dt_on = t_on_end - t_on_start
i_boundary = (V_SUPPLY / R) + (i_boundary - V_SUPPLY / R) * np.exp(-R * dt_on / L)
# ---- OFF phase (0 V applied, free-wheeling through diode) ----
t_off_start = t_on_end
t_off_end = min((cycle + 1) * T_PWM, T_END)
if t_off_end > t_off_start:
seg_t_start.append(t_off_start)
seg_V.append(0.0)
seg_i0.append(i_boundary)
dt_off = t_off_end - t_off_start
i_boundary = i_boundary * np.exp(-R * dt_off / L)
cycle += 1
seg_t_start = np.array(seg_t_start)
seg_V = np.array(seg_V)
seg_i0 = np.array(seg_i0)
# --- Step 2: evaluate on the dense time array (vectorised) ---
idx = np.searchsorted(seg_t_start, t, side='right') - 1
idx = np.clip(idx, 0, len(seg_t_start) - 1)
dt_in_seg = t - seg_t_start[idx]
V_at_t = seg_V[idx]
i0_at_t = seg_i0[idx]
i_pwm = (V_at_t / R) + (i0_at_t - V_at_t / R) * np.exp(-R * dt_in_seg / L)
v_pwm = V_at_t # switching waveform for plotting
v_avg = D_continuous * V_SUPPLY # average-model voltage
# ============================================================
# Console Output — sanity-check steady-state values
# ============================================================
print(f"RL time constant τ = L/R = {tau*1e3:.3f} ms")
print(f"PWM period T = 1/f = {T_PWM*1e6:.1f} µs")
print(f"Sim resolution Δt = {DT*1e9:.0f} ns ({int(T_PWM/DT)} pts/cycle)")
print()
print("Expected steady-state currents i_ss = (V/R)·D :")
for d in [0.2, 0.4, 0.6, 0.8]:
print(f" D = {d:.1f} → i_ss = {V_SUPPLY / R * d:.3f} A")
# ============================================================
# Plotting — 4-panel comparison
# ============================================================
t_us = t * 1e6 # time axis in µs
fig, axes = plt.subplots(4, 1, figsize=(14, 10), sharex=True)
fig.suptitle("PWM RL-Circuit Model Comparison (16 kHz, 1 ms window)",
fontsize=13, fontweight='bold')
# --- 1. Duty-cycle command ---
ax = axes[0]
ax.plot(t_us, D_continuous * 100, color='tab:purple', linewidth=1.2)
ax.set_ylabel("Duty Cycle [%]")
ax.set_ylim(0, 100)
ax.grid(True, alpha=0.3)
# --- 2. Voltage waveforms ---
ax = axes[1]
ax.plot(t_us, v_pwm, color='tab:orange', linewidth=0.6, label="True PWM voltage")
ax.plot(t_us, v_avg, color='tab:blue', linewidth=1.4, label="Average voltage D·V",
linestyle='--')
ax.set_ylabel("Voltage [V]")
ax.set_ylim(-0.5, V_SUPPLY + 1)
ax.legend(loc='upper left', fontsize=9)
ax.grid(True, alpha=0.3)
# --- 3. Current comparison ---
ax = axes[2]
ax.plot(t_us, i_pwm, color='tab:red', linewidth=0.7, label="True PWM current (exact)")
ax.plot(t_us, i_avg, color='tab:blue', linewidth=1.4, label="Averaged-model current",
linestyle='--')
ax.set_ylabel("Current [A]")
ax.legend(loc='upper left', fontsize=9)
ax.grid(True, alpha=0.3)
# --- 4. Difference / ripple ---
ax = axes[3]
ax.plot(t_us, (i_pwm - i_avg) * 1e3, color='tab:green', linewidth=0.7)
ax.set_ylabel("Δi (PWM − avg) [mA]")
ax.set_xlabel("Time [µs]")
ax.grid(True, alpha=0.3)
plt.tight_layout()
plt.show()

BIN
lev_sim/RL_Trials/gap_error_plot_20251211_084252.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 115 KiB

BIN
lev_sim/RL_Trials/gap_error_plot_20251211_102102.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 221 KiB

BIN
lev_sim/RL_Trials/gap_error_plot_20251211_110643.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 282 KiB

BIN
lev_sim/RL_Trials/gap_error_plot_20251211_191801.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 159 KiB

29
lev_sim/RL_Trials/info.txt Normal file
View File

@@ -0,0 +1,29 @@
first trial was with plain gap-error minimization reward,
# --- FIX 2: Smoother Reward Function ---
# Reward function
reward = 1.0 # Survival bonus
# Distance Penalty (Squared is smoother than linear for fine control)
reward -= (gap_error * 100)**2
# Orientation Penalties
reward -= (roll_angle * 10)**2
reward -= (pitch_angle * 10)**2
next, added the following:
contact_points = p.getContactPoints(bodyA=self.podId, bodyB=self.trackId)
has_contact = len(contact_points) > 0
# Don't terminate on contact.
# Instead, penalize it, but allow the episode to continue so it can try to fix it.
# if has_contact:
# 5.0 is painful, but surviving 100 steps of pain is better than immediate death (-50)
reward -= len(contact_points)
# at this point, we still either stick or fall, no hovering training has been achieved.
# Tried increasing lambda value and starting at optimal all the time.
#Tried reducing entropy and resetting all params but allowing for full range of motion without bolts - 7 pm ish

109
lev_sim/RL_Trials/training_log_20251211_084252.txt Normal file
View File

@@ -0,0 +1,109 @@
Training Started: 20251211_084252
Number of Episodes: 1000
Print Frequency: 10
Target Gap Height: 16.491741 mm
======================================================================
Ep 10 | Reward: 339.6 | Length: 2000 | R/step: 0.170 ( 17.0% of max) | Gap Error: 11.386 mm
Ep 20 | Reward: 582.7 | Length: 2000 | R/step: 0.291 ( 29.1% of max) | Gap Error: 12.576 mm
Ep 30 | Reward: 1572.2 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.500 mm
Ep 40 | Reward: 1574.1 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.505 mm
Ep 50 | Reward: 1272.2 | Length: 2000 | R/step: 0.636 ( 63.6% of max) | Gap Error: 16.009 mm
Ep 60 | Reward: 1874.6 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.990 mm
Ep 70 | Reward: 1570.4 | Length: 2000 | R/step: 0.785 ( 78.5% of max) | Gap Error: 17.495 mm
Ep 80 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 90 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 100 | Reward: 1571.3 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.498 mm
Ep 110 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 120 | Reward: 1874.0 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 19.000 mm
Ep 130 | Reward: 1874.1 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.998 mm
Ep 140 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.997 mm
Ep 150 | Reward: 1874.1 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.998 mm
Ep 160 | Reward: 1873.8 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 19.002 mm
Ep 170 | Reward: 1874.4 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.990 mm
Ep 180 | Reward: 1874.9 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.987 mm
Ep 190 | Reward: 1875.5 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.927 mm
Ep 200 | Reward: 1876.3 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.949 mm
Ep 210 | Reward: 1273.8 | Length: 2000 | R/step: 0.637 ( 63.7% of max) | Gap Error: 16.011 mm
Ep 220 | Reward: 1571.1 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.494 mm
Ep 230 | Reward: 1874.6 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.990 mm
Ep 240 | Reward: 1877.3 | Length: 2000 | R/step: 0.939 ( 93.9% of max) | Gap Error: 18.912 mm
Ep 250 | Reward: 1584.6 | Length: 2000 | R/step: 0.792 ( 79.2% of max) | Gap Error: 17.495 mm
Ep 260 | Reward: 1876.5 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.948 mm
Ep 270 | Reward: 1876.6 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.955 mm
Ep 280 | Reward: 1576.3 | Length: 2000 | R/step: 0.788 ( 78.8% of max) | Gap Error: 17.433 mm
Ep 290 | Reward: 1878.4 | Length: 2000 | R/step: 0.939 ( 93.9% of max) | Gap Error: 18.920 mm
Ep 300 | Reward: 1877.9 | Length: 2000 | R/step: 0.939 ( 93.9% of max) | Gap Error: 18.884 mm
Ep 310 | Reward: 1878.5 | Length: 2000 | R/step: 0.939 ( 93.9% of max) | Gap Error: 18.880 mm
Ep 320 | Reward: 1878.0 | Length: 2000 | R/step: 0.939 ( 93.9% of max) | Gap Error: 18.899 mm
Ep 330 | Reward: 1575.8 | Length: 2000 | R/step: 0.788 ( 78.8% of max) | Gap Error: 17.475 mm
Ep 340 | Reward: 1874.9 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.986 mm
Ep 350 | Reward: 1873.8 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 19.001 mm
Ep 360 | Reward: 1874.0 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.999 mm
Ep 370 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 380 | Reward: 1874.1 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 390 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 400 | Reward: 1873.9 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.998 mm
Ep 410 | Reward: 1873.9 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 19.002 mm
Ep 420 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 430 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.998 mm
Ep 440 | Reward: 1874.0 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.999 mm
Ep 450 | Reward: 1874.0 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 19.000 mm
Ep 460 | Reward: 1874.1 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.997 mm
Ep 470 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 480 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 490 | Reward: 1874.5 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.992 mm
Ep 500 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 510 | Reward: 1874.7 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.991 mm
Ep 520 | Reward: 1874.5 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.989 mm
Ep 530 | Reward: 1874.7 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.990 mm
Ep 540 | Reward: 1874.9 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.987 mm
Ep 550 | Reward: 1875.1 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.985 mm
Ep 560 | Reward: 1572.3 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.487 mm
Ep 570 | Reward: 1874.4 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 580 | Reward: 1274.7 | Length: 2000 | R/step: 0.637 ( 63.7% of max) | Gap Error: 16.011 mm
Ep 590 | Reward: 1875.5 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.979 mm
Ep 600 | Reward: 1876.6 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.964 mm
Ep 610 | Reward: 1875.6 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.977 mm
Ep 620 | Reward: 1875.2 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.981 mm
Ep 630 | Reward: 1875.4 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.976 mm
Ep 640 | Reward: 1874.8 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.988 mm
Ep 650 | Reward: 1574.9 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.495 mm
Ep 660 | Reward: 1875.1 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.983 mm
Ep 670 | Reward: 1875.4 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.981 mm
Ep 680 | Reward: 1875.2 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.983 mm
Ep 690 | Reward: 1574.9 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.504 mm
Ep 700 | Reward: 1874.6 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.990 mm
Ep 710 | Reward: 1874.4 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 720 | Reward: 1572.4 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.500 mm
Ep 730 | Reward: 1874.2 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 740 | Reward: 1874.7 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.987 mm
Ep 750 | Reward: 1874.7 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.988 mm
Ep 760 | Reward: 1874.8 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.989 mm
Ep 770 | Reward: 1874.6 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.993 mm
Ep 780 | Reward: 1874.8 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.988 mm
Ep 790 | Reward: 1574.2 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.502 mm
Ep 800 | Reward: 1271.4 | Length: 2000 | R/step: 0.636 ( 63.6% of max) | Gap Error: 16.010 mm
Ep 810 | Reward: 1874.5 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.993 mm
Ep 820 | Reward: 1576.0 | Length: 2000 | R/step: 0.788 ( 78.8% of max) | Gap Error: 17.509 mm
Ep 830 | Reward: 1573.7 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.502 mm
Ep 840 | Reward: 1874.4 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.995 mm
Ep 850 | Reward: 1875.2 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.981 mm
Ep 860 | Reward: 971.6 | Length: 2000 | R/step: 0.486 ( 48.6% of max) | Gap Error: 14.520 mm
Ep 870 | Reward: 1574.9 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.502 mm
Ep 880 | Reward: 1874.4 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.992 mm
Ep 890 | Reward: 1874.1 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.997 mm
Ep 900 | Reward: 1873.9 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.999 mm
Ep 910 | Reward: 1572.2 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.495 mm
Ep 920 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 930 | Reward: 1874.3 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.996 mm
Ep 940 | Reward: 1874.6 | Length: 2000 | R/step: 0.937 ( 93.7% of max) | Gap Error: 18.990 mm
Ep 950 | Reward: 1573.6 | Length: 2000 | R/step: 0.787 ( 78.7% of max) | Gap Error: 17.497 mm
Ep 960 | Reward: 1571.9 | Length: 2000 | R/step: 0.786 ( 78.6% of max) | Gap Error: 17.484 mm
Ep 970 | Reward: 1271.5 | Length: 2000 | R/step: 0.636 ( 63.6% of max) | Gap Error: 16.006 mm
Ep 980 | Reward: 1875.0 | Length: 2000 | R/step: 0.938 ( 93.8% of max) | Gap Error: 18.985 mm
Ep 990 | Reward: 1570.5 | Length: 2000 | R/step: 0.785 ( 78.5% of max) | Gap Error: 17.488 mm
Ep 1000 | Reward: 1575.9 | Length: 2000 | R/step: 0.788 ( 78.8% of max) | Gap Error: 17.503 mm
======================================================================
Training Completed: 20251211_095328

109
lev_sim/RL_Trials/training_log_20251211_102102.txt Normal file
View File

@@ -0,0 +1,109 @@
Training Started: 20251211_102102
Number of Episodes: 1000
Print Frequency: 10
Target Gap Height: 16.491741 mm
======================================================================
Ep 10 | Reward: -7172.7 | Length: 500 | R/step: -14.345 (-1434.5% of max) | Gap Error: 16.068 mm
Ep 20 | Reward: -7031.0 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.962 mm
Ep 30 | Reward: -7101.5 | Length: 500 | R/step: -14.203 (-1420.3% of max) | Gap Error: 17.537 mm
Ep 40 | Reward: -7031.1 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.972 mm
Ep 50 | Reward: -7030.8 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.949 mm
Ep 60 | Reward: -7031.1 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.975 mm
Ep 70 | Reward: -7031.1 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.974 mm
Ep 80 | Reward: -7031.2 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.979 mm
Ep 90 | Reward: -7031.1 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.962 mm
Ep 100 | Reward: -7031.1 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.978 mm
Ep 110 | Reward: -7248.5 | Length: 500 | R/step: -14.497 (-1449.7% of max) | Gap Error: 14.626 mm
Ep 120 | Reward: -7164.7 | Length: 500 | R/step: -14.329 (-1432.9% of max) | Gap Error: 16.227 mm
Ep 130 | Reward: -7239.3 | Length: 500 | R/step: -14.479 (-1447.9% of max) | Gap Error: 14.752 mm
Ep 140 | Reward: -7611.2 | Length: 500 | R/step: -15.222 (-1522.2% of max) | Gap Error: 7.389 mm
Ep 150 | Reward: -7316.7 | Length: 500 | R/step: -14.633 (-1463.3% of max) | Gap Error: 13.257 mm
Ep 160 | Reward: -7657.2 | Length: 500 | R/step: -15.314 (-1531.4% of max) | Gap Error: 6.463 mm
Ep 170 | Reward: -7564.9 | Length: 500 | R/step: -15.130 (-1513.0% of max) | Gap Error: 8.263 mm
Ep 180 | Reward: -7320.2 | Length: 500 | R/step: -14.640 (-1464.0% of max) | Gap Error: 13.172 mm
Ep 190 | Reward: -7601.5 | Length: 500 | R/step: -15.203 (-1520.3% of max) | Gap Error: 7.545 mm
Ep 200 | Reward: -7311.8 | Length: 500 | R/step: -14.624 (-1462.4% of max) | Gap Error: 13.311 mm
Ep 210 | Reward: -7100.9 | Length: 500 | R/step: -14.202 (-1420.2% of max) | Gap Error: 17.572 mm
Ep 220 | Reward: -7170.8 | Length: 500 | R/step: -14.342 (-1434.2% of max) | Gap Error: 16.143 mm
Ep 230 | Reward: -7241.1 | Length: 500 | R/step: -14.482 (-1448.2% of max) | Gap Error: 14.729 mm
Ep 240 | Reward: -7310.3 | Length: 500 | R/step: -14.621 (-1462.1% of max) | Gap Error: 13.338 mm
Ep 250 | Reward: -7244.1 | Length: 500 | R/step: -14.488 (-1448.8% of max) | Gap Error: 14.699 mm
Ep 260 | Reward: -7172.7 | Length: 500 | R/step: -14.345 (-1434.5% of max) | Gap Error: 16.127 mm
Ep 270 | Reward: -7031.2 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.962 mm
Ep 280 | Reward: -7031.2 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.977 mm
Ep 290 | Reward: -7031.3 | Length: 500 | R/step: -14.063 (-1406.3% of max) | Gap Error: 18.967 mm
Ep 300 | Reward: -7031.4 | Length: 500 | R/step: -14.063 (-1406.3% of max) | Gap Error: 18.965 mm
Ep 310 | Reward: -7031.3 | Length: 500 | R/step: -14.063 (-1406.3% of max) | Gap Error: 18.927 mm
Ep 320 | Reward: -7031.3 | Length: 500 | R/step: -14.063 (-1406.3% of max) | Gap Error: 18.980 mm
Ep 330 | Reward: -7031.4 | Length: 500 | R/step: -14.063 (-1406.3% of max) | Gap Error: 18.957 mm
Ep 340 | Reward: -7101.4 | Length: 500 | R/step: -14.203 (-1420.3% of max) | Gap Error: 17.557 mm
Ep 350 | Reward: -7102.1 | Length: 500 | R/step: -14.204 (-1420.4% of max) | Gap Error: 17.547 mm
Ep 360 | Reward: -7312.6 | Length: 500 | R/step: -14.625 (-1462.5% of max) | Gap Error: 13.311 mm
Ep 370 | Reward: -7315.5 | Length: 500 | R/step: -14.631 (-1463.1% of max) | Gap Error: 13.255 mm
Ep 380 | Reward: -7380.4 | Length: 500 | R/step: -14.761 (-1476.1% of max) | Gap Error: 11.917 mm
Ep 390 | Reward: -7171.6 | Length: 500 | R/step: -14.343 (-1434.3% of max) | Gap Error: 16.108 mm
Ep 400 | Reward: -7244.8 | Length: 500 | R/step: -14.490 (-1449.0% of max) | Gap Error: 14.659 mm
Ep 410 | Reward: -7030.6 | Length: 500 | R/step: -14.061 (-1406.1% of max) | Gap Error: 18.943 mm
Ep 420 | Reward: -7169.8 | Length: 500 | R/step: -14.340 (-1434.0% of max) | Gap Error: 16.169 mm
Ep 430 | Reward: -7381.3 | Length: 500 | R/step: -14.763 (-1476.3% of max) | Gap Error: 11.908 mm
Ep 440 | Reward: -7306.4 | Length: 500 | R/step: -14.613 (-1461.3% of max) | Gap Error: 13.356 mm
Ep 450 | Reward: -7099.3 | Length: 500 | R/step: -14.199 (-1419.9% of max) | Gap Error: 17.564 mm
Ep 460 | Reward: -7031.0 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.969 mm
Ep 470 | Reward: -7030.5 | Length: 500 | R/step: -14.061 (-1406.1% of max) | Gap Error: 18.934 mm
Ep 480 | Reward: -7100.7 | Length: 500 | R/step: -14.201 (-1420.1% of max) | Gap Error: 17.549 mm
Ep 490 | Reward: -7169.3 | Length: 500 | R/step: -14.339 (-1433.9% of max) | Gap Error: 16.184 mm
Ep 500 | Reward: -7101.3 | Length: 500 | R/step: -14.203 (-1420.3% of max) | Gap Error: 17.559 mm
Ep 510 | Reward: -7168.6 | Length: 500 | R/step: -14.337 (-1433.7% of max) | Gap Error: 16.185 mm
Ep 520 | Reward: -7385.4 | Length: 500 | R/step: -14.771 (-1477.1% of max) | Gap Error: 11.860 mm
Ep 530 | Reward: -7386.8 | Length: 500 | R/step: -14.774 (-1477.4% of max) | Gap Error: 11.816 mm
Ep 540 | Reward: -7173.0 | Length: 500 | R/step: -14.346 (-1434.6% of max) | Gap Error: 16.121 mm
Ep 550 | Reward: -7244.3 | Length: 500 | R/step: -14.489 (-1448.9% of max) | Gap Error: 14.693 mm
Ep 560 | Reward: -7242.9 | Length: 500 | R/step: -14.486 (-1448.6% of max) | Gap Error: 14.731 mm
Ep 570 | Reward: -7315.1 | Length: 500 | R/step: -14.630 (-1463.0% of max) | Gap Error: 13.236 mm
Ep 580 | Reward: -7243.6 | Length: 500 | R/step: -14.487 (-1448.7% of max) | Gap Error: 14.684 mm
Ep 590 | Reward: -7031.1 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.979 mm
Ep 600 | Reward: -7168.0 | Length: 500 | R/step: -14.336 (-1433.6% of max) | Gap Error: 16.203 mm
Ep 610 | Reward: -7169.4 | Length: 500 | R/step: -14.339 (-1433.9% of max) | Gap Error: 16.183 mm
Ep 620 | Reward: -7102.4 | Length: 500 | R/step: -14.205 (-1420.5% of max) | Gap Error: 17.536 mm
Ep 630 | Reward: -7166.3 | Length: 500 | R/step: -14.333 (-1433.3% of max) | Gap Error: 16.216 mm
Ep 640 | Reward: -7030.7 | Length: 500 | R/step: -14.061 (-1406.1% of max) | Gap Error: 18.949 mm
Ep 650 | Reward: -7031.0 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.966 mm
Ep 660 | Reward: -7098.9 | Length: 500 | R/step: -14.198 (-1419.8% of max) | Gap Error: 17.581 mm
Ep 670 | Reward: -7236.5 | Length: 500 | R/step: -14.473 (-1447.3% of max) | Gap Error: 14.809 mm
Ep 680 | Reward: -7174.4 | Length: 500 | R/step: -14.349 (-1434.9% of max) | Gap Error: 16.106 mm
Ep 690 | Reward: -7314.2 | Length: 500 | R/step: -14.628 (-1462.8% of max) | Gap Error: 13.294 mm
Ep 700 | Reward: -7102.0 | Length: 500 | R/step: -14.204 (-1420.4% of max) | Gap Error: 17.539 mm
Ep 710 | Reward: -7314.9 | Length: 500 | R/step: -14.630 (-1463.0% of max) | Gap Error: 13.277 mm
Ep 720 | Reward: -7310.9 | Length: 500 | R/step: -14.622 (-1462.2% of max) | Gap Error: 13.335 mm
Ep 730 | Reward: -7315.2 | Length: 500 | R/step: -14.630 (-1463.0% of max) | Gap Error: 13.275 mm
Ep 740 | Reward: -7386.0 | Length: 500 | R/step: -14.772 (-1477.2% of max) | Gap Error: 11.842 mm
Ep 750 | Reward: -7300.9 | Length: 500 | R/step: -14.602 (-1460.2% of max) | Gap Error: 13.480 mm
Ep 760 | Reward: -7311.7 | Length: 500 | R/step: -14.623 (-1462.3% of max) | Gap Error: 13.314 mm
Ep 770 | Reward: -7382.8 | Length: 500 | R/step: -14.766 (-1476.6% of max) | Gap Error: 11.885 mm
Ep 780 | Reward: -7529.6 | Length: 500 | R/step: -15.059 (-1505.9% of max) | Gap Error: 8.946 mm
Ep 790 | Reward: -7523.1 | Length: 500 | R/step: -15.046 (-1504.6% of max) | Gap Error: 9.063 mm
Ep 800 | Reward: -7391.4 | Length: 500 | R/step: -14.783 (-1478.3% of max) | Gap Error: 11.758 mm
Ep 810 | Reward: -7531.9 | Length: 500 | R/step: -15.064 (-1506.4% of max) | Gap Error: 8.915 mm
Ep 820 | Reward: -7462.2 | Length: 500 | R/step: -14.924 (-1492.4% of max) | Gap Error: 10.318 mm
Ep 830 | Reward: -7214.0 | Length: 500 | R/step: -14.428 (-1442.8% of max) | Gap Error: 15.251 mm
Ep 840 | Reward: -7426.1 | Length: 500 | R/step: -14.852 (-1485.2% of max) | Gap Error: 10.944 mm
Ep 850 | Reward: -7460.9 | Length: 500 | R/step: -14.922 (-1492.2% of max) | Gap Error: 10.270 mm
Ep 860 | Reward: -7663.9 | Length: 500 | R/step: -15.328 (-1532.8% of max) | Gap Error: 6.237 mm
Ep 870 | Reward: -7437.2 | Length: 500 | R/step: -14.874 (-1487.4% of max) | Gap Error: 10.765 mm
Ep 880 | Reward: -6530.8 | Length: 421 | R/step: -15.524 (-1552.4% of max) | Gap Error: 7.413 mm
Ep 890 | Reward: -7696.3 | Length: 500 | R/step: -15.393 (-1539.3% of max) | Gap Error: 5.736 mm
Ep 900 | Reward: -7732.4 | Length: 500 | R/step: -15.465 (-1546.5% of max) | Gap Error: 5.083 mm
Ep 910 | Reward: -7746.3 | Length: 500 | R/step: -15.493 (-1549.3% of max) | Gap Error: 4.839 mm
Ep 920 | Reward: -7745.4 | Length: 500 | R/step: -15.491 (-1549.1% of max) | Gap Error: 4.833 mm
Ep 930 | Reward: -7749.5 | Length: 500 | R/step: -15.499 (-1549.9% of max) | Gap Error: 4.838 mm
Ep 940 | Reward: -5944.1 | Length: 383 | R/step: -15.536 (-1553.6% of max) | Gap Error: 9.080 mm
Ep 950 | Reward: -4103.1 | Length: 261 | R/step: -15.739 (-1573.9% of max) | Gap Error: 12.558 mm
Ep 960 | Reward: -4697.1 | Length: 303 | R/step: -15.517 (-1551.7% of max) | Gap Error: 12.987 mm
Ep 970 | Reward: -4455.6 | Length: 288 | R/step: -15.455 (-1545.5% of max) | Gap Error: 14.009 mm
Ep 980 | Reward: -3854.4 | Length: 251 | R/step: -15.356 (-1535.6% of max) | Gap Error: 14.805 mm
Ep 990 | Reward: -5979.5 | Length: 415 | R/step: -14.405 (-1440.5% of max) | Gap Error: 17.115 mm
Ep 1000 | Reward: -7031.2 | Length: 500 | R/step: -14.062 (-1406.2% of max) | Gap Error: 18.909 mm
======================================================================
Training Completed: 20251211_103854

109
lev_sim/RL_Trials/training_log_20251211_110643.txt Normal file
View File

@@ -0,0 +1,109 @@
Training Started: 20251211_110643
Number of Episodes: 1000
Print Frequency: 10
Target Gap Height: 16.491741 mm
======================================================================
Ep 10 | Reward: -7123.0 | Length: 457 | R/step: -15.580 (-1558.0% of max) | Gap Error: 14.704 mm
Ep 20 | Reward: -6697.0 | Length: 418 | R/step: -16.025 (-1602.5% of max) | Gap Error: 11.138 mm
Ep 30 | Reward: -6050.2 | Length: 377 | R/step: -16.031 (-1603.1% of max) | Gap Error: 12.985 mm
Ep 40 | Reward: -7891.0 | Length: 500 | R/step: -15.782 (-1578.2% of max) | Gap Error: 11.726 mm
Ep 50 | Reward: -8254.2 | Length: 500 | R/step: -16.508 (-1650.8% of max) | Gap Error: 4.602 mm
Ep 60 | Reward: -8255.5 | Length: 500 | R/step: -16.511 (-1651.1% of max) | Gap Error: 4.537 mm
Ep 70 | Reward: -8185.8 | Length: 500 | R/step: -16.372 (-1637.2% of max) | Gap Error: 5.910 mm
Ep 80 | Reward: -7920.1 | Length: 500 | R/step: -15.840 (-1584.0% of max) | Gap Error: 11.089 mm
Ep 90 | Reward: -8262.8 | Length: 500 | R/step: -16.526 (-1652.6% of max) | Gap Error: 4.395 mm
Ep 100 | Reward: -8164.6 | Length: 500 | R/step: -16.329 (-1632.9% of max) | Gap Error: 6.340 mm
Ep 110 | Reward: -5294.0 | Length: 332 | R/step: -15.960 (-1596.0% of max) | Gap Error: 14.124 mm
Ep 120 | Reward: -6081.3 | Length: 375 | R/step: -16.230 (-1623.0% of max) | Gap Error: 11.482 mm
Ep 130 | Reward: -7346.6 | Length: 458 | R/step: -16.058 (-1605.8% of max) | Gap Error: 10.391 mm
Ep 140 | Reward: -7065.4 | Length: 458 | R/step: -15.427 (-1542.7% of max) | Gap Error: 15.974 mm
Ep 150 | Reward: -6516.7 | Length: 415 | R/step: -15.695 (-1569.5% of max) | Gap Error: 13.902 mm
Ep 160 | Reward: -6601.1 | Length: 417 | R/step: -15.822 (-1582.2% of max) | Gap Error: 13.542 mm
Ep 170 | Reward: -6440.3 | Length: 416 | R/step: -15.463 (-1546.3% of max) | Gap Error: 16.043 mm
Ep 180 | Reward: -5308.5 | Length: 336 | R/step: -15.799 (-1579.9% of max) | Gap Error: 15.176 mm
Ep 190 | Reward: -5847.7 | Length: 378 | R/step: -15.458 (-1545.8% of max) | Gap Error: 16.860 mm
Ep 200 | Reward: -5325.4 | Length: 333 | R/step: -16.007 (-1600.7% of max) | Gap Error: 14.364 mm
Ep 210 | Reward: -5893.1 | Length: 376 | R/step: -15.652 (-1565.2% of max) | Gap Error: 15.352 mm
Ep 220 | Reward: -7743.7 | Length: 500 | R/step: -15.487 (-1548.7% of max) | Gap Error: 14.608 mm
Ep 230 | Reward: -7180.3 | Length: 460 | R/step: -15.596 (-1559.6% of max) | Gap Error: 14.406 mm
Ep 240 | Reward: -7221.9 | Length: 459 | R/step: -15.741 (-1574.1% of max) | Gap Error: 13.200 mm
Ep 250 | Reward: -5970.0 | Length: 377 | R/step: -15.852 (-1585.2% of max) | Gap Error: 14.499 mm
Ep 260 | Reward: -7481.9 | Length: 462 | R/step: -16.212 (-1621.2% of max) | Gap Error: 8.666 mm
Ep 270 | Reward: -7189.7 | Length: 457 | R/step: -15.732 (-1573.2% of max) | Gap Error: 13.117 mm
Ep 280 | Reward: -6560.4 | Length: 418 | R/step: -15.710 (-1571.0% of max) | Gap Error: 14.139 mm
Ep 290 | Reward: -7423.1 | Length: 459 | R/step: -16.186 (-1618.6% of max) | Gap Error: 9.175 mm
Ep 300 | Reward: -7137.2 | Length: 458 | R/step: -15.594 (-1559.4% of max) | Gap Error: 14.544 mm
Ep 310 | Reward: -6457.1 | Length: 416 | R/step: -15.522 (-1552.2% of max) | Gap Error: 15.529 mm
Ep 320 | Reward: -7154.6 | Length: 459 | R/step: -15.598 (-1559.8% of max) | Gap Error: 14.440 mm
Ep 330 | Reward: -7814.2 | Length: 500 | R/step: -15.628 (-1562.8% of max) | Gap Error: 13.236 mm
Ep 340 | Reward: -7199.5 | Length: 457 | R/step: -15.744 (-1574.4% of max) | Gap Error: 13.286 mm
Ep 350 | Reward: -7234.7 | Length: 460 | R/step: -15.738 (-1573.8% of max) | Gap Error: 13.107 mm
Ep 360 | Reward: -7819.8 | Length: 500 | R/step: -15.640 (-1564.0% of max) | Gap Error: 13.160 mm
Ep 370 | Reward: -7148.2 | Length: 459 | R/step: -15.580 (-1558.0% of max) | Gap Error: 14.240 mm
Ep 380 | Reward: -7888.0 | Length: 500 | R/step: -15.776 (-1577.6% of max) | Gap Error: 11.788 mm
Ep 390 | Reward: -6321.7 | Length: 416 | R/step: -15.186 (-1518.6% of max) | Gap Error: 18.744 mm
Ep 400 | Reward: -7809.0 | Length: 500 | R/step: -15.618 (-1561.8% of max) | Gap Error: 13.359 mm
Ep 410 | Reward: -7004.8 | Length: 459 | R/step: -15.274 (-1527.4% of max) | Gap Error: 17.361 mm
Ep 420 | Reward: -7003.1 | Length: 455 | R/step: -15.391 (-1539.1% of max) | Gap Error: 15.613 mm
Ep 430 | Reward: -7280.0 | Length: 458 | R/step: -15.906 (-1590.6% of max) | Gap Error: 11.408 mm
Ep 440 | Reward: -7168.7 | Length: 460 | R/step: -15.594 (-1559.4% of max) | Gap Error: 13.981 mm
Ep 450 | Reward: -7605.7 | Length: 500 | R/step: -15.211 (-1521.1% of max) | Gap Error: 17.346 mm
Ep 460 | Reward: -6611.7 | Length: 417 | R/step: -15.871 (-1587.1% of max) | Gap Error: 12.581 mm
Ep 470 | Reward: -7086.7 | Length: 459 | R/step: -15.429 (-1542.9% of max) | Gap Error: 15.629 mm
Ep 480 | Reward: -7674.5 | Length: 500 | R/step: -15.349 (-1534.9% of max) | Gap Error: 16.008 mm
Ep 490 | Reward: -6481.8 | Length: 417 | R/step: -15.529 (-1552.9% of max) | Gap Error: 15.462 mm
Ep 500 | Reward: -7959.4 | Length: 500 | R/step: -15.919 (-1591.9% of max) | Gap Error: 10.456 mm
Ep 510 | Reward: -7088.4 | Length: 459 | R/step: -15.436 (-1543.6% of max) | Gap Error: 15.514 mm
Ep 520 | Reward: -7600.0 | Length: 500 | R/step: -15.200 (-1520.0% of max) | Gap Error: 17.460 mm
Ep 530 | Reward: -5979.7 | Length: 378 | R/step: -15.819 (-1581.9% of max) | Gap Error: 14.469 mm
Ep 540 | Reward: -6667.2 | Length: 416 | R/step: -16.038 (-1603.8% of max) | Gap Error: 11.886 mm
Ep 550 | Reward: -7165.7 | Length: 460 | R/step: -15.595 (-1559.5% of max) | Gap Error: 14.504 mm
Ep 560 | Reward: -7209.9 | Length: 458 | R/step: -15.739 (-1573.9% of max) | Gap Error: 12.949 mm
Ep 570 | Reward: -7745.6 | Length: 500 | R/step: -15.491 (-1549.1% of max) | Gap Error: 14.593 mm
Ep 580 | Reward: -7678.6 | Length: 500 | R/step: -15.357 (-1535.7% of max) | Gap Error: 15.895 mm
Ep 590 | Reward: -7107.0 | Length: 456 | R/step: -15.579 (-1557.9% of max) | Gap Error: 14.736 mm
Ep 600 | Reward: -7189.8 | Length: 458 | R/step: -15.712 (-1571.2% of max) | Gap Error: 13.386 mm
Ep 610 | Reward: -6914.9 | Length: 458 | R/step: -15.115 (-1511.5% of max) | Gap Error: 18.915 mm
Ep 620 | Reward: -7748.9 | Length: 500 | R/step: -15.498 (-1549.8% of max) | Gap Error: 14.607 mm
Ep 630 | Reward: -7894.4 | Length: 500 | R/step: -15.789 (-1578.9% of max) | Gap Error: 11.719 mm
Ep 640 | Reward: -6970.0 | Length: 457 | R/step: -15.255 (-1525.5% of max) | Gap Error: 17.182 mm
Ep 650 | Reward: -7746.9 | Length: 500 | R/step: -15.494 (-1549.4% of max) | Gap Error: 14.609 mm
Ep 660 | Reward: -7604.6 | Length: 500 | R/step: -15.209 (-1520.9% of max) | Gap Error: 17.373 mm
Ep 670 | Reward: -7747.7 | Length: 500 | R/step: -15.495 (-1549.5% of max) | Gap Error: 14.560 mm
Ep 680 | Reward: -6720.6 | Length: 419 | R/step: -16.043 (-1604.3% of max) | Gap Error: 11.076 mm
Ep 690 | Reward: -6998.9 | Length: 458 | R/step: -15.275 (-1527.5% of max) | Gap Error: 17.079 mm
Ep 700 | Reward: -7746.6 | Length: 500 | R/step: -15.493 (-1549.3% of max) | Gap Error: 14.625 mm
Ep 710 | Reward: -7894.9 | Length: 500 | R/step: -15.790 (-1579.0% of max) | Gap Error: 11.707 mm
Ep 720 | Reward: -7966.2 | Length: 500 | R/step: -15.932 (-1593.2% of max) | Gap Error: 10.268 mm
Ep 730 | Reward: -7966.1 | Length: 500 | R/step: -15.932 (-1593.2% of max) | Gap Error: 10.285 mm
Ep 740 | Reward: -7604.9 | Length: 500 | R/step: -15.210 (-1521.0% of max) | Gap Error: 17.376 mm
Ep 750 | Reward: -7673.6 | Length: 500 | R/step: -15.347 (-1534.7% of max) | Gap Error: 16.016 mm
Ep 760 | Reward: -7964.8 | Length: 500 | R/step: -15.930 (-1593.0% of max) | Gap Error: 10.311 mm
Ep 770 | Reward: -7747.0 | Length: 500 | R/step: -15.494 (-1549.4% of max) | Gap Error: 14.583 mm
Ep 780 | Reward: -7963.6 | Length: 500 | R/step: -15.927 (-1592.7% of max) | Gap Error: 10.346 mm
Ep 790 | Reward: -7209.1 | Length: 458 | R/step: -15.747 (-1574.7% of max) | Gap Error: 13.169 mm
Ep 800 | Reward: -7879.7 | Length: 500 | R/step: -15.759 (-1575.9% of max) | Gap Error: 11.951 mm
Ep 810 | Reward: -7960.0 | Length: 500 | R/step: -15.920 (-1592.0% of max) | Gap Error: 10.372 mm
Ep 820 | Reward: -7818.1 | Length: 500 | R/step: -15.636 (-1563.6% of max) | Gap Error: 13.177 mm
Ep 830 | Reward: -7965.3 | Length: 500 | R/step: -15.931 (-1593.1% of max) | Gap Error: 10.322 mm
Ep 840 | Reward: -7891.4 | Length: 500 | R/step: -15.783 (-1578.3% of max) | Gap Error: 11.716 mm
Ep 850 | Reward: -7883.5 | Length: 500 | R/step: -15.767 (-1576.7% of max) | Gap Error: 11.861 mm
Ep 860 | Reward: -7531.1 | Length: 500 | R/step: -15.062 (-1506.2% of max) | Gap Error: 18.853 mm
Ep 870 | Reward: -7601.2 | Length: 500 | R/step: -15.202 (-1520.2% of max) | Gap Error: 17.472 mm
Ep 880 | Reward: -7747.0 | Length: 500 | R/step: -15.494 (-1549.4% of max) | Gap Error: 14.594 mm
Ep 890 | Reward: -7600.6 | Length: 500 | R/step: -15.201 (-1520.1% of max) | Gap Error: 17.470 mm
Ep 900 | Reward: -7676.1 | Length: 500 | R/step: -15.352 (-1535.2% of max) | Gap Error: 15.984 mm
Ep 910 | Reward: -7750.4 | Length: 500 | R/step: -15.501 (-1550.1% of max) | Gap Error: 14.519 mm
Ep 920 | Reward: -7820.0 | Length: 500 | R/step: -15.640 (-1564.0% of max) | Gap Error: 13.170 mm
Ep 930 | Reward: -7884.7 | Length: 500 | R/step: -15.769 (-1576.9% of max) | Gap Error: 11.879 mm
Ep 940 | Reward: -7604.0 | Length: 500 | R/step: -15.208 (-1520.8% of max) | Gap Error: 17.410 mm
Ep 950 | Reward: -7677.5 | Length: 500 | R/step: -15.355 (-1535.5% of max) | Gap Error: 15.963 mm
Ep 960 | Reward: -7883.3 | Length: 500 | R/step: -15.767 (-1576.7% of max) | Gap Error: 11.895 mm
Ep 970 | Reward: -7818.1 | Length: 500 | R/step: -15.636 (-1563.6% of max) | Gap Error: 13.216 mm
Ep 980 | Reward: -7743.4 | Length: 500 | R/step: -15.487 (-1548.7% of max) | Gap Error: 14.655 mm
Ep 990 | Reward: -7960.6 | Length: 500 | R/step: -15.921 (-1592.1% of max) | Gap Error: 10.375 mm
Ep 1000 | Reward: -7601.5 | Length: 500 | R/step: -15.203 (-1520.3% of max) | Gap Error: 17.483 mm
======================================================================
Training Completed: 20251211_112339

23
lev_sim/RL_Trials/training_log_20251211_121404.txt Normal file
View File

@@ -0,0 +1,23 @@
Training Started: 20251211_121404
Number of Episodes: 2000
Print Frequency: 20
Target Gap Height: 16.491741 mm
Network: 256 hidden units with LayerNorm
Policy LR: 5e-4, Value LR: 1e-3, Entropy: 0.02
======================================================================
Ep 20 | R: -16131.1 | Len: 500 | R/s: -32.26 (-4032.8%) | Gap: 4.41mm (min: 4.26) | Best: 4.26mm
Ep 40 | R: -16140.1 | Len: 500 | R/s: -32.28 (-4035.0%) | Gap: 4.36mm (min: 4.25) | Best: 4.25mm
Ep 60 | R: -16140.8 | Len: 500 | R/s: -32.28 (-4035.2%) | Gap: 4.35mm (min: 4.22) | Best: 4.22mm
Ep 80 | R: -16143.7 | Len: 500 | R/s: -32.29 (-4035.9%) | Gap: 4.33mm (min: 4.22) | Best: 4.22mm
Ep 100 | R: -16142.5 | Len: 500 | R/s: -32.29 (-4035.6%) | Gap: 4.35mm (min: 4.26) | Best: 4.22mm
Ep 120 | R: -16142.2 | Len: 500 | R/s: -32.28 (-4035.5%) | Gap: 4.35mm (min: 4.26) | Best: 4.22mm
Ep 140 | R: -16142.9 | Len: 500 | R/s: -32.29 (-4035.7%) | Gap: 4.33mm (min: 4.23) | Best: 4.22mm
Ep 160 | R: -16144.1 | Len: 500 | R/s: -32.29 (-4036.0%) | Gap: 4.32mm (min: 4.21) | Best: 4.21mm
Ep 180 | R: -16141.0 | Len: 500 | R/s: -32.28 (-4035.3%) | Gap: 4.36mm (min: 4.22) | Best: 4.21mm
Ep 200 | R: -16143.8 | Len: 500 | R/s: -32.29 (-4035.9%) | Gap: 4.33mm (min: 4.21) | Best: 4.21mm
Ep 220 | R: -16144.3 | Len: 500 | R/s: -32.29 (-4036.1%) | Gap: 4.33mm (min: 4.23) | Best: 4.21mm
Ep 240 | R: -16145.9 | Len: 500 | R/s: -32.29 (-4036.5%) | Gap: 4.32mm (min: 4.21) | Best: 4.21mm
Ep 260 | R: -16142.5 | Len: 500 | R/s: -32.28 (-4035.6%) | Gap: 4.34mm (min: 4.24) | Best: 4.21mm
Ep 280 | R: -16146.9 | Len: 500 | R/s: -32.29 (-4036.7%) | Gap: 4.32mm (min: 4.21) | Best: 4.21mm
Ep 300 | R: -16145.6 | Len: 500 | R/s: -32.29 (-4036.4%) | Gap: 4.34mm (min: 4.21) | Best: 4.21mm

19
lev_sim/RL_Trials/training_log_20251211_122333.txt Normal file
View File

@@ -0,0 +1,19 @@
Training Started: 20251211_122333
Number of Episodes: 2000
Print Frequency: 20
Target Gap Height: 16.491741 mm
Network: 256 hidden units with LayerNorm
Policy LR: 5e-4, Value LR: 1e-3, Entropy: 0.02
======================================================================
Ep 20 | R: -16150.6 | Len: 500 | R/s: -32.30 (-4037.7%) | Gap: 4.18mm (min: 4.10) | Best: 4.10mm
Ep 40 | R: -16154.2 | Len: 500 | R/s: -32.31 (-4038.5%) | Gap: 4.18mm (min: 4.13) | Best: 4.10mm
Ep 60 | R: -16155.5 | Len: 500 | R/s: -32.31 (-4038.9%) | Gap: 4.16mm (min: 4.12) | Best: 4.10mm
Ep 80 | R: -16151.9 | Len: 500 | R/s: -32.30 (-4038.0%) | Gap: 4.20mm (min: 4.13) | Best: 4.10mm
Ep 100 | R: -16155.1 | Len: 500 | R/s: -32.31 (-4038.8%) | Gap: 4.18mm (min: 4.13) | Best: 4.10mm
Ep 120 | R: -16155.3 | Len: 500 | R/s: -32.31 (-4038.8%) | Gap: 4.19mm (min: 4.14) | Best: 4.10mm
Ep 140 | R: -16154.6 | Len: 500 | R/s: -32.31 (-4038.7%) | Gap: 4.18mm (min: 4.12) | Best: 4.10mm
Ep 160 | R: -16154.8 | Len: 500 | R/s: -32.31 (-4038.7%) | Gap: 4.19mm (min: 4.12) | Best: 4.10mm
Ep 180 | R: -16157.5 | Len: 500 | R/s: -32.32 (-4039.4%) | Gap: 4.19mm (min: 4.11) | Best: 4.10mm
Ep 200 | R: -16157.7 | Len: 500 | R/s: -32.32 (-4039.4%) | Gap: 4.21mm (min: 4.17) | Best: 4.10mm
Ep 220 | R: -16159.4 | Len: 500 | R/s: -32.32 (-4039.8%) | Gap: 4.19mm (min: 4.12) | Best: 4.10mm

9
lev_sim/RL_Trials/training_log_20251211_184547.txt Normal file
View File

@@ -0,0 +1,9 @@
Training Started: 20251211_184547
Number of Episodes: 2000
Print Frequency: 20
Target Gap Height: 16.491741 mm
Network: 256 hidden units with LayerNorm
Policy LR: 5e-4, Value LR: 1e-3, Entropy: 0.02
======================================================================
Ep 20 | R: -14585.9 | Len: 453 | R/s: -32.19 (-4023.9%) | Gap: 21.69mm (min:10.80) | Best: 10.80mm

12
lev_sim/RL_Trials/training_log_20251211_185541.txt Normal file
View File

@@ -0,0 +1,12 @@
Training Started: 20251211_185541
Number of Episodes: 2000
Print Frequency: 20
Target Gap Height: 16.491741 mm
Network: 256 hidden units with LayerNorm
Policy LR: 5e-4, Value LR: 1e-3, Entropy: 0.02
======================================================================
Ep 20 | R: -264.1 | Len: 28 | R/s: -9.47 (-1183.4%) | Gap: 20.27mm (min:11.48) | Best: 11.48mm
Ep 40 | R: -222.6 | Len: 24 | R/s: -9.45 (-1181.5%) | Gap: 21.62mm (min:19.97) | Best: 11.48mm
Ep 60 | R: -211.8 | Len: 23 | R/s: -9.37 (-1171.3%) | Gap: 21.47mm (min:19.53) | Best: 11.48mm
Ep 80 | R: -144.7 | Len: 15 | R/s: -9.65 (-1206.2%) | Gap: 21.89mm (min:20.01) | Best: 11.48mm

40
lev_sim/RL_Trials/training_log_20251211_190447.txt Normal file
View File

@@ -0,0 +1,40 @@
Training Started: 20251211_190447
Number of Episodes: 2000
Print Frequency: 20
Target Gap Height: 16.491741 mm
Network: 256 hidden units with LayerNorm
Policy LR: 5e-4, Value LR: 1e-3, Entropy: 0.02
======================================================================
Ep 20 | R: -2049.0 | Len: 66 | R/s: -31.16 (-3895.5%) | Gap: 13.46mm (min:10.84) | Best: 10.84mm
Ep 40 | R: -1124.2 | Len: 36 | R/s: -30.89 (-3860.7%) | Gap: 12.94mm (min:10.62) | Best: 10.62mm
Ep 60 | R: -900.8 | Len: 29 | R/s: -30.75 (-3843.2%) | Gap: 12.07mm (min:10.35) | Best: 10.35mm
Ep 80 | R: -885.5 | Len: 29 | R/s: -30.75 (-3843.2%) | Gap: 11.88mm (min:10.26) | Best: 10.26mm
Ep 100 | R: -1242.5 | Len: 40 | R/s: -30.99 (-3873.3%) | Gap: 13.41mm (min:10.71) | Best: 10.26mm
Ep 120 | R: -972.3 | Len: 32 | R/s: -30.77 (-3846.0%) | Gap: 12.38mm (min:10.39) | Best: 10.26mm
Ep 140 | R: -1182.0 | Len: 38 | R/s: -30.90 (-3862.8%) | Gap: 13.08mm (min:10.60) | Best: 10.26mm
Ep 160 | R: -953.0 | Len: 31 | R/s: -30.74 (-3842.8%) | Gap: 12.06mm (min:10.68) | Best: 10.26mm
Ep 180 | R: -1178.0 | Len: 38 | R/s: -30.92 (-3864.9%) | Gap: 12.61mm (min:10.55) | Best: 10.26mm
Ep 200 | R: -3914.1 | Len: 125 | R/s: -31.34 (-3917.2%) | Gap: 13.81mm (min:10.84) | Best: 10.26mm
Ep 220 | R: -13001.1 | Len: 412 | R/s: -31.59 (-3948.3%) | Gap: 17.74mm (min:11.72) | Best: 10.26mm
Ep 240 | R: -15119.0 | Len: 478 | R/s: -31.60 (-3950.4%) | Gap: 18.71mm (min:13.85) | Best: 10.26mm
Ep 260 | R: -12974.1 | Len: 411 | R/s: -31.59 (-3949.3%) | Gap: 17.73mm (min:12.36) | Best: 10.26mm
Ep 280 | R: -13695.3 | Len: 433 | R/s: -31.60 (-3949.5%) | Gap: 18.07mm (min:12.24) | Best: 10.26mm
Ep 300 | R: -15814.3 | Len: 500 | R/s: -31.63 (-3953.6%) | Gap: 18.97mm (min:18.92) | Best: 10.26mm
Ep 320 | R: -15814.0 | Len: 500 | R/s: -31.63 (-3953.5%) | Gap: 18.97mm (min:18.93) | Best: 10.26mm
Ep 340 | R: -15809.3 | Len: 500 | R/s: -31.62 (-3952.3%) | Gap: 18.96mm (min:18.89) | Best: 10.26mm
Ep 360 | R: -15816.5 | Len: 500 | R/s: -31.63 (-3954.1%) | Gap: 18.97mm (min:18.91) | Best: 10.26mm
Ep 380 | R: -15815.5 | Len: 500 | R/s: -31.63 (-3953.9%) | Gap: 18.97mm (min:18.92) | Best: 10.26mm
Ep 400 | R: -15818.2 | Len: 500 | R/s: -31.64 (-3954.5%) | Gap: 18.98mm (min:18.93) | Best: 10.26mm
Ep 420 | R: -15815.1 | Len: 500 | R/s: -31.63 (-3953.8%) | Gap: 18.97mm (min:18.89) | Best: 10.26mm
Ep 440 | R: -15816.0 | Len: 500 | R/s: -31.63 (-3954.0%) | Gap: 18.97mm (min:18.94) | Best: 10.26mm
Ep 460 | R: -15818.5 | Len: 500 | R/s: -31.64 (-3954.6%) | Gap: 18.98mm (min:18.93) | Best: 10.26mm
Ep 480 | R: -15819.3 | Len: 500 | R/s: -31.64 (-3954.8%) | Gap: 18.98mm (min:18.94) | Best: 10.26mm
Ep 500 | R: -15818.5 | Len: 500 | R/s: -31.64 (-3954.6%) | Gap: 18.97mm (min:18.94) | Best: 10.26mm
Ep 520 | R: -15819.8 | Len: 500 | R/s: -31.64 (-3954.9%) | Gap: 18.98mm (min:18.94) | Best: 10.26mm
Ep 540 | R: -15818.8 | Len: 500 | R/s: -31.64 (-3954.7%) | Gap: 18.97mm (min:18.92) | Best: 10.26mm
Ep 560 | R: -15818.3 | Len: 500 | R/s: -31.64 (-3954.6%) | Gap: 18.97mm (min:18.92) | Best: 10.26mm
Ep 580 | R: -15822.9 | Len: 500 | R/s: -31.65 (-3955.7%) | Gap: 18.98mm (min:18.93) | Best: 10.26mm
Ep 600 | R: -15819.6 | Len: 500 | R/s: -31.64 (-3954.9%) | Gap: 18.97mm (min:18.92) | Best: 10.26mm
Ep 620 | R: -15817.8 | Len: 500 | R/s: -31.64 (-3954.5%) | Gap: 18.96mm (min:18.91) | Best: 10.26mm
Ep 640 | R: -15821.5 | Len: 500 | R/s: -31.64 (-3955.4%) | Gap: 18.98mm (min:18.92) | Best: 10.26mm

112
lev_sim/RL_Trials/training_log_20251211_191801.txt Normal file
View File

@@ -0,0 +1,112 @@
Training Started: 20251211_191801
Number of Episodes: 2000
Print Frequency: 20
Target Gap Height: 16.491741 mm
Network: 256 hidden units with LayerNorm
Policy LR: 5e-4, Value LR: 1e-3, Entropy: 0.02
======================================================================
Ep 20 | R: 755.5 | Len: 204 | R/s: 3.70 (462.3%) | Gap: 16.74mm (min:14.73) | Best: 14.73mm
Ep 40 | R: 407.2 | Len: 116 | R/s: 3.52 (439.9%) | Gap: 15.56mm (min:13.80) | Best: 13.80mm
Ep 60 | R: 157.4 | Len: 57 | R/s: 2.78 (347.5%) | Gap: 14.87mm (min:13.91) | Best: 13.80mm
Ep 80 | R: 182.7 | Len: 61 | R/s: 2.98 (372.0%) | Gap: 15.06mm (min:14.21) | Best: 13.80mm
Ep 100 | R: 487.2 | Len: 134 | R/s: 3.65 (455.9%) | Gap: 16.10mm (min:14.31) | Best: 13.80mm
Ep 120 | R: 1113.1 | Len: 297 | R/s: 3.75 (468.3%) | Gap: 17.64mm (min:15.95) | Best: 13.80mm
Ep 140 | R: 1434.7 | Len: 385 | R/s: 3.72 (465.6%) | Gap: 18.21mm (min:16.13) | Best: 13.80mm
Ep 160 | R: 641.4 | Len: 172 | R/s: 3.72 (464.8%) | Gap: 16.69mm (min:15.38) | Best: 13.80mm
Ep 180 | R: 1029.0 | Len: 274 | R/s: 3.76 (469.7%) | Gap: 17.43mm (min:14.60) | Best: 13.80mm
Ep 200 | R: 287.0 | Len: 85 | R/s: 3.39 (424.0%) | Gap: 15.61mm (min:14.18) | Best: 13.80mm
Ep 220 | R: 330.9 | Len: 94 | R/s: 3.52 (440.4%) | Gap: 15.85mm (min:14.93) | Best: 13.80mm
Ep 240 | R: 336.4 | Len: 103 | R/s: 3.28 (409.7%) | Gap: 15.15mm (min:13.83) | Best: 13.80mm
Ep 260 | R: 128.2 | Len: 50 | R/s: 2.58 (321.9%) | Gap: 14.51mm (min:13.90) | Best: 13.80mm
Ep 280 | R: 116.0 | Len: 46 | R/s: 2.51 (313.3%) | Gap: 14.30mm (min:13.49) | Best: 13.49mm
Ep 300 | R: 95.0 | Len: 39 | R/s: 2.45 (306.0%) | Gap: 13.85mm (min:13.19) | Best: 13.19mm
Ep 320 | R: 772.1 | Len: 200 | R/s: 3.86 (482.9%) | Gap: 16.77mm (min:15.05) | Best: 13.19mm
Ep 340 | R: 152.7 | Len: 54 | R/s: 2.84 (354.5%) | Gap: 14.81mm (min:13.96) | Best: 13.19mm
Ep 360 | R: 118.1 | Len: 47 | R/s: 2.52 (315.6%) | Gap: 14.36mm (min:13.50) | Best: 13.19mm
Ep 380 | R: 290.8 | Len: 81 | R/s: 3.60 (450.2%) | Gap: 15.56mm (min:14.06) | Best: 13.19mm
Ep 400 | R: 230.0 | Len: 69 | R/s: 3.35 (418.5%) | Gap: 15.38mm (min:14.74) | Best: 13.19mm
Ep 420 | R: 305.9 | Len: 85 | R/s: 3.58 (447.5%) | Gap: 15.82mm (min:15.08) | Best: 13.19mm
Ep 440 | R: 450.6 | Len: 116 | R/s: 3.90 (487.0%) | Gap: 16.25mm (min:14.81) | Best: 13.19mm
Ep 460 | R: 624.2 | Len: 161 | R/s: 3.89 (486.0%) | Gap: 16.65mm (min:15.01) | Best: 13.19mm
Ep 480 | R: 710.6 | Len: 192 | R/s: 3.70 (462.6%) | Gap: 16.62mm (min:14.71) | Best: 13.19mm
Ep 500 | R: 131.1 | Len: 49 | R/s: 2.65 (331.8%) | Gap: 14.44mm (min:13.45) | Best: 13.19mm
Ep 520 | R: 169.4 | Len: 58 | R/s: 2.90 (362.5%) | Gap: 14.97mm (min:14.22) | Best: 13.19mm
Ep 540 | R: 929.9 | Len: 263 | R/s: 3.53 (441.4%) | Gap: 16.99mm (min:14.49) | Best: 13.19mm
Ep 560 | R: 1760.6 | Len: 500 | R/s: 3.52 (440.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 580 | R: 1763.0 | Len: 500 | R/s: 3.53 (440.7%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 600 | R: 1775.4 | Len: 500 | R/s: 3.55 (443.8%) | Gap: 18.99mm (min:18.91) | Best: 13.19mm
Ep 620 | R: 1298.7 | Len: 355 | R/s: 3.66 (457.5%) | Gap: 17.94mm (min:14.49) | Best: 13.19mm
Ep 640 | R: 1576.3 | Len: 438 | R/s: 3.60 (450.3%) | Gap: 18.63mm (min:16.35) | Best: 13.19mm
Ep 660 | R: 1762.6 | Len: 500 | R/s: 3.53 (440.7%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 680 | R: 1761.3 | Len: 500 | R/s: 3.52 (440.3%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 700 | R: 1761.0 | Len: 500 | R/s: 3.52 (440.2%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 720 | R: 1754.8 | Len: 500 | R/s: 3.51 (438.7%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 740 | R: 1755.3 | Len: 500 | R/s: 3.51 (438.8%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 760 | R: 1756.6 | Len: 500 | R/s: 3.51 (439.2%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 780 | R: 1759.2 | Len: 500 | R/s: 3.52 (439.8%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 800 | R: 1756.9 | Len: 500 | R/s: 3.51 (439.2%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 820 | R: 1759.2 | Len: 500 | R/s: 3.52 (439.8%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 840 | R: 1593.2 | Len: 436 | R/s: 3.65 (456.7%) | Gap: 18.62mm (min:16.57) | Best: 13.19mm
Ep 860 | R: 1209.1 | Len: 334 | R/s: 3.62 (452.2%) | Gap: 17.92mm (min:15.21) | Best: 13.19mm
Ep 880 | R: 509.8 | Len: 149 | R/s: 3.43 (429.0%) | Gap: 16.16mm (min:14.21) | Best: 13.19mm
Ep 900 | R: 496.0 | Len: 148 | R/s: 3.36 (419.9%) | Gap: 15.86mm (min:14.56) | Best: 13.19mm
Ep 920 | R: 1770.0 | Len: 500 | R/s: 3.54 (442.5%) | Gap: 18.99mm (min:18.97) | Best: 13.19mm
Ep 940 | R: 1763.3 | Len: 500 | R/s: 3.53 (440.8%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 960 | R: 1753.9 | Len: 500 | R/s: 3.51 (438.5%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 980 | R: 1751.9 | Len: 500 | R/s: 3.50 (438.0%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1000 | R: 1756.6 | Len: 500 | R/s: 3.51 (439.1%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1020 | R: 1754.6 | Len: 500 | R/s: 3.51 (438.7%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1040 | R: 1759.2 | Len: 500 | R/s: 3.52 (439.8%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1060 | R: 1756.7 | Len: 500 | R/s: 3.51 (439.2%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1080 | R: 1758.8 | Len: 500 | R/s: 3.52 (439.7%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1100 | R: 1756.2 | Len: 500 | R/s: 3.51 (439.1%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1120 | R: 1756.5 | Len: 500 | R/s: 3.51 (439.1%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1140 | R: 1760.5 | Len: 500 | R/s: 3.52 (440.1%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1160 | R: 1760.5 | Len: 500 | R/s: 3.52 (440.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1180 | R: 1756.5 | Len: 500 | R/s: 3.51 (439.1%) | Gap: 19.00mm (min:18.99) | Best: 13.19mm
Ep 1200 | R: 1760.0 | Len: 500 | R/s: 3.52 (440.0%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1220 | R: 1758.7 | Len: 500 | R/s: 3.52 (439.7%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1240 | R: 1760.4 | Len: 500 | R/s: 3.52 (440.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1260 | R: 1753.5 | Len: 500 | R/s: 3.51 (438.4%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1280 | R: 1753.9 | Len: 500 | R/s: 3.51 (438.5%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1300 | R: 1758.0 | Len: 500 | R/s: 3.52 (439.5%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1320 | R: 1762.7 | Len: 500 | R/s: 3.53 (440.7%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1340 | R: 1693.0 | Len: 459 | R/s: 3.69 (460.9%) | Gap: 18.61mm (min:16.25) | Best: 13.19mm
Ep 1360 | R: 713.0 | Len: 181 | R/s: 3.94 (492.2%) | Gap: 16.38mm (min:15.05) | Best: 13.19mm
Ep 1380 | R: 2118.4 | Len: 486 | R/s: 4.36 (545.4%) | Gap: 18.38mm (min:17.32) | Best: 13.19mm
Ep 1400 | R: 2157.4 | Len: 495 | R/s: 4.36 (544.9%) | Gap: 18.50mm (min:18.01) | Best: 13.19mm
Ep 1420 | R: 1181.5 | Len: 262 | R/s: 4.50 (563.0%) | Gap: 16.90mm (min:15.79) | Best: 13.19mm
Ep 1440 | R: 1332.5 | Len: 298 | R/s: 4.46 (558.1%) | Gap: 17.08mm (min:15.65) | Best: 13.19mm
Ep 1460 | R: 1496.5 | Len: 332 | R/s: 4.51 (563.8%) | Gap: 17.27mm (min:15.62) | Best: 13.19mm
Ep 1480 | R: 1545.4 | Len: 339 | R/s: 4.56 (570.1%) | Gap: 17.26mm (min:15.87) | Best: 13.19mm
Ep 1500 | R: 862.8 | Len: 201 | R/s: 4.29 (536.3%) | Gap: 16.17mm (min:14.88) | Best: 13.19mm
Ep 1520 | R: 809.8 | Len: 193 | R/s: 4.20 (524.6%) | Gap: 16.03mm (min:14.74) | Best: 13.19mm
Ep 1540 | R: 861.1 | Len: 204 | R/s: 4.22 (527.7%) | Gap: 16.25mm (min:14.93) | Best: 13.19mm
Ep 1560 | R: 1445.2 | Len: 329 | R/s: 4.40 (549.4%) | Gap: 17.24mm (min:15.19) | Best: 13.19mm
Ep 1580 | R: 1993.4 | Len: 486 | R/s: 4.11 (513.2%) | Gap: 18.55mm (min:16.26) | Best: 13.19mm
Ep 1600 | R: 1985.4 | Len: 500 | R/s: 3.97 (496.4%) | Gap: 18.75mm (min:18.57) | Best: 13.19mm
Ep 1620 | R: 1776.8 | Len: 500 | R/s: 3.55 (444.2%) | Gap: 18.97mm (min:18.91) | Best: 13.19mm
Ep 1640 | R: 1755.2 | Len: 500 | R/s: 3.51 (438.8%) | Gap: 18.99mm (min:18.97) | Best: 13.19mm
Ep 1660 | R: 1751.1 | Len: 500 | R/s: 3.50 (437.8%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1680 | R: 1746.6 | Len: 500 | R/s: 3.49 (436.7%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1700 | R: 1746.2 | Len: 500 | R/s: 3.49 (436.5%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1720 | R: 1747.8 | Len: 500 | R/s: 3.50 (437.0%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1740 | R: 1743.0 | Len: 500 | R/s: 3.49 (435.8%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1760 | R: 1743.4 | Len: 500 | R/s: 3.49 (435.8%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1780 | R: 1744.3 | Len: 500 | R/s: 3.49 (436.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1800 | R: 1744.0 | Len: 500 | R/s: 3.49 (436.0%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1820 | R: 1739.4 | Len: 500 | R/s: 3.48 (434.8%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1840 | R: 1736.2 | Len: 500 | R/s: 3.47 (434.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1860 | R: 1732.7 | Len: 500 | R/s: 3.47 (433.2%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1880 | R: 1732.1 | Len: 500 | R/s: 3.46 (433.0%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1900 | R: 1732.2 | Len: 500 | R/s: 3.46 (433.0%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1920 | R: 1729.1 | Len: 500 | R/s: 3.46 (432.3%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1940 | R: 1728.6 | Len: 500 | R/s: 3.46 (432.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1960 | R: 1728.0 | Len: 500 | R/s: 3.46 (432.0%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 1980 | R: 1728.5 | Len: 500 | R/s: 3.46 (432.1%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
Ep 2000 | R: 1726.8 | Len: 500 | R/s: 3.45 (431.7%) | Gap: 19.00mm (min:19.00) | Best: 13.19mm
======================================================================
Training Completed: 20251211_195411
Best Gap Error Achieved: 13.191 mm

1777
lev_sim/Use of AI.txt Normal file
View File

File diff suppressed because it is too large Load Diff

362
lev_sim/archive_not_used/maglev_linearizer.py Normal file
View File

@@ -0,0 +1,362 @@
"""
Magnetic Levitation Jacobian Linearizer
Computes the local linear (Jacobian) approximation of the degree-6 polynomial
force/torque model at any operating point. The result is an LTI gain matrix
that relates small perturbations in (currL, currR, roll, gap_height) to
perturbations in (Force, Torque):
[ΔF ] [∂F/∂currL ∂F/∂currR ∂F/∂roll ∂F/∂gap] [ΔcurrL ]
[ΔTau] ≈ J [∂T/∂currL ∂T/∂currR ∂T/∂roll ∂T/∂gap] [ΔcurrR ]
[Δroll ]
[Δgap ]
Since the polynomial is analytic, all derivatives are computed exactly
(symbolic differentiation of the power-product terms), NOT by finite
differences.
The chain rule is applied automatically to convert the internal invGap
(= 1/gap_height) variable back to physical gap_height [mm].
Usage:
lin = MaglevLinearizer("maglev_model.pkl")
plant = lin.linearize(currL=-15, currR=-15, roll=0.0, gap_height=10.0)
print(plant)
print(plant.dF_dcurrL) # single gain
print(plant.control_jacobian) # 2×2 matrix mapping ΔcurrL/R → ΔF/ΔT
f, t = plant.predict(delta_currL=0.5) # quick what-if
"""
import numpy as np
import joblib
import os
class LinearizedPlant:
"""
Holds the Jacobian linearization of the force/torque model at one
operating point.
Attributes
----------
operating_point : dict
The (currL, currR, roll, gap_height) where linearization was computed.
f0 : float
Force [N] at the operating point.
tau0 : float
Torque [mN·m] at the operating point.
jacobian : ndarray, shape (2, 4)
Full Jacobian:
Row 0 = Force derivatives, Row 1 = Torque derivatives.
Columns = [currL [A], currR [A], rollDeg [deg], gap_height [mm]]
"""
INPUT_LABELS = ['currL [A]', 'currR [A]', 'rollDeg [deg]', 'gap_height [mm]']
def __init__(self, operating_point, f0, tau0, jacobian):
self.operating_point = operating_point
self.f0 = f0
self.tau0 = tau0
self.jacobian = jacobian
# ---- Individual gain accessors ----
@property
def dF_dcurrL(self):
"""∂Force/∂currL [N/A] at operating point."""
return self.jacobian[0, 0]
@property
def dF_dcurrR(self):
"""∂Force/∂currR [N/A] at operating point."""
return self.jacobian[0, 1]
@property
def dF_droll(self):
"""∂Force/∂roll [N/deg] at operating point."""
return self.jacobian[0, 2]
@property
def dF_dgap(self):
"""∂Force/∂gap_height [N/mm] at operating point.
Typically positive (unstable): force increases as gap shrinks.
"""
return self.jacobian[0, 3]
@property
def dT_dcurrL(self):
"""∂Torque/∂currL [mN·m/A] at operating point."""
return self.jacobian[1, 0]
@property
def dT_dcurrR(self):
"""∂Torque/∂currR [mN·m/A] at operating point."""
return self.jacobian[1, 1]
@property
def dT_droll(self):
"""∂Torque/∂roll [mN·m/deg] at operating point."""
return self.jacobian[1, 2]
@property
def dT_dgap(self):
"""∂Torque/∂gap_height [mN·m/mm] at operating point."""
return self.jacobian[1, 3]
@property
def control_jacobian(self):
"""2×2 sub-matrix mapping control inputs [ΔcurrL, ΔcurrR] → [ΔF, ΔT].
This is the "B" portion of the linearized plant that the PID
controller acts through.
"""
return self.jacobian[:, :2]
@property
def state_jacobian(self):
"""2×2 sub-matrix mapping state perturbations [Δroll, Δgap] → [ΔF, ΔT].
Contains the magnetic stiffness (∂F/∂gap) and roll coupling.
This feeds into the "A" matrix of the full mechanical state-space.
"""
return self.jacobian[:, 2:]
def predict(self, delta_currL=0.0, delta_currR=0.0,
delta_roll=0.0, delta_gap=0.0):
"""
Predict force and torque using the linear approximation.
Returns (force, torque) including the nominal operating-point values.
"""
delta = np.array([delta_currL, delta_currR, delta_roll, delta_gap])
perturbation = self.jacobian @ delta
return self.f0 + perturbation[0], self.tau0 + perturbation[1]
def __repr__(self):
op = self.operating_point
lines = [
f"LinearizedPlant @ currL={op['currL']:.1f}A, "
f"currR={op['currR']:.1f}A, "
f"roll={op['roll']:.2f}°, "
f"gap={op['gap_height']:.2f}mm",
f" F₀ = {self.f0:+.4f} N τ₀ = {self.tau0:+.4f} mN·m",
f" ∂F/∂currL = {self.dF_dcurrL:+.4f} N/A "
f"∂T/∂currL = {self.dT_dcurrL:+.4f} mN·m/A",
f" ∂F/∂currR = {self.dF_dcurrR:+.4f} N/A "
f"∂T/∂currR = {self.dT_dcurrR:+.4f} mN·m/A",
f" ∂F/∂roll = {self.dF_droll:+.4f} N/deg "
f"∂T/∂roll = {self.dT_droll:+.4f} mN·m/deg",
f" ∂F/∂gap = {self.dF_dgap:+.4f} N/mm "
f"∂T/∂gap = {self.dT_dgap:+.4f} mN·m/mm",
]
return '\n'.join(lines)
class MaglevLinearizer:
"""
Jacobian linearizer for the polynomial maglev force/torque model.
Loads the same .pkl model as MaglevPredictor, but instead of just
evaluating the polynomial, computes exact analytical partial derivatives
at any operating point.
"""
def __init__(self, model_path='maglev_model.pkl'):
if not os.path.exists(model_path):
raise FileNotFoundError(
f"Model file '{model_path}' not found. "
"Train and save the model from Function Fitting.ipynb first."
)
data = joblib.load(model_path)
poly_transformer = data['poly_features']
linear_model = data['model']
# powers_: (n_terms, n_inputs) — exponent matrix from sklearn
# Transpose to (n_inputs, n_terms) for broadcasting with x[:, None]
self.powers = poly_transformer.powers_.T.astype(np.float64)
self.force_coef = linear_model.coef_[0] # (n_terms,)
self.torque_coef = linear_model.coef_[1] # (n_terms,)
self.force_intercept = linear_model.intercept_[0]
self.torque_intercept = linear_model.intercept_[1]
self.degree = data['degree']
self.n_terms = self.powers.shape[1]
def _to_internal(self, currL, currR, roll, gap_height):
"""Convert physical inputs to the polynomial's internal variables."""
invGap = 1.0 / max(gap_height, 1e-6)
return np.array([currL, currR, roll, invGap], dtype=np.float64)
def evaluate(self, currL, currR, roll, gap_height):
"""
Evaluate the full (nonlinear) polynomial at a single point.
Returns
-------
force : float [N]
torque : float [mN·m]
"""
x = self._to_internal(currL, currR, roll, gap_height)
poly_features = np.prod(x[:, None] ** self.powers, axis=0)
force = np.dot(self.force_coef, poly_features) + self.force_intercept
torque = np.dot(self.torque_coef, poly_features) + self.torque_intercept
return float(force), float(torque)
def _jacobian_internal(self, x):
"""
Compute the 2×4 Jacobian w.r.t. the internal polynomial variables
(currL, currR, rollDeg, invGap).
For each variable x_k, the partial derivative of a polynomial term
c · x₁^a₁ · x₂^a₂ · x₃^a₃ · x₄^a₄
is:
c · a_k · x_k^(a_k - 1) · ∏_{j≠k} x_j^a_j
This is computed vectorised over all terms simultaneously.
"""
jac = np.zeros((2, 4))
for k in range(4):
# a_k for every term — this becomes the multiplicative scale
scale = self.powers[k, :] # (n_terms,)
# Reduce the k-th exponent by 1 (floored at 0; the scale
# factor of 0 for constant-in-x_k terms zeros those out)
deriv_powers = self.powers.copy()
deriv_powers[k, :] = np.maximum(deriv_powers[k, :] - 1.0, 0.0)
# Evaluate the derivative polynomial
poly_terms = np.prod(x[:, None] ** deriv_powers, axis=0)
deriv_features = scale * poly_terms # (n_terms,)
jac[0, k] = np.dot(self.force_coef, deriv_features)
jac[1, k] = np.dot(self.torque_coef, deriv_features)
return jac
def linearize(self, currL, currR, roll, gap_height):
"""
Compute the Jacobian linearization at the given operating point.
Parameters
----------
currL : float Left coil current [A]
currR : float Right coil current [A]
roll : float Roll angle [deg]
gap_height : float Air gap [mm]
Returns
-------
LinearizedPlant
Contains the operating-point values (F₀, τ₀) and the 2×4
Jacobian with columns [currL, currR, roll, gap_height].
"""
x = self._to_internal(currL, currR, roll, gap_height)
f0, tau0 = self.evaluate(currL, currR, roll, gap_height)
# Jacobian in internal coordinates (w.r.t. invGap in column 3)
jac_internal = self._jacobian_internal(x)
# Chain rule: ∂f/∂gap = ∂f/∂invGap · d(invGap)/d(gap)
# = ∂f/∂invGap · (−1 / gap²)
jac = jac_internal.copy()
jac[:, 3] *= -1.0 / (gap_height ** 2)
return LinearizedPlant(
operating_point={
'currL': currL, 'currR': currR,
'roll': roll, 'gap_height': gap_height,
},
f0=f0,
tau0=tau0,
jacobian=jac,
)
def gain_schedule(self, gap_heights, currL, currR, roll=0.0):
"""
Precompute linearizations across a range of gap heights at fixed
current and roll. Useful for visualising how plant gains vary
and for designing a gain-scheduled PID.
Parameters
----------
gap_heights : array-like of float [mm]
currL, currR : float [A]
roll : float [deg], default 0
Returns
-------
list of LinearizedPlant, one per gap height
"""
return [
self.linearize(currL, currR, roll, g)
for g in gap_heights
]
# ==========================================================================
# Demo / quick validation
# ==========================================================================
if __name__ == '__main__':
import sys
model_path = os.path.join(os.path.dirname(__file__), 'maglev_model.pkl')
lin = MaglevLinearizer(model_path)
# --- Single-point linearization ---
print("=" * 70)
print("SINGLE-POINT LINEARIZATION")
print("=" * 70)
plant = lin.linearize(currL=-15, currR=-15, roll=0.0, gap_height=10.0)
print(plant)
print()
# Quick sanity check: compare linear prediction vs full polynomial
dc = 0.5 # small current perturbation
f_lin, t_lin = plant.predict(delta_currL=dc)
f_act, t_act = lin.evaluate(-15 + dc, -15, 0.0, 10.0)
print(f"Linearised vs Actual (ΔcurrL = {dc:+.1f} A):")
print(f" Force: {f_lin:.4f} vs {f_act:.4f} (err {abs(f_lin-f_act):.6f} N)")
print(f" Torque: {t_lin:.4f} vs {t_act:.4f} (err {abs(t_lin-t_act):.6f} mN·m)")
print()
# --- Gain schedule across gap heights ---
print("=" * 70)
print("GAIN SCHEDULE (currL = currR = -15 A, roll = 0°)")
print("=" * 70)
gaps = [6, 8, 10, 12, 15, 20, 25]
plants = lin.gain_schedule(gaps, currL=-15, currR=-15, roll=0.0)
header = f"{'Gap [mm]':>10} {'F₀ [N]':>10} {'∂F/∂iL':>10} {'∂F/∂iR':>10} {'∂F/∂gap':>10} {'∂T/∂iL':>12} {'∂T/∂iR':>12}"
print(header)
print("-" * len(header))
for p in plants:
g = p.operating_point['gap_height']
print(
f"{g:10.1f} {p.f0:10.3f} {p.dF_dcurrL:10.4f} "
f"{p.dF_dcurrR:10.4f} {p.dF_dgap:10.4f} "
f"{p.dT_dcurrL:12.4f} {p.dT_dcurrR:12.4f}"
)
print()
# --- Note on PID usage ---
print("=" * 70)
print("NOTES FOR PID DESIGN")
print("=" * 70)
print("""
At each operating point, the linearized electromagnetic plant is:
[ΔF ] [∂F/∂iL ∂F/∂iR] [ΔiL] [∂F/∂roll ∂F/∂gap] [Δroll]
[ΔTau] = [∂T/∂iL ∂T/∂iR] [ΔiR] + [∂T/∂roll ∂T/∂gap] [Δgap ]
^^^^^^^^^^^^^^^^ ^^^^^^^^^^^^^^^^^^^^
control_jacobian state_jacobian
The full mechanical dynamics (linearized) are:
m · Δg̈ap = ΔF - m·g (vertical — note ∂F/∂gap > 0 means unstable)
Iz · Δroll̈ = ΔTau (roll)
So the PID loop sees:
control_jacobian → the gain from current commands to force/torque
state_jacobian → the coupling from state perturbations (acts like
a destabilising spring for gap, restoring for roll)
""")

653
lev_sim/archive_not_used/maglev_state_space.py Normal file
View File

@@ -0,0 +1,653 @@
"""
Full Linearized State-Space Model for the Guadaloop Maglev Pod
==============================================================
Combines three dynamic layers into a single LTI system ẋ = Ax + Bu, y = Cx:
Layer 1 — Coil RL dynamics (electrical):
di/dt = (V·pwm − R·i) / L
This is already linear. A first-order lag from PWM command to current.
Layer 2 — Electromagnetic force/torque map (from Ansys polynomial):
(F, τ) = f(iL, iR, roll, gap)
Nonlinear, but the MaglevLinearizer gives us the Jacobian at any
operating point, making it locally linear.
Layer 3 — Rigid-body mechanics (Newton/Euler):
m·z̈ = F_front + F_back − m·g (heave)
Iy·θ̈ = L_arm·(F_front − F_back) (pitch from force differential)
Ix·φ̈ = τ_front + τ_back (roll from magnetic torque)
These are linear once the force/torque are linearized.
The key coupling: the pod is rigid, so front and back yoke gaps are NOT
independent. They are related to the average gap and pitch angle:
gap_front = gap_avg − L_arm · pitch
gap_back = gap_avg + L_arm · pitch
This means a pitch perturbation changes both yoke gaps, which changes both
yoke forces, which feeds back into the heave and pitch dynamics. The
electromagnetic Jacobian captures how force/torque respond to these gap
changes, creating the destabilizing "magnetic stiffness" that makes maglev
inherently open-loop unstable.
State vector (10 states):
x = [gap_avg, gap_vel, pitch, pitch_rate, roll, roll_rate,
i_FL, i_FR, i_BL, i_BR]
- gap_avg [m]: average air gap (track-to-yoke distance)
- gap_vel [m/s]: d(gap_avg)/dt
- pitch [rad]: rotation about Y axis (positive = back hangs lower)
- pitch_rate [rad/s]
- roll [rad]: rotation about X axis
- roll_rate [rad/s]
- i_FL..BR [A]: the four coil currents
Input vector (4 inputs):
u = [pwm_FL, pwm_FR, pwm_BL, pwm_BR] (duty cycles, dimensionless)
Output vector (3 outputs):
y = [gap_avg, pitch, roll]
"""
import numpy as np
import os
from maglev_linearizer import MaglevLinearizer
# ---------------------------------------------------------------------------
# Physical constants and unit conversions
# ---------------------------------------------------------------------------
GRAVITY = 9.81 # m/s²
DEG2RAD = np.pi / 180.0
RAD2DEG = 180.0 / np.pi
# State indices (for readability)
GAP, GAPV, PITCH, PITCHV, ROLL, ROLLV, I_FL, I_FR, I_BL, I_BR = range(10)
# ===================================================================
# StateSpaceResult — the output container
# ===================================================================
class StateSpaceResult:
"""
Holds the A, B, C, D matrices of the linearized plant plus
operating-point metadata and stability analysis.
"""
STATE_LABELS = [
'gap_avg [m]', 'gap_vel [m/s]',
'pitch [rad]', 'pitch_rate [rad/s]',
'roll [rad]', 'roll_rate [rad/s]',
'i_FL [A]', 'i_FR [A]', 'i_BL [A]', 'i_BR [A]',
]
INPUT_LABELS = ['pwm_FL', 'pwm_FR', 'pwm_BL', 'pwm_BR']
OUTPUT_LABELS = ['gap_avg [m]', 'pitch [rad]', 'roll [rad]']
def __init__(self, A, B, C, D, operating_point,
equilibrium_force_error, plant_front, plant_back):
self.A = A
self.B = B
self.C = C
self.D = D
self.operating_point = operating_point
self.equilibrium_force_error = equilibrium_force_error
self.plant_front = plant_front # LinearizedPlant for front yoke
self.plant_back = plant_back # LinearizedPlant for back yoke
@property
def eigenvalues(self):
"""Eigenvalues of A, sorted by decreasing real part."""
eigs = np.linalg.eigvals(self.A)
return eigs[np.argsort(-np.real(eigs))]
@property
def is_open_loop_stable(self):
return bool(np.all(np.real(self.eigenvalues) < 0))
@property
def unstable_eigenvalues(self):
eigs = self.eigenvalues
return eigs[np.real(eigs) > 1e-8]
def to_scipy(self):
"""Convert to scipy.signal.StateSpace for frequency-domain analysis."""
from scipy.signal import StateSpace
return StateSpace(self.A, self.B, self.C, self.D)
def print_A_structure(self):
"""Print the A matrix with row/column labels for physical insight."""
labels_short = ['gap', 'ġap', 'θ', 'θ̇', 'φ', 'φ̇',
'iFL', 'iFR', 'iBL', 'iBR']
print("\nA matrix (non-zero entries):")
print("-" * 65)
for i in range(10):
for j in range(10):
if abs(self.A[i, j]) > 1e-10:
print(f" A[{labels_short[i]:>3}, {labels_short[j]:>3}] "
f"= {self.A[i,j]:+12.4f}")
print("-" * 65)
def print_B_structure(self):
"""Print the B matrix with labels."""
labels_short = ['gap', 'ġap', 'θ', 'θ̇', 'φ', 'φ̇',
'iFL', 'iFR', 'iBL', 'iBR']
u_labels = ['uFL', 'uFR', 'uBL', 'uBR']
print("\nB matrix (non-zero entries):")
print("-" * 50)
for i in range(10):
for j in range(4):
if abs(self.B[i, j]) > 1e-10:
print(f" B[{labels_short[i]:>3}, {u_labels[j]:>3}] "
f"= {self.B[i,j]:+12.4f}")
print("-" * 50)
def __repr__(self):
op = self.operating_point
eigs = self.eigenvalues
at_eq = abs(self.equilibrium_force_error) < 0.5
eq_str = ('AT EQUILIBRIUM' if at_eq
else f'NOT AT EQUILIBRIUM — {self.equilibrium_force_error:+.2f} N residual')
lines = [
"=" * 70,
"LINEARIZED MAGLEV STATE-SPACE (ẋ = Ax + Bu, y = Cx)",
"=" * 70,
f"Operating point:",
f" gap = {op['gap_height']:.2f} mm, "
f"currL = {op['currL']:.2f} A, "
f"currR = {op['currR']:.2f} A, "
f"roll = {op['roll']:.1f}°, "
f"pitch = {op['pitch']:.1f}°",
f" F_front = {self.plant_front.f0:.3f} N, "
f"F_back = {self.plant_back.f0:.3f} N, "
f"F_total = {self.plant_front.f0 + self.plant_back.f0:.3f} N, "
f"Weight = {op['mass'] * GRAVITY:.3f} N",
f" >> {eq_str}",
"",
f"System: {self.A.shape[0]} states × "
f"{self.B.shape[1]} inputs × "
f"{self.C.shape[0]} outputs",
f"Open-loop stable: {self.is_open_loop_stable}",
"",
"Eigenvalues of A:",
]
# Group complex conjugate pairs
printed = set()
for i, ev in enumerate(eigs):
if i in printed:
continue
re_part = np.real(ev)
im_part = np.imag(ev)
stability = "UNSTABLE" if re_part > 1e-8 else "stable"
if abs(im_part) < 1e-6:
lines.append(
f" λ = {re_part:+12.4f} "
f" τ = {abs(1/re_part)*1000 if abs(re_part) > 1e-8 else float('inf'):.2f} ms"
f" ({stability})"
)
else:
# Find conjugate pair
for j in range(i + 1, len(eigs)):
if j not in printed and abs(eigs[j] - np.conj(ev)) < 1e-6:
printed.add(j)
break
omega_n = abs(ev)
lines.append(
f" λ = {re_part:+12.4f} ± {abs(im_part):.4f}j"
f" ω_n = {omega_n:.1f} rad/s"
f" ({stability})"
)
lines.extend(["", "=" * 70])
return '\n'.join(lines)
# ===================================================================
# MaglevStateSpace — the builder
# ===================================================================
class MaglevStateSpace:
"""
Assembles the full 10-state linearized state-space from the
electromagnetic Jacobian + rigid body dynamics + coil dynamics.
Physical parameters come from the URDF (pod.xml) and MagLevCoil.
"""
def __init__(self, linearizer,
mass=9.4,
I_roll=0.0192942414, # Ixx from pod.xml [kg·m²]
I_pitch=0.130582305, # Iyy from pod.xml [kg·m²]
coil_R=1.1, # from MagLevCoil in lev_pod_env.py
coil_L=0.0025, # 2.5 mH
V_supply=12.0, # supply voltage [V]
L_arm=0.1259): # front/back yoke X-offset [m]
self.linearizer = linearizer
self.mass = mass
self.I_roll = I_roll
self.I_pitch = I_pitch
self.coil_R = coil_R
self.coil_L = coil_L
self.V_supply = V_supply
self.L_arm = L_arm
@staticmethod
def _convert_jacobian_to_si(jac):
"""
Convert a linearizer Jacobian from mixed units to pure SI.
The linearizer returns:
Row 0: Force [N] per [A, A, deg, mm]
Row 1: Torque [mN·m] per [A, A, deg, mm]
We need:
Row 0: Force [N] per [A, A, rad, m]
Row 1: Torque [N·m] per [A, A, rad, m]
Conversion factors:
col 0,1 (current): ×1 for force, ×(1/1000) for torque
col 2 (roll): ×(180/π) for force, ×(180/π)/1000 for torque
col 3 (gap): ×1000 for force, ×(1000/1000)=×1 for torque
"""
si = np.zeros((2, 4))
# Force row — already in N
si[0, 0] = jac[0, 0] # N/A → N/A
si[0, 1] = jac[0, 1] # N/A → N/A
si[0, 2] = jac[0, 2] * RAD2DEG # N/deg → N/rad
si[0, 3] = jac[0, 3] * 1000.0 # N/mm → N/m
# Torque row — from mN·m to N·m
si[1, 0] = jac[1, 0] / 1000.0 # mN·m/A → N·m/A
si[1, 1] = jac[1, 1] / 1000.0 # mN·m/A → N·m/A
si[1, 2] = jac[1, 2] / 1000.0 * RAD2DEG # mN·m/deg → N·m/rad
si[1, 3] = jac[1, 3] # mN·m/mm → N·m/m (factors cancel)
return si
def build(self, gap_height, currL, currR, roll=0.0, pitch=0.0):
"""
Build the A, B, C, D matrices at a given operating point.
Parameters
----------
gap_height : float Average gap [mm]
currL : float Equilibrium left coil current [A] (same front & back)
currR : float Equilibrium right coil current [A]
roll : float Equilibrium roll angle [deg], default 0
pitch : float Equilibrium pitch angle [deg], default 0
Non-zero pitch means front/back gaps differ.
Returns
-------
StateSpaceResult
"""
m = self.mass
Ix = self.I_roll
Iy = self.I_pitch
R = self.coil_R
Lc = self.coil_L
V = self.V_supply
La = self.L_arm
# ------------------------------------------------------------------
# Step 1: Compute individual yoke gaps from average gap + pitch
#
# The pod is rigid. If it pitches, the front and back yoke ends
# are at different distances from the track:
# gap_front = gap_avg − L_arm · sin(pitch) ≈ gap_avg − L_arm · pitch
# gap_back = gap_avg + L_arm · sin(pitch) ≈ gap_avg + L_arm · pitch
#
# Sign convention (from lev_pod_env.py lines 230-232):
# positive pitch = back gap > front gap (back hangs lower)
# ------------------------------------------------------------------
pitch_rad = pitch * DEG2RAD
# L_arm [m] * sin(pitch) [rad] → meters; convert to mm for linearizer
gap_front_mm = gap_height - La * np.sin(pitch_rad) * 1000.0
gap_back_mm = gap_height + La * np.sin(pitch_rad) * 1000.0
# ------------------------------------------------------------------
# Step 2: Linearize each yoke independently
#
# Each U-yoke has its own (iL, iR) pair and sees its own gap.
# Both yokes see the same roll angle (the pod is rigid).
# The linearizer returns the Jacobian in mixed units.
# ------------------------------------------------------------------
plant_f = self.linearizer.linearize(currL, currR, roll, gap_front_mm)
plant_b = self.linearizer.linearize(currL, currR, roll, gap_back_mm)
# ------------------------------------------------------------------
# Step 3: Convert Jacobians to SI
#
# After this, all gains are in [N or N·m] per [A, A, rad, m].
# Columns: [currL, currR, roll, gap_height]
# ------------------------------------------------------------------
Jf = self._convert_jacobian_to_si(plant_f.jacobian)
Jb = self._convert_jacobian_to_si(plant_b.jacobian)
# Unpack for clarity — subscript _f = front yoke, _b = back yoke
# Force gains
kFiL_f, kFiR_f, kFr_f, kFg_f = Jf[0]
kFiL_b, kFiR_b, kFr_b, kFg_b = Jb[0]
# Torque gains
kTiL_f, kTiR_f, kTr_f, kTg_f = Jf[1]
kTiL_b, kTiR_b, kTr_b, kTg_b = Jb[1]
# ------------------------------------------------------------------
# Step 4: Assemble the A matrix (10 × 10)
#
# The A matrix encodes three kinds of coupling:
#
# (a) Kinematic identities: gap_vel = d(gap)/dt, etc.
# These are always 1.0 on the super-diagonal of the
# position/velocity pairs.
#
# (b) Electromagnetic coupling through current states:
# Coil currents produce forces/torques. The linearized
# gains (∂F/∂i, ∂T/∂i) appear in the acceleration rows.
# This is the path from current states to mechanical
# acceleration — the "plant gain" that PID acts through.
#
# (c) Electromagnetic coupling through mechanical states:
# Gap and roll perturbations change the force/torque.
# This creates feedback loops:
#
# - ∂F/∂gap < 0 → gap perturbation changes force in a
# direction that AMPLIFIES the gap error → UNSTABLE
# (magnetic stiffness is "negative spring")
#
# - ∂T/∂roll → roll perturbation changes torque;
# sign determines whether roll is self-correcting or not
#
# - Pitch couples through gap_front/gap_back dependence
# on pitch angle, creating pitch instability too
# ------------------------------------------------------------------
A = np.zeros((10, 10))
# (a) Kinematic identities
A[GAP, GAPV] = 1.0
A[PITCH, PITCHV] = 1.0
A[ROLL, ROLLV] = 1.0
# ------------------------------------------------------------------
# HEAVE: m · Δgap̈ = −(ΔF_front + ΔF_back)
#
# The negative sign is because force is upward (+Z) but gap
# is measured downward (gap shrinks when pod moves up).
# At equilibrium F₀ = mg; perturbation ΔF pushes pod up → gap shrinks.
#
# Expanding ΔF using the rigid-body gap coupling:
# ΔF_front = kFg_f·(Δgap − La·Δpitch) + kFr_f·Δroll + kFiL_f·ΔiFL + kFiR_f·ΔiFR
# ΔF_back = kFg_b·(Δgap + La·Δpitch) + kFr_b·Δroll + kFiL_b·ΔiBL + kFiR_b·ΔiBR
# ------------------------------------------------------------------
# Gap → gap acceleration (magnetic stiffness, UNSTABLE)
A[GAPV, GAP] = -(kFg_f + kFg_b) / m
# Pitch → gap acceleration (cross-coupling through differential gap)
A[GAPV, PITCH] = -(-kFg_f + kFg_b) * La / m
# Roll → gap acceleration
A[GAPV, ROLL] = -(kFr_f + kFr_b) / m
# Current → gap acceleration (the control path!)
A[GAPV, I_FL] = -kFiL_f / m
A[GAPV, I_FR] = -kFiR_f / m
A[GAPV, I_BL] = -kFiL_b / m
A[GAPV, I_BR] = -kFiR_b / m
# ------------------------------------------------------------------
# PITCH: Iy · Δpitcḧ = La · (ΔF_front − ΔF_back)
#
# Pitch torque comes from DIFFERENTIAL FORCE, not from the
# electromagnetic torque (which acts on roll). This is because
# the front yoke is at x = +La and the back at x = −La:
# τ_pitch = F_front·La − F_back·La = La·(F_front − F_back)
#
# At symmetric equilibrium, F_front = F_back → zero pitch torque. ✓
# A pitch perturbation breaks this symmetry through the gap coupling.
# ------------------------------------------------------------------
# Gap → pitch acceleration (zero at symmetric equilibrium)
A[PITCHV, GAP] = La * (kFg_f - kFg_b) / Iy
# Pitch → pitch acceleration (pitch instability — UNSTABLE)
# = −La²·(kFg_f + kFg_b)/Iy. Since kFg < 0 → positive → unstable.
A[PITCHV, PITCH] = -La**2 * (kFg_f + kFg_b) / Iy
# Roll → pitch acceleration
A[PITCHV, ROLL] = La * (kFr_f - kFr_b) / Iy
# Current → pitch acceleration
A[PITCHV, I_FL] = La * kFiL_f / Iy
A[PITCHV, I_FR] = La * kFiR_f / Iy
A[PITCHV, I_BL] = -La * kFiL_b / Iy
A[PITCHV, I_BR] = -La * kFiR_b / Iy
# ------------------------------------------------------------------
# ROLL: Ix · Δroll̈ = Δτ_front + Δτ_back
#
# Unlike pitch (driven by force differential), roll is driven by
# the electromagnetic TORQUE directly. In the Ansys model, torque
# is the moment about the X axis produced by the asymmetric flux
# in the left vs right legs of each U-yoke.
#
# The torque Jacobian entries determine stability:
# - ∂T/∂roll: if this causes torque that amplifies roll → unstable
# - ∂T/∂iL, ∂T/∂iR: how current asymmetry controls roll
# ------------------------------------------------------------------
# Gap → roll acceleration
A[ROLLV, GAP] = (kTg_f + kTg_b) / Ix
# Pitch → roll acceleration (cross-coupling)
A[ROLLV, PITCH] = (-kTg_f + kTg_b) * La / Ix
# Roll → roll acceleration (roll stiffness)
A[ROLLV, ROLL] = (kTr_f + kTr_b) / Ix
# Current → roll acceleration
A[ROLLV, I_FL] = kTiL_f / Ix
A[ROLLV, I_FR] = kTiR_f / Ix
A[ROLLV, I_BL] = kTiL_b / Ix
A[ROLLV, I_BR] = kTiR_b / Ix
# ------------------------------------------------------------------
# COIL DYNAMICS: L·di/dt = V·pwm − R·i
#
# Rearranged: di/dt = −(R/L)·i + (V/L)·pwm
#
# This is a simple first-order lag with:
# - Time constant τ_coil = L/R = 2.5ms/1.1 = 2.27 ms
# - Eigenvalue = −R/L = −440 (very fast, well-damped)
#
# The coil dynamics act as a low-pass filter between the PWM
# command and the actual current. For PID frequencies below
# ~100 Hz, this lag is small but not negligible.
# ------------------------------------------------------------------
for k in range(I_FL, I_BR + 1):
A[k, k] = -R / Lc
# ------------------------------------------------------------------
# Step 5: B matrix (10 × 4)
#
# Only the coil states respond directly to the PWM inputs.
# The mechanical states are affected INDIRECTLY: pwm → current
# → force/torque → acceleration. This indirect path shows up
# as the product A_mech_curr × B_curr_pwm in the transfer function.
#
# B[coil_k, pwm_k] = V_supply / L_coil
# ------------------------------------------------------------------
B = np.zeros((10, 4))
for k in range(4):
B[I_FL + k, k] = V / Lc
# ------------------------------------------------------------------
# Step 6: C matrix (3 × 10)
#
# Default outputs are the three controlled DOFs:
# gap_avg, pitch, roll
# These are directly the position states.
# ------------------------------------------------------------------
C = np.zeros((3, 10))
C[0, GAP] = 1.0 # gap_avg
C[1, PITCH] = 1.0 # pitch
C[2, ROLL] = 1.0 # roll
# D = 0 (no direct feedthrough from PWM to position)
D = np.zeros((3, 4))
# ------------------------------------------------------------------
# Step 7: Equilibrium check
#
# At a valid operating point, the total magnetic force should
# equal the pod weight. A large residual means the linearization
# is valid mathematically but not physically meaningful (the pod
# wouldn't hover at this point without acceleration).
# ------------------------------------------------------------------
F_total = plant_f.f0 + plant_b.f0
weight = m * GRAVITY
eq_error = F_total - weight
return StateSpaceResult(
A=A, B=B, C=C, D=D,
operating_point={
'gap_height': gap_height,
'currL': currL, 'currR': currR,
'roll': roll, 'pitch': pitch,
'mass': m,
},
equilibrium_force_error=eq_error,
plant_front=plant_f,
plant_back=plant_b,
)
def find_equilibrium_current(self, gap_height, roll=0.0, tol=0.01):
"""
Find the symmetric current (currL = currR = I) that makes
total force = weight at the given gap height.
Uses bisection over the current range. The search assumes
negative currents produce attractive (upward) force, which
matches the Ansys model convention.
Parameters
----------
gap_height : float Target gap [mm]
roll : float Roll angle [deg], default 0
tol : float Force tolerance [N]
Returns
-------
float : equilibrium current [A]
"""
target_per_yoke = self.mass * GRAVITY / 2.0
def force_residual(curr):
f, _ = self.linearizer.evaluate(curr, curr, roll, gap_height)
return f - target_per_yoke
# Bisection search over negative current range
# (More negative = stronger attraction)
a, b = -20.0, 0.0
fa, fb = force_residual(a), force_residual(b)
if fa * fb > 0:
# Try positive range too
a, b = 0.0, 20.0
fa, fb = force_residual(a), force_residual(b)
if fa * fb > 0:
raise ValueError(
f"Cannot find equilibrium current at gap={gap_height}mm. "
f"Force at I=−20A: {target_per_yoke + force_residual(-20):.1f}N, "
f"at I=0: {target_per_yoke + force_residual(0):.1f}N, "
f"at I=+20A: {target_per_yoke + force_residual(20):.1f}N, "
f"target per yoke: {target_per_yoke:.1f}N"
)
for _ in range(100):
mid = (a + b) / 2.0
fmid = force_residual(mid)
if abs(fmid) < tol:
return mid
if fa * fmid < 0:
b = mid
else:
a, fa = mid, fmid
return (a + b) / 2.0
# ======================================================================
# Demo
# ======================================================================
if __name__ == '__main__':
model_path = os.path.join(os.path.dirname(__file__), 'maglev_model.pkl')
lin = MaglevLinearizer(model_path)
ss = MaglevStateSpace(lin)
# ------------------------------------------------------------------
# Find the equilibrium current at the target gap
# ------------------------------------------------------------------
TARGET_GAP_MM = 16.491741 # from lev_pod_env.py
print("=" * 70)
print("FINDING EQUILIBRIUM CURRENT")
print("=" * 70)
I_eq = ss.find_equilibrium_current(TARGET_GAP_MM)
F_eq, T_eq = lin.evaluate(I_eq, I_eq, 0.0, TARGET_GAP_MM)
print(f"Target gap: {TARGET_GAP_MM:.3f} mm")
print(f"Pod weight: {ss.mass * GRAVITY:.3f} N ({ss.mass} kg)")
print(f"Required per yoke: {ss.mass * GRAVITY / 2:.3f} N")
print(f"Equilibrium current: {I_eq:.4f} A (symmetric, currL = currR)")
print(f"Force per yoke at equilibrium: {F_eq:.3f} N")
print(f"Equilibrium PWM duty cycle: {I_eq * ss.coil_R / ss.V_supply:.4f}")
print()
# ------------------------------------------------------------------
# Build the state-space at equilibrium
# ------------------------------------------------------------------
result = ss.build(
gap_height=TARGET_GAP_MM,
currL=I_eq,
currR=I_eq,
roll=0.0,
pitch=0.0,
)
print(result)
print()
# ------------------------------------------------------------------
# Show the coupling structure
# ------------------------------------------------------------------
result.print_A_structure()
result.print_B_structure()
# ------------------------------------------------------------------
# Physical interpretation of key eigenvalues
# ------------------------------------------------------------------
eigs = result.eigenvalues
unstable = result.unstable_eigenvalues
print(f"\nUnstable modes: {len(unstable)}")
for ev in unstable:
# Time to double = ln(2) / real_part
t_double = np.log(2) / np.real(ev) * 1000 # ms
print(f" λ = {np.real(ev):+.4f} → amplitude doubles in {t_double:.1f} ms")
print()
print("The PID loop must have bandwidth FASTER than these unstable modes")
print("to stabilize the plant.")
# ------------------------------------------------------------------
# Gain schedule: how eigenvalues change with gap
# ------------------------------------------------------------------
print("\n" + "=" * 70)
print("GAIN SCHEDULE: unstable eigenvalues vs gap height")
print("=" * 70)
gaps = [8, 10, 12, 14, TARGET_GAP_MM, 18, 20, 25]
header = f"{'Gap [mm]':>10} {'I_eq [A]':>10} {'λ_heave':>12} {'t_dbl [ms]':>12} {'λ_pitch':>12} {'t_dbl [ms]':>12}"
print(header)
print("-" * len(header))
for g in gaps:
try:
I = ss.find_equilibrium_current(g)
r = ss.build(g, I, I, 0.0, 0.0)
ue = r.unstable_eigenvalues
real_ue = sorted(np.real(ue), reverse=True)
# Typically: largest = heave, second = pitch
lam_h = real_ue[0] if len(real_ue) > 0 else 0.0
lam_p = real_ue[1] if len(real_ue) > 1 else 0.0
t_h = np.log(2) / lam_h * 1000 if lam_h > 0 else float('inf')
t_p = np.log(2) / lam_p * 1000 if lam_p > 0 else float('inf')
print(f"{g:10.2f} {I:10.4f} {lam_h:+12.4f} {t_h:12.1f} "
f"{lam_p:+12.4f} {t_p:12.1f}")
except ValueError as e:
print(f"{g:10.2f} (no equilibrium found)")

1032
lev_sim/lev_PID.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

795
lev_sim/lev_PPO.ipynb Normal file
View File

File diff suppressed because one or more lines are too long

730
lev_sim/lev_pod_env.py Normal file
View File

@@ -0,0 +1,730 @@
import gymnasium as gym
from gymnasium import spaces
import pybullet as p
import pybullet_data
import numpy as np
import os
from datetime import datetime
from mag_lev_coil import MagLevCoil
from maglev_predictor import MaglevPredictor
TARGET_GAP = 11.86 / 1000 # target gap height for 9.4 kg pod in meters
class LevPodEnv(gym.Env):
def __init__(self, use_gui=False, initial_gap_mm=10.0, max_steps=2000, disturbance_force_std=0.0,
record_video=False, record_telemetry=False, record_dir="recordings"):
super(LevPodEnv, self).__init__()
# Store initial gap height parameter
self.initial_gap_mm = initial_gap_mm
self.max_episode_steps = max_steps
self.current_step = 0
# Stochastic disturbance parameter (standard deviation of random force in Newtons)
self.disturbance_force_std = disturbance_force_std
# Recording parameters
self.record_video = record_video
self.record_telemetry = record_telemetry
self.record_dir = record_dir
self._frame_skip = 4 # Capture every 4th step → 60fps video from 240Hz sim
self._video_width = 640
self._video_height = 480
self._frames = []
self._telemetry = {}
self._recording_active = False
# The following was coded by AI - see [1]
# --- 1. Define Action & Observation Spaces ---
# Action: 4 PWM duty cycles between -1 and 1 (4 independent coils)
# [front_left, front_right, back_left, back_right] corresponding to +Y and -Y ends of each U-yoke
self.action_space = spaces.Box(low=-1, high=1, shape=(4,), dtype=np.float32)
# Observation: 4 normalized noisy sensor gap heights + 4 normalized velocities
# Gaps normalized by 0.030m, velocities by 0.1 m/s
self.observation_space = spaces.Box(low=-5.0, high=5.0, shape=(8,), dtype=np.float32)
# --- 2. Setup Physics & Actuators ---
self.dt = 1./240. # PyBullet default timestep
self.coil_front_L = MagLevCoil(1.1, 0.0025, 12, 10.2)
self.coil_front_R = MagLevCoil(1.1, 0.0025, 12, 10.2)
self.coil_back_L = MagLevCoil(1.1, 0.0025, 12, 10.2)
self.coil_back_R = MagLevCoil(1.1, 0.0025, 12, 10.2)
# Sensor noise parameters
self.sensor_noise_std = 0.0001 # 0.1mm standard deviation
# Normalization constants for observations
self.gap_scale = 0.015 # Normalize gaps by +-15mm max expected deviation from middle
self.velocity_scale = 0.1 # Normalize velocities by 0.1 m/s max expected velocity
# Maglev force/torque predictor
self.predictor = MaglevPredictor()
# Connect to PyBullet (DIRECT is faster for training, GUI for debugging)
self.client = p.connect(p.GUI if use_gui else p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# Store references
self.trackId = None
self.podId = None
self.collision_local_positions = []
self.yoke_indices = [] # For force application
self.yoke_labels = []
self.sensor_indices = [] # For gap height measurement
self.sensor_labels = []
# For velocity calculation
self.prev_sensor_gaps = None
def reset(self, seed=None, options=None):
# Reset PyBullet simulation
p.resetSimulation(physicsClientId=self.client)
p.setGravity(0, 0, -9.81, physicsClientId=self.client)
p.setTimeStep(self.dt, physicsClientId=self.client)
# Create the maglev track (inverted system - track above, pod hangs below)
# Track bottom surface at Z=0
track_collision = p.createCollisionShape(
shapeType=p.GEOM_BOX,
halfExtents=[1.0, 0.2, 0.010],
physicsClientId=self.client
)
track_visual = p.createVisualShape(
shapeType=p.GEOM_BOX,
halfExtents=[1.0, 0.2, 0.010],
rgbaColor=[0.3, 0.3, 0.3, 0.8],
physicsClientId=self.client
)
self.trackId = p.createMultiBody(
baseMass=0, # Static
baseCollisionShapeIndex=track_collision,
baseVisualShapeIndex=track_visual,
basePosition=[0, 0, 0.010], # Track center at Z=10mm, bottom at Z=0
physicsClientId=self.client
)
p.changeDynamics(self.trackId, -1,
lateralFriction=0.3,
restitution=0.1,
physicsClientId=self.client)
urdf_path = self._create_modified_urdf()
# Determine start condition
# if np.random.rand() > 0.5:
# # Spawn exactly at target
# spawn_gap_mm = TARGET_GAP * 1000.0
# # # Add tiny noise
# # spawn_gap_mm += np.random.uniform(-0.5, 0.5)
# else:
spawn_gap_mm = self.initial_gap_mm
start_z = -(0.09085 + spawn_gap_mm / 1000.0)
start_pos = [0, 0, start_z]
start_orientation = p.getQuaternionFromEuler([0, 0, 0])
self.podId = p.loadURDF(urdf_path, start_pos, start_orientation, physicsClientId=self.client)
# The following was coded by AI - see [2]
# Parse collision shapes to identify yokes and sensors
collision_shapes = p.getCollisionShapeData(self.podId, -1, physicsClientId=self.client)
self.collision_local_positions = []
self.yoke_indices = []
self.yoke_labels = []
self.sensor_indices = []
self.sensor_labels = []
# Expected heights for detection
expected_yoke_sensor_z = 0.08585 # Yokes and sensors always at this height
expected_bolt_z = 0.08585 + self.initial_gap_mm / 1000.0 # Bolts at gap-dependent height
for i, shape in enumerate(collision_shapes):
shape_type = shape[2]
local_pos = shape[5]
self.collision_local_positions.append(local_pos)
# Check if at sensor/yoke height (Z ≈ 0.08585m) - NOT bolts
if abs(local_pos[2] - expected_yoke_sensor_z) < 0.001:
if shape_type == p.GEOM_BOX:
# Yokes are BOX type at the four corners (size 0.0254)
self.yoke_indices.append(i)
x_pos = "Front" if local_pos[0] > 0 else "Back"
y_pos = "Left" if local_pos[1] > 0 else "Right"
self.yoke_labels.append(f"{x_pos}_{y_pos}")
elif shape_type == p.GEOM_CYLINDER or shape_type == p.GEOM_MESH:
# Sensors: distinguish by position pattern
if abs(local_pos[0]) < 0.06 or abs(local_pos[1]) < 0.02:
self.sensor_indices.append(i)
if abs(local_pos[0]) < 0.001: # Center sensors (X ≈ 0)
# +Y = Left, -Y = Right (consistent with yoke labeling)
label = "Center_Left" if local_pos[1] > 0 else "Center_Right"
else: # Front/back sensors (Y ≈ 0)
label = "Front" if local_pos[0] > 0 else "Back"
self.sensor_labels.append(label)
self.coil_front_L.current = 0
self.coil_front_R.current = 0
self.coil_back_L.current = 0
self.coil_back_R.current = 0
self.prev_sensor_gaps = None
obs = self._get_obs(initial_reset=True)
self.current_step = 0
# --- Recording setup ---
# Finalize any previous episode's recording before starting new one
if self._recording_active:
self._finalize_recording()
if self.record_video or self.record_telemetry:
self._recording_active = True
os.makedirs(self.record_dir, exist_ok=True)
if self.record_video:
self._frames = []
# Pod body center ≈ start_z, yoke tops at ≈ start_z + 0.091
# Track bottom at Z=0. Focus camera on the pod body, looking from the side
# so both the pod and the track bottom (with gap between) are visible.
pod_center_z = start_z + 0.045 # Approximate visual center of pod
self._view_matrix = p.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=[0, 0, pod_center_z],
distance=0.55,
yaw=50, # Front-right angle
pitch=-5, # Nearly horizontal to see gap from the side
roll=0,
upAxisIndex=2,
physicsClientId=self.client
)
self._proj_matrix = p.computeProjectionMatrixFOV(
fov=35, # Narrow FOV for less distortion at close range
aspect=self._video_width / self._video_height,
nearVal=0.01,
farVal=2.0,
physicsClientId=self.client
)
if self.record_telemetry:
self._telemetry = {
'time': [], 'gap_FL': [], 'gap_FR': [], 'gap_BL': [], 'gap_BR': [],
'gap_front_avg': [], 'gap_back_avg': [], 'gap_avg': [],
'roll_deg': [], 'pitch_deg': [],
'curr_FL': [], 'curr_FR': [], 'curr_BL': [], 'curr_BR': [],
'force_front': [], 'force_back': [],
'torque_front': [], 'torque_back': [],
}
return obs, {}
# The following was generated by AI - see [14]
def step(self, action):
# Check if PyBullet connection is still active (GUI might be closed)
try:
p.getConnectionInfo(physicsClientId=self.client)
except p.error:
# Connection lost - GUI was closed
return self._get_obs(), -100.0, True, True, {'error': 'GUI closed'}
# Update Coil Currents from PWM Actions
pwm_front_L = action[0] # yoke +x,+y
pwm_front_R = action[1] # yoke +x,-y
pwm_back_L = action[2] # yoke -x,+y
pwm_back_R = action[3] # yoke -x,-y
curr_front_L = self.coil_front_L.update(pwm_front_L, self.dt)
curr_front_R = self.coil_front_R.update(pwm_front_R, self.dt)
curr_back_L = self.coil_back_L.update(pwm_back_L, self.dt)
curr_back_R = self.coil_back_R.update(pwm_back_R, self.dt)
# --- 2. Get Current Pod State ---
pos, orn = p.getBasePositionAndOrientation(self.podId, physicsClientId=self.client)
lin_vel, ang_vel = p.getBaseVelocity(self.podId, physicsClientId=self.client)
# Convert quaternion to rotation matrix
rot_matrix = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3)
# --- 3. Calculate Gap Heights at Yoke Positions (for force prediction) ---
# Calculate world positions of the 4 yokes (ends of U-yokes)
yoke_gap_heights_dict = {} # Store by label for easy access
for i, yoke_idx in enumerate(self.yoke_indices):
local_pos = self.collision_local_positions[yoke_idx]
local_vec = np.array(local_pos)
world_offset = rot_matrix @ local_vec
world_pos = np.array(pos) + world_offset
# Top surface of yoke box (add half-height = 5mm)
yoke_top_z = world_pos[2] + 0.005
# Gap height: track bottom (Z=0) to yoke top (negative Z)
gap_height = -yoke_top_z # Convert to positive gap in meters
yoke_gap_heights_dict[self.yoke_labels[i]] = gap_height
# Average gap heights for each U-shaped yoke (average left and right ends)
# Front yoke: average of Front_Left and Front_Right
# Back yoke: average of Back_Left and Back_Right
avg_gap_front = (yoke_gap_heights_dict.get('Front_Left', 0.010) +
yoke_gap_heights_dict.get('Front_Right', 0.010)) / 2
avg_gap_back = (yoke_gap_heights_dict.get('Back_Left', 0.010) +
yoke_gap_heights_dict.get('Back_Right', 0.010)) / 2
front_left_gap = yoke_gap_heights_dict.get('Front_Left', 0.010)
front_right_gap = yoke_gap_heights_dict.get('Front_Right', 0.010)
back_left_gap = yoke_gap_heights_dict.get('Back_Left', 0.010)
back_right_gap = yoke_gap_heights_dict.get('Back_Right', 0.010)
# hypotenuses
y_distance = 0.1016 # 2 * 0.0508m (left to right distance)
x_distance = 0.2518 # 2 * 0.1259m (front to back distance)
# Roll angle
# When right side has larger gap, roll is negative
roll_angle_front = np.arcsin(-(front_right_gap - front_left_gap) / y_distance)
roll_angle_back = np.arcsin(-(back_right_gap - back_left_gap) / y_distance)
roll_angle = (roll_angle_front + roll_angle_back) / 2
# When back has larger gap, pitch is positive
pitch_angle_left = np.arcsin((back_left_gap - front_left_gap) / x_distance)
pitch_angle_right = np.arcsin((back_right_gap - front_right_gap) / x_distance)
pitch_angle = (pitch_angle_left + pitch_angle_right) / 2
# Predict Forces and Torques using Maglev Predictor
# Gap heights in mm
gap_front_mm = avg_gap_front * 1000
gap_back_mm = avg_gap_back * 1000
# Roll angle in degrees
roll_deg = np.degrees(roll_angle)
# Predict force and torque for each U-shaped yoke
# Front yoke
force_front, torque_front = self.predictor.predict(
curr_front_L, curr_front_R, roll_deg, gap_front_mm
)
# Back yoke
force_back, torque_back = self.predictor.predict(
curr_back_L, curr_back_R, roll_deg, gap_back_mm
)
# --- 5. Apply Forces and Torques to Pod ---
# Forces are applied at Y=0 (center of U-yoke) at each X position
# This is where the actual magnetic force acts on the U-shaped yoke
# Apply force at front yoke center (X=+0.1259, Y=0)
front_yoke_center = [0.1259, 0, 0.08585] # From pod.xml yoke positions
p.applyExternalForce(
self.podId, -1,
forceObj=[0, 0, force_front],
posObj=front_yoke_center,
flags=p.LINK_FRAME,
physicsClientId=self.client
)
# Apply force at back yoke center (X=-0.1259, Y=0)
back_yoke_center = [-0.1259, 0, 0.08585]
p.applyExternalForce(
self.podId, -1,
forceObj=[0, 0, force_back],
posObj=back_yoke_center,
flags=p.LINK_FRAME,
physicsClientId=self.client
)
# Apply roll torques
# Each yoke produces its own torque about X axis
torque_front_Nm = torque_front / 1000 # Convert from mN·m to N·m
torque_back_Nm = torque_back / 1000
# Apply torques at respective yoke positions
p.applyExternalTorque(
self.podId, -1,
torqueObj=[torque_front_Nm, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self.client
)
p.applyExternalTorque(
self.podId, -1,
torqueObj=[torque_back_Nm, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self.client
)
# --- 5b. Apply Stochastic Disturbance Force and Torques (if enabled) ---
if self.disturbance_force_std > 0:
disturbance_force = np.random.normal(0, self.disturbance_force_std)
p.applyExternalForce(
self.podId, -1,
forceObj=[0, 0, disturbance_force],
posObj=[0, 0, 0],
flags=p.LINK_FRAME,
physicsClientId=self.client
)
# Roll and pitch disturbance torques, scaled from heave force (torque ~ force * moment_arm)
# Moment arm ~ 0.15 m so e.g. 1 N force -> 0.15 N·m torque std
disturbance_torque_std = self.disturbance_force_std * 0.15
roll_torque = np.random.normal(0, disturbance_torque_std)
pitch_torque = np.random.normal(0, disturbance_torque_std)
p.applyExternalTorque(
self.podId, -1,
torqueObj=[roll_torque, pitch_torque, 0],
flags=p.LINK_FRAME,
physicsClientId=self.client
)
# --- 6. Step Simulation ---
p.stepSimulation(physicsClientId=self.client)
self.current_step += 1
# Check for physical contact with track (bolts touching)
contact_points = p.getContactPoints(bodyA=self.podId, bodyB=self.trackId, physicsClientId=self.client)
has_contact = len(contact_points) > 0
# --- 7. Get New Observation ---
obs = self._get_obs()
# --- 8. Calculate Reward ---
# Goal: Hover at target gap (11.8mm), minimize roll/pitch, minimize power
target_gap = TARGET_GAP # 11.8mm in meters
avg_gap = (avg_gap_front + avg_gap_back) / 2
gap_error = abs(avg_gap - target_gap)
# Power dissipation (all 4 coils)
power = (curr_front_L**2 * self.coil_front_L.R +
curr_front_R**2 * self.coil_front_R.R +
curr_back_L**2 * self.coil_back_L.R +
curr_back_R**2 * self.coil_back_R.R)
# --- Improved Reward Function ---
# Use reward shaping with reasonable scales to enable learning
# 1. Gap Error Reward (most important)
# Use exponential decay for smooth gradient near target
gap_error_mm = gap_error * 1000 # Convert to mm
gap_reward = 10.0 * np.exp(-0.5 * (gap_error_mm / 3.0)**2) # Peak at 0mm error, 3mm std dev
# 2. Orientation Penalties (smaller scale)
roll_penalty = abs(np.degrees(roll_angle)) * 0.02
pitch_penalty = abs(np.degrees(pitch_angle)) * 0.02
# 3. Velocity Penalty (discourage rapid oscillations)
z_velocity = lin_vel[2]
velocity_penalty = abs(z_velocity) * 0.1
# 4. Contact Penalty
contact_points = p.getContactPoints(bodyA=self.podId, bodyB=self.trackId)
contact_penalty = len(contact_points) * 0.2
# 5. Power Penalty (encourage efficiency, but small weight)
power_penalty = power * 0.001
# Combine rewards (scaled to ~[-5, +1] range per step)
reward = gap_reward - roll_penalty - pitch_penalty - velocity_penalty - contact_penalty - power_penalty
# Check Termination (tighter bounds for safety)
terminated = False
truncated = False
# Terminate if gap is too small (crash) or too large (lost)
if avg_gap < 0.003 or avg_gap > 0.035:
terminated = True
reward = -10.0 # Failure penalty (scaled down)
# Terminate if orientation is too extreme
if abs(roll_angle) > np.radians(15) or abs(pitch_angle) > np.radians(15):
terminated = True
reward = -10.0
# Success bonus for stable hovering near target
if gap_error_mm < 1.0 and abs(np.degrees(roll_angle)) < 2.0 and abs(np.degrees(pitch_angle)) < 2.0:
reward += 2.0 # Bonus for excellent control
# Ground truth info dict (from PyBullet physical state, not sensor observations)
info = {
'curr_front_L': curr_front_L,
'curr_front_R': curr_front_R,
'curr_back_L': curr_back_L,
'curr_back_R': curr_back_R,
'gap_front_yoke': avg_gap_front,
'gap_back_yoke': avg_gap_back,
'gap_avg': avg_gap,
'roll': roll_angle,
'pitch': pitch_angle,
'force_front': force_front,
'force_back': force_back,
'torque_front': torque_front,
'torque_back': torque_back
}
# --- Recording ---
if self.record_video and self.current_step % self._frame_skip == 0:
_, _, rgb, _, _ = p.getCameraImage(
width=self._video_width,
height=self._video_height,
viewMatrix=self._view_matrix,
projectionMatrix=self._proj_matrix,
physicsClientId=self.client
)
self._frames.append(np.array(rgb, dtype=np.uint8).reshape(
self._video_height, self._video_width, 4)[:, :, :3]) # RGBA → RGB
if self.record_telemetry:
t = self._telemetry
t['time'].append(self.current_step * self.dt)
t['gap_FL'].append(front_left_gap * 1000)
t['gap_FR'].append(front_right_gap * 1000)
t['gap_BL'].append(back_left_gap * 1000)
t['gap_BR'].append(back_right_gap * 1000)
t['gap_front_avg'].append(avg_gap_front * 1000)
t['gap_back_avg'].append(avg_gap_back * 1000)
t['gap_avg'].append(avg_gap * 1000)
t['roll_deg'].append(np.degrees(roll_angle))
t['pitch_deg'].append(np.degrees(pitch_angle))
t['curr_FL'].append(curr_front_L)
t['curr_FR'].append(curr_front_R)
t['curr_BL'].append(curr_back_L)
t['curr_BR'].append(curr_back_R)
t['force_front'].append(force_front)
t['force_back'].append(force_back)
t['torque_front'].append(torque_front)
t['torque_back'].append(torque_back)
if terminated or truncated:
self._finalize_recording()
return obs, reward, terminated, truncated, info
def apply_impulse(self, force_z: float, position: list = None):
"""
Apply a one-time impulse force to the pod.
Args:
force_z: Vertical force in Newtons (positive = upward)
position: Local position [x, y, z] to apply force (default: center of mass)
"""
if position is None:
position = [0, 0, 0]
p.applyExternalForce(
self.podId, -1,
forceObj=[0, 0, force_z],
posObj=position,
flags=p.LINK_FRAME,
physicsClientId=self.client
)
def apply_torque_impulse(self, torque_nm: list):
"""
Apply a one-time impulse torque to the pod (body frame).
Args:
torque_nm: [Tx, Ty, Tz] in N·m (LINK_FRAME: X=roll, Y=pitch, Z=yaw)
"""
p.applyExternalTorque(
self.podId, -1,
torqueObj=torque_nm,
flags=p.LINK_FRAME,
physicsClientId=self.client
)
# The following was generated by AI - see [15]
def _get_obs(self, initial_reset=False):
"""
Returns observation: [gaps(4), velocities(4)]
Uses noisy sensor readings + computed velocities for microcontroller-friendly deployment
"""
pos, orn = p.getBasePositionAndOrientation(self.podId, physicsClientId=self.client)
# Convert quaternion to rotation matrix
rot_matrix = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3)
# Calculate sensor gap heights with noise
sensor_gap_heights = {}
for i, sensor_idx in enumerate(self.sensor_indices):
local_pos = self.collision_local_positions[sensor_idx]
local_vec = np.array(local_pos)
world_offset = rot_matrix @ local_vec
world_pos = np.array(pos) + world_offset
# Top surface of sensor (add half-height = 5mm)
sensor_top_z = world_pos[2] + 0.005
# Gap height: track bottom (Z=0) to sensor top
gap_height = -sensor_top_z
# Add measurement noise
noisy_gap = gap_height + np.random.normal(0, self.sensor_noise_std)
# sensor_gap_heights[self.sensor_labels[i]] = noisy_gap
sensor_gap_heights[self.sensor_labels[i]] = gap_height
# Pack sensor measurements in consistent order
# [center_right, center_left, front, back]
gaps = np.array([
sensor_gap_heights.get('Center_Right', 0.010),
sensor_gap_heights.get('Center_Left', 0.010),
sensor_gap_heights.get('Front', 0.010),
sensor_gap_heights.get('Back', 0.010)
], dtype=np.float32)
# Compute velocities (d_gap/dt)
if initial_reset or (self.prev_sensor_gaps is None):
# First observation - no velocity information yet
velocities = np.zeros(4, dtype=np.float32)
else:
# Compute velocity as finite difference
velocities = (gaps - self.prev_sensor_gaps) / self.dt
# Store for next step
self.prev_sensor_gaps = gaps.copy()
# Normalize observations
gaps_normalized = (gaps - TARGET_GAP) / self.gap_scale
velocities_normalized = velocities / self.velocity_scale
# Concatenate: [normalized_gaps, normalized_velocities]
obs = np.concatenate([gaps_normalized, velocities_normalized])
return obs
# The following was generated by AI - see [16]
def _create_modified_urdf(self):
"""
Create a modified URDF with bolt positions adjusted based on initial gap height.
Bolts are at Z = 0.08585 + gap_mm/1000 (relative to pod origin).
Yokes and sensors remain at Z = 0.08585 (relative to pod origin).
"""
import tempfile
# Calculate bolt Z position
bolt_z = 0.08585 + self.initial_gap_mm / 1000.0
# Read the original URDF template
urdf_template_path = os.path.join(os.path.dirname(__file__), "pod.xml")
with open(urdf_template_path, 'r') as f:
urdf_content = f.read()
# Replace the bolt Z positions (originally at 0.09585)
# There are 4 bolts at different X,Y positions but same Z
urdf_modified = urdf_content.replace(
'xyz="0.285 0.03 0.09585"',
f'xyz="0.285 0.03 {bolt_z:.6f}"'
).replace(
'xyz="0.285 -0.03 0.09585"',
f'xyz="0.285 -0.03 {bolt_z:.6f}"'
).replace(
'xyz="-0.285 0.03 0.09585"',
f'xyz="-0.285 0.03 {bolt_z:.6f}"'
).replace(
'xyz="-0.285 -0.03 0.09585"',
f'xyz="-0.285 -0.03 {bolt_z:.6f}"'
)
# Write to a temporary file
with tempfile.NamedTemporaryFile(mode='w', suffix='.urdf', delete=False) as f:
f.write(urdf_modified)
temp_urdf_path = f.name
return temp_urdf_path
def _finalize_recording(self):
"""Save recorded video and/or telemetry plot to disk."""
if not self._recording_active:
return
self._recording_active = False
timestamp = datetime.now().strftime("%Y%m%d_%H%M%S")
# --- Save video ---
if self.record_video and len(self._frames) > 0:
import imageio.v3 as iio
video_path = os.path.join(self.record_dir, f"sim_{timestamp}.mp4")
fps = int(round(1.0 / (self.dt * self._frame_skip))) # 60fps
iio.imwrite(video_path, self._frames, fps=fps, codec="h264")
print(f"Video saved: {video_path} ({len(self._frames)} frames, {fps}fps)")
self._frames = []
# --- Save telemetry plot ---
if self.record_telemetry and len(self._telemetry.get('time', [])) > 0:
self._save_telemetry_plot(timestamp)
def _save_telemetry_plot(self, timestamp):
"""Generate and save a 4-panel telemetry figure."""
import matplotlib.pyplot as plt
t = {k: np.array(v) for k, v in self._telemetry.items()}
time = t['time']
target_mm = TARGET_GAP * 1000
weight = 9.4 * 9.81 # Pod weight in N
fig, axes = plt.subplots(4, 1, figsize=(12, 14), sharex=True)
fig.suptitle(f'Simulation Telemetry | gap₀={self.initial_gap_mm}mm target={target_mm:.1f}mm',
fontsize=14, fontweight='bold')
# --- Panel 1: Gap heights ---
ax = axes[0]
ax.plot(time, t['gap_FL'], lw=1, alpha=0.6, label='FL')
ax.plot(time, t['gap_FR'], lw=1, alpha=0.6, label='FR')
ax.plot(time, t['gap_BL'], lw=1, alpha=0.6, label='BL')
ax.plot(time, t['gap_BR'], lw=1, alpha=0.6, label='BR')
ax.plot(time, t['gap_avg'], lw=2, color='black', label='Average')
ax.axhline(y=target_mm, color='orange', ls='--', lw=2, label=f'Target ({target_mm:.1f}mm)')
ax.set_ylabel('Gap Height (mm)')
ax.legend(loc='best', ncol=3, fontsize=9)
ax.grid(True, alpha=0.3)
final_err = abs(t['gap_avg'][-1] - target_mm)
ax.text(0.98, 0.02, f'Final error: {final_err:.3f}mm',
transform=ax.transAxes, ha='right', va='bottom', fontsize=10,
bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))
# --- Panel 2: Roll & Pitch ---
ax = axes[1]
ax.plot(time, t['roll_deg'], lw=1.5, label='Roll')
ax.plot(time, t['pitch_deg'], lw=1.5, label='Pitch')
ax.axhline(y=0, color='gray', ls='--', lw=1)
ax.set_ylabel('Angle (degrees)')
ax.legend(loc='best', fontsize=9)
ax.grid(True, alpha=0.3)
# --- Panel 3: Coil currents ---
ax = axes[2]
ax.plot(time, t['curr_FL'], lw=1, alpha=0.8, label='FL')
ax.plot(time, t['curr_FR'], lw=1, alpha=0.8, label='FR')
ax.plot(time, t['curr_BL'], lw=1, alpha=0.8, label='BL')
ax.plot(time, t['curr_BR'], lw=1, alpha=0.8, label='BR')
total = np.abs(t['curr_FL']) + np.abs(t['curr_FR']) + np.abs(t['curr_BL']) + np.abs(t['curr_BR'])
ax.plot(time, total, lw=2, color='black', ls='--', label='Total |I|')
ax.set_ylabel('Current (A)')
ax.legend(loc='best', ncol=3, fontsize=9)
ax.grid(True, alpha=0.3)
# --- Panel 4: Forces ---
ax = axes[3]
ax.plot(time, t['force_front'], lw=1.5, label='Front yoke')
ax.plot(time, t['force_back'], lw=1.5, label='Back yoke')
f_total = t['force_front'] + t['force_back']
ax.plot(time, f_total, lw=2, color='black', label='Total')
ax.axhline(y=weight, color='red', ls='--', lw=1.5, label=f'Weight ({weight:.1f}N)')
ax.set_ylabel('Force (N)')
ax.set_xlabel('Time (s)')
ax.legend(loc='best', ncol=2, fontsize=9)
ax.grid(True, alpha=0.3)
plt.tight_layout()
plot_path = os.path.join(self.record_dir, f"sim_{timestamp}_telemetry.png")
fig.savefig(plot_path, dpi=200, bbox_inches='tight')
plt.close(fig)
print(f"Telemetry plot saved: {plot_path}")
self._telemetry = {}
def close(self):
self._finalize_recording()
try:
p.disconnect(physicsClientId=self.client)
except p.error:
pass # Already disconnected
def render(self):
"""Rendering is handled by PyBullet GUI mode"""
pass

24
lev_sim/mag_lev_coil.py Normal file
View File

@@ -0,0 +1,24 @@
# The following was generated by AI - see [17]
class MagLevCoil:
def __init__(self, r_resistance, l_inductance, source_voltage, maxCurrent):
self.R = r_resistance
self.L = l_inductance
self.current = 0.0
self.Vs = source_voltage
self.Imax = maxCurrent
def update(self, pwm_duty_cycle, dt):
"""
Simulates the coil circuit and force generation.
pwm_duty_cycle: -1.0 to 1.0
"""
# Simple First-order RL circuit approximation
# V_in = Duty * V_source
voltage = pwm_duty_cycle * self.Vs
# di/dt = (V - I*R) / L
di = (voltage - self.current * self.R) / self.L
self.current += di * dt
self.current = min(max(-self.Imax, self.current), self.Imax)
return self.current

BIN
lev_sim/maglev_model.pkl Normal file
View File

Binary file not shown.

121
lev_sim/maglev_predictor.py Normal file
View File

@@ -0,0 +1,121 @@
"""
Magnetic Levitation Force and Torque Predictor
Optimized Inference using Pre-Trained Scikit-Learn Model
This module loads a saved .pkl model (PolynomialFeatures + LinearRegression)
and executes predictions using optimized NumPy vectorization for high-speed simulation.
Usage:
predictor = MaglevPredictor("maglev_model.pkl")
force, torque = predictor.predict(currL=-15, currR=-15, roll=1.0, gap_height=10.0)
"""
import numpy as np
import joblib
import os
# Default path: next to this module so it works regardless of cwd
_DEFAULT_MODEL_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "maglev_model.pkl")
class MaglevPredictor:
def __init__(self, model_path=None):
"""
Initialize predictor by loading the pickle file and extracting
raw matrices for fast inference.
"""
path = model_path if model_path is not None else _DEFAULT_MODEL_PATH
if not os.path.exists(path):
raise FileNotFoundError(f"Model file '{path}' not found. Please train and save the model first.")
print(f"Loading maglev model from {path}...")
data = joblib.load(path)
# 1. Extract Scikit-Learn Objects
poly_transformer = data['poly_features']
linear_model = data['model']
# 2. Extract Raw Matrices for Speed (Bypasses sklearn overhead)
# powers_: Matrix of shape (n_output_features, n_input_features)
# Represents the exponents for each term. e.g. x1^2 * x2^1
self.powers = poly_transformer.powers_.T # Transpose for broadcasting
# coef_: Shape (n_targets, n_output_features) -> (2, n_poly_terms)
self.coef = linear_model.coef_
# intercept_: Shape (n_targets,) -> (2,)
self.intercept = linear_model.intercept_
print(f"Model loaded. Degree: {data['degree']}")
print(f"Force R2: {data['performance']['force_r2']:.4f}")
print(f"Torque R2: {data['performance']['torque_r2']:.4f}")
def predict(self, currL, currR, roll, gap_height):
"""
Fast single-sample prediction using pure NumPy.
Args:
currL, currR: Currents [A]
roll: Roll angle [deg]
gap_height: Gap [mm]
Returns:
(force [N], torque [mN·m])
"""
# 1. Pre-process Input (Must match training order: currL, currR, roll, invGap)
# Clamp gap to avoid division by zero
safe_gap = max(gap_height, 1e-6)
invGap = 1.0 / safe_gap
# Input Vector: shape (4,)
x = np.array([currL, currR, roll, invGap])
# 2. Polynomial Expansion (Vectorized)
# Compute x^p for every term.
# x is (4,), self.powers is (4, n_terms)
# Broadcasting: x[:, None] is (4,1). Result is (4, n_terms).
# Product along axis 0 reduces it to (n_terms,)
# Note: This is equivalent to PolynomialFeatures.transform but 100x faster for single samples
poly_features = np.prod(x[:, None] ** self.powers, axis=0)
# 3. Linear Prediction
# (2, n_terms) dot (n_terms,) -> (2,)
result = np.dot(self.coef, poly_features) + self.intercept
return result[0], result[1]
def predict_batch(self, currL, currR, roll, gap_height):
"""
Fast batch prediction for array inputs.
"""
# 1. Pre-process Inputs
gap_height = np.asarray(gap_height)
invGap = 1.0 / np.maximum(gap_height, 1e-6)
# Stack inputs: shape (N, 4)
X = np.column_stack((currL, currR, roll, invGap))
# 2. Polynomial Expansion
# X is (N, 4). Powers is (4, n_terms).
# We want (N, n_terms).
# Method: X[:, :, None] -> (N, 4, 1)
# Powers[None, :, :] -> (1, 4, n_terms)
# Power: (N, 4, n_terms)
# Prod axis 1: (N, n_terms)
poly_features = np.prod(X[:, :, None] ** self.powers[None, :, :], axis=1)
# 3. Linear Prediction
# (N, n_terms) @ (n_terms, 2) + (2,)
result = np.dot(poly_features, self.coef.T) + self.intercept
return result[:, 0], result[:, 1]
if __name__ == "__main__":
# Test
try:
p = MaglevPredictor()
f, t = p.predict(-15, -15, 1.0, 10.0)
print(f"Force: {f:.3f}, Torque: {t:.3f}")
except FileNotFoundError as e:
print(e)

337
lev_sim/optuna_pid_tune.py Normal file
View File

@@ -0,0 +1,337 @@
"""
Optuna (TPE / Bayesian-style) optimization for the three-PID maglev controller.
Tunes all nine gains (height, roll, pitch × Kp, Ki, Kd) jointly so coupling is respected.
Noisy optimization: set disturbance_force_std > 0 so the env applies random force/torque
each step (system changes slightly each run). To keep Bayesian optimization effective,
use n_objective_repeats > 1: each candidate is evaluated multiple times and the mean cost
is returned, reducing variance so TPE can compare trials reliably.
Run from the command line or import and call run_optimization() from a notebook.
"""
import json
import os
import sys
import numpy as np
import optuna
from optuna.samplers import TPESampler
from lev_pod_env import TARGET_GAP
from pid_controller import DEFAULT_GAINS
from pid_simulation import run_pid_simulation, build_feedforward_lut
# Save pid_best_params.json next to this script so notebook and CLI find it regardless of cwd
_SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
# Default optimization config: long enough to see late instability (~8s+), not so long that optimizer goes ultra-conservative
DEFAULT_MAX_STEPS = 1500
DEFAULT_INITIAL_GAPS_MM = [8.0, 15.0] # Two conditions for robustness (bracket 11.86mm target)
DEFAULT_N_TRIALS = 200
DEFAULT_TIMEOUT_S = 3600
TARGET_GAP_MM = TARGET_GAP * 1000
def _cost_from_data(
data: dict,
target_gap_mm: float,
penalty_early: float = 500.0,
weight_late_osc: float = 3.0,
weight_steady_state: float = 2.0,
) -> float:
"""
Single scalar cost from one run. Lower is better.
- ITAE for gap error and |roll|/|pitch| (tracking over time).
- Late-oscillation penalty: std(roll) and std(pitch) over the *last 50%* of the run,
plus max |roll|/|pitch| in that window, so gains that go unstable after ~half the run are penalized.
- Steady-state term: mean absolute gap error and mean |roll|/|pitch| over the *last 20%*,
so we reward settling at target with small angles.
- Penalty if episode ended early (crash/instability), regardless of run length.
- Small penalty on mean total current (efficiency).
"""
t = np.asarray(data["time"])
dt = np.diff(t, prepend=0.0)
gap_err_mm = np.abs(np.asarray(data["gap_avg"]) - target_gap_mm)
itae_gap = np.sum(t * gap_err_mm * dt)
roll_deg = np.asarray(data["roll_deg"])
pitch_deg = np.asarray(data["pitch_deg"])
roll_abs = np.abs(roll_deg)
pitch_abs = np.abs(pitch_deg)
itae_roll = np.sum(t * roll_abs * dt)
itae_pitch = np.sum(t * pitch_abs * dt)
n = len(t)
early_penalty = penalty_early if data.get("terminated_early", False) else 0.0
# Late half: penalize oscillation (std) and large angles (max) so "good for 6s then violent roll" is expensive
half = max(1, n // 2)
roll_last = roll_deg[-half:]
pitch_last = pitch_deg[-half:]
late_osc_roll = np.std(roll_last) + np.max(np.abs(roll_last))
late_osc_pitch = np.std(pitch_last) + np.max(np.abs(pitch_last))
late_osc_penalty = weight_late_osc * (late_osc_roll + late_osc_pitch)
# Last 20%: steady-state error — want small gap error and small roll/pitch at end
tail = max(1, n // 5)
ss_gap = np.mean(np.abs(np.asarray(data["gap_avg"])[-tail:] - target_gap_mm))
ss_roll = np.mean(roll_abs[-tail:])
ss_pitch = np.mean(pitch_abs[-tail:])
steady_state_penalty = weight_steady_state * (ss_gap + ss_roll + ss_pitch)
mean_current = np.mean(data["current_total"])
current_penalty = 0.01 * mean_current
return (
itae_gap
+ 2.0 * (itae_roll + itae_pitch)
+ early_penalty
+ late_osc_penalty
+ steady_state_penalty
+ current_penalty
)
def objective(
trial: optuna.Trial,
initial_gaps_mm: list,
max_steps: int,
use_feedforward: bool,
penalty_early: float,
disturbance_force_std: float = 0.0,
n_objective_repeats: int = 1,
) -> float:
"""
Optuna objective: suggest 9 gains, run simulation(s), return cost.
With disturbance_force_std > 0, the env applies random force/torque disturbances each step,
so the same gains yield different costs across runs. For Bayesian optimization under noise,
set n_objective_repeats > 1 to evaluate each candidate multiple times and return the mean cost,
reducing variance so TPE can compare trials reliably.
"""
# All three loops tuned together (recommended: they interact)
height_kp = trial.suggest_float("height_kp", 5.0, 200.0, log=True)
height_ki = trial.suggest_float("height_ki", 0.5, 50.0, log=True)
height_kd = trial.suggest_float("height_kd", 1.0, 50.0, log=True)
roll_kp = trial.suggest_float("roll_kp", 0.1, 20.0, log=True)
roll_ki = trial.suggest_float("roll_ki", 0.01, 5.0, log=True)
roll_kd = trial.suggest_float("roll_kd", 0.01, 5.0, log=True)
pitch_kp = trial.suggest_float("pitch_kp", 0.1, 20.0, log=True)
pitch_ki = trial.suggest_float("pitch_ki", 0.01, 5.0, log=True)
pitch_kd = trial.suggest_float("pitch_kd", 0.01, 5.0, log=True)
gains = {
"height_kp": height_kp, "height_ki": height_ki, "height_kd": height_kd,
"roll_kp": roll_kp, "roll_ki": roll_ki, "roll_kd": roll_kd,
"pitch_kp": pitch_kp, "pitch_ki": pitch_ki, "pitch_kd": pitch_kd,
}
cost_sum = 0.0
n_evals = 0
for _ in range(n_objective_repeats):
for initial_gap in initial_gaps_mm:
data = run_pid_simulation(
initial_gap_mm=initial_gap,
max_steps=max_steps,
use_gui=False,
disturbance_force_std=disturbance_force_std,
use_feedforward=use_feedforward,
record_video=False,
record_telemetry=False,
gains=gains,
verbose=False,
)
n = len(data["time"])
data["terminated_early"] = n < max_steps - 10
cost_sum += _cost_from_data(data, TARGET_GAP_MM, penalty_early=penalty_early)
n_evals += 1
return cost_sum / n_evals
def run_optimization(
n_trials: int = DEFAULT_N_TRIALS,
timeout: int = DEFAULT_TIMEOUT_S,
initial_gaps_mm: list = None,
max_steps: int = DEFAULT_MAX_STEPS,
use_feedforward: bool = True,
penalty_early: float = 500.0,
disturbance_force_std: float = 0.0,
n_objective_repeats: int = 1,
out_dir: str = None,
study_name: str = "pid_maglev",
) -> optuna.Study:
"""
Run Optuna study (TPE sampler) and save best params to JSON.
disturbance_force_std: passed to env so each step gets random force/torque noise (N).
n_objective_repeats: when > 1, each trial is evaluated this many times (different noise)
and the mean cost is returned, so Bayesian optimization sees a less noisy objective.
Returns:
study: Optuna Study (study.best_params, study.best_value).
"""
if initial_gaps_mm is None:
initial_gaps_mm = DEFAULT_INITIAL_GAPS_MM
if out_dir is None:
out_dir = _SCRIPT_DIR
# Build feedforward LUT once so first trial doesn't do it inside run
build_feedforward_lut()
sampler = TPESampler(
n_startup_trials=min(20, n_trials // 4),
n_ei_candidates=24,
seed=42,
)
study = optuna.create_study(
direction="minimize",
study_name=study_name,
sampler=sampler,
)
study.optimize(
lambda t: objective(
t,
initial_gaps_mm=initial_gaps_mm,
max_steps=max_steps,
use_feedforward=use_feedforward,
penalty_early=penalty_early,
disturbance_force_std=disturbance_force_std,
n_objective_repeats=n_objective_repeats,
),
n_trials=n_trials,
timeout=timeout,
show_progress_bar=True,
)
out_path = os.path.join(out_dir, "pid_best_params.json")
with open(out_path, "w") as f:
json.dump(study.best_params, f, indent=2)
print(f"Best cost: {study.best_value:.4f}")
print(f"Best params saved to {out_path}")
return study
def run_staged_optimization(
stage_steps: list = None,
n_trials_per_stage: int = 50,
timeout_per_stage: int = None,
initial_gaps_mm: list = None,
use_feedforward: bool = True,
penalty_early: float = 500.0,
disturbance_force_std: float = 0.0,
n_objective_repeats: int = 1,
out_dir: str = None,
) -> list:
"""
Run optimization in stages with increasing max_steps, warm-starting each stage from the previous best.
disturbance_force_std: passed to env for stochastic force/torque noise (N).
n_objective_repeats: mean over this many evaluations per trial for a less noisy objective.
Example: stage_steps=[1500, 3000, 6000]
- Stage 1: optimize with 1500 steps (finds good gap/initial roll), save best.
- Stage 2: optimize with 3000 steps; first trial is the 1500-step best (evaluated at 3000 steps), then TPE suggests improvements.
- Stage 3: same with 6000 steps starting from stage 2's best.
This keeps good lift-off and gap control from the short-horizon run while refining for late-horizon roll stability.
"""
if stage_steps is None:
stage_steps = [1500, 3000, 6000]
if initial_gaps_mm is None:
initial_gaps_mm = DEFAULT_INITIAL_GAPS_MM
if out_dir is None:
out_dir = _SCRIPT_DIR
if timeout_per_stage is None:
timeout_per_stage = DEFAULT_TIMEOUT_S
build_feedforward_lut()
best_params_path = os.path.join(out_dir, "pid_best_params.json")
studies = []
for stage_idx, max_steps in enumerate(stage_steps):
print(f"\n--- Stage {stage_idx + 1}/{len(stage_steps)}: max_steps={max_steps} ---")
study = optuna.create_study(
direction="minimize",
study_name=f"pid_maglev_stage_{max_steps}",
sampler=TPESampler(
n_startup_trials=min(20, n_trials_per_stage // 4),
n_ei_candidates=24,
seed=42 + stage_idx,
),
)
# Warm start: enqueue previous stage's best so we refine from it (stage 0 has no previous)
if stage_idx > 0 and os.path.isfile(best_params_path):
with open(best_params_path) as f:
prev_best = json.load(f)
study.enqueue_trial(prev_best)
print(f"Enqueued previous best params (from stage {stage_steps[stage_idx - 1]} steps) as first trial.")
study.optimize(
lambda t: objective(
t,
initial_gaps_mm=initial_gaps_mm,
max_steps=max_steps,
use_feedforward=use_feedforward,
penalty_early=penalty_early,
disturbance_force_std=disturbance_force_std,
n_objective_repeats=n_objective_repeats,
),
n_trials=n_trials_per_stage,
timeout=timeout_per_stage,
show_progress_bar=True,
)
with open(best_params_path, "w") as f:
json.dump(study.best_params, f, indent=2)
stage_path = os.path.join(out_dir, f"pid_best_params_{max_steps}.json")
with open(stage_path, "w") as f:
json.dump(study.best_params, f, indent=2)
print(f"Stage best cost: {study.best_value:.4f} -> saved to {best_params_path} and {stage_path}")
studies.append(study)
print(f"\nStaged optimization done. Final best params in {best_params_path}")
return studies
if __name__ == "__main__":
import argparse
parser = argparse.ArgumentParser(description="Optuna PID tuning for maglev three-loop controller")
parser.add_argument("--n_trials", type=int, default=DEFAULT_N_TRIALS)
parser.add_argument("--timeout", type=int, default=DEFAULT_TIMEOUT_S)
parser.add_argument("--max_steps", type=int, default=DEFAULT_MAX_STEPS)
parser.add_argument("--gaps", type=float, nargs="+", default=DEFAULT_INITIAL_GAPS_MM)
parser.add_argument("--out_dir", type=str, default=_SCRIPT_DIR, help="Directory for pid_best_params.json (default: same as script)")
parser.add_argument("--no_feedforward", action="store_true")
parser.add_argument("--staged", action="store_true", help="Staged optimization: 1500 -> 3000 -> 6000 steps, each stage warm-starts from previous best")
parser.add_argument("--stage_steps", type=int, nargs="+", default=[1500, 3000, 6000], help="Steps per stage when using --staged (default: 1500 3000 6000)")
parser.add_argument("--n_trials_per_stage", type=int, default=DEFAULT_N_TRIALS, help="Trials per stage when using --staged")
parser.add_argument("--disturbance_force_std", type=float, default=0.0, help="Env disturbance force std (N); roll/pitch torque scale with this. Use >0 for noisy optimization.")
parser.add_argument("--n_objective_repeats", type=int, default=1, help="Evaluate each trial this many times and report mean cost (reduces noise for Bayesian optimization)")
args = parser.parse_args()
if args.staged:
run_staged_optimization(
stage_steps=args.stage_steps,
n_trials_per_stage=args.n_trials_per_stage,
timeout_per_stage=args.timeout,
initial_gaps_mm=args.gaps,
use_feedforward=not args.no_feedforward,
disturbance_force_std=args.disturbance_force_std,
n_objective_repeats=args.n_objective_repeats,
out_dir=args.out_dir,
)
else:
run_optimization(
n_trials=args.n_trials,
timeout=args.timeout,
initial_gaps_mm=args.gaps,
max_steps=args.max_steps,
use_feedforward=not args.no_feedforward,
disturbance_force_std=args.disturbance_force_std,
n_objective_repeats=args.n_objective_repeats,
out_dir=args.out_dir,
)

11
lev_sim/pid_best_params.json Normal file
View File

@@ -0,0 +1,11 @@
{
"height_kp": 100.962395560374814,
"height_ki": 0.0,
"height_kd": 8.1738092637166575,
"roll_kp": 0.600856607986966,
"roll_ki": 0.0,
"roll_kd": -0.1,
"pitch_kp": 50.0114708799258271,
"pitch_ki": 0,
"pitch_kd": 1.8990306608320433
}

11
lev_sim/pid_best_params_1500.json Normal file
View File

@@ -0,0 +1,11 @@
{
"height_kp": 61.34660453658844,
"height_ki": 5.337339965349835,
"height_kd": 12.13071554123404,
"roll_kp": 5.838881924776812,
"roll_ki": 0.11040111644948386,
"roll_kd": 0.0401383180893775,
"pitch_kp": 0.10114651080341679,
"pitch_ki": 0.06948994902747452,
"pitch_kd": 0.16948986671689478
}

11
lev_sim/pid_best_params_3000.json Normal file
View File

@@ -0,0 +1,11 @@
{
"height_kp": 6.79952358593656,
"height_ki": 2.7199339229214856,
"height_kd": 12.166576111298163,
"roll_kp": 1.7073146141313746,
"roll_ki": 0.026221209129363342,
"roll_kd": 0.018353603438859525,
"pitch_kp": 1.2333241666054757,
"pitch_ki": 0.03544610305322942,
"pitch_kd": 0.3711162924888727
}

11
lev_sim/pid_best_params_6000.json Normal file
View File

@@ -0,0 +1,11 @@
{
"height_kp": 5.962395560374814,
"height_ki": 0.5381137743695537,
"height_kd": 1.1738092637166575,
"roll_kp": 0.48249575996443006,
"roll_ki": 0.016923890033141546,
"roll_kd": 0.013609429509460135,
"pitch_kp": 0.10114708799258271,
"pitch_ki": 1.906050976563914,
"pitch_kd": 1.8990306608320433
}

92
lev_sim/pid_controller.py Normal file
View File

@@ -0,0 +1,92 @@
"""
PID controller and default gains for the maglev three-loop (height, roll, pitch) control.
Used by lev_PID.ipynb and optuna_pid_tune.py.
"""
import numpy as np
class PIDController:
"""Simple PID controller with anti-windup."""
def __init__(
self,
kp: float,
ki: float,
kd: float,
output_min: float = -1.0,
output_max: float = 1.0,
integral_limit: float = None,
):
self.kp = kp
self.ki = ki
self.kd = kd
self.output_min = output_min
self.output_max = output_max
self.integral_limit = (
integral_limit if integral_limit is not None else abs(output_max) * 2
)
self.integral = 0.0
self.prev_error = 0.0
self.first_update = True
def reset(self):
"""Reset controller state."""
self.integral = 0.0
self.prev_error = 0.0
self.first_update = True
def update(self, error: float, dt: float) -> float:
"""Compute PID output.
Args:
error: Current error (setpoint - measurement)
dt: Time step in seconds
Returns:
Control output (clamped to output limits)
"""
p_term = self.kp * error
self.integral += error * dt
self.integral = np.clip(
self.integral, -self.integral_limit, self.integral_limit
)
i_term = self.ki * self.integral
if self.first_update:
d_term = 0.0
self.first_update = False
else:
d_term = self.kd * (error - self.prev_error) / dt
self.prev_error = error
output = p_term + i_term + d_term
return np.clip(output, self.output_min, self.output_max)
# Default gains: height (main), roll, pitch.
# Optimizer and notebook can override via gains dict passed to run_pid_simulation.
DEFAULT_GAINS = {
"height_kp": 50.0,
"height_ki": 5.0,
"height_kd": 10.0,
"roll_kp": 2.0,
"roll_ki": 0.5,
"roll_kd": 0.5,
"pitch_kp": 2.0,
"pitch_ki": 0.5,
"pitch_kd": 0.5,
}
# Backward-compat names for notebook (optional)
HEIGHT_KP = DEFAULT_GAINS["height_kp"]
HEIGHT_KI = DEFAULT_GAINS["height_ki"]
HEIGHT_KD = DEFAULT_GAINS["height_kd"]
ROLL_KP = DEFAULT_GAINS["roll_kp"]
ROLL_KI = DEFAULT_GAINS["roll_ki"]
ROLL_KD = DEFAULT_GAINS["roll_kd"]
PITCH_KP = DEFAULT_GAINS["pitch_kp"]
PITCH_KI = DEFAULT_GAINS["pitch_ki"]
PITCH_KD = DEFAULT_GAINS["pitch_kd"]

216
lev_sim/pid_simulation.py Normal file
View File

@@ -0,0 +1,216 @@
"""
Feedforward + PID simulation runner for the maglev pod.
Uses LevPodEnv, MaglevPredictor (feedforward), and PIDController with configurable gains.
"""
import numpy as np
from lev_pod_env import LevPodEnv, TARGET_GAP
from maglev_predictor import MaglevPredictor
from pid_controller import PIDController, DEFAULT_GAINS
# Feedforward LUT (built on first use)
_FF_GAP_LUT = None
_FF_PWM_LUT = None
def build_feedforward_lut(
pod_mass: float = 9.4,
coil_r: float = 1.1,
v_supply: float = 12.0,
gap_min: float = 3.0,
gap_max: float = 35.0,
n_points: int = 500,
):
"""Build gap [mm] -> equilibrium PWM lookup table. Returns (gap_lut, pwm_lut) for np.interp."""
global _FF_GAP_LUT, _FF_PWM_LUT
target_per_yoke = pod_mass * 9.81 / 2.0
predictor = MaglevPredictor()
def _find_eq_current(gap_mm):
a, b = -10.2, 10.2
fa, _ = predictor.predict(a, a, 0.0, gap_mm)
for _ in range(80):
mid = (a + b) / 2.0
fm, _ = predictor.predict(mid, mid, 0.0, gap_mm)
if (fa > target_per_yoke) == (fm > target_per_yoke):
a, fa = mid, fm
else:
b = mid
return (a + b) / 2.0
_FF_GAP_LUT = np.linspace(gap_min, gap_max, n_points)
curr_lut = np.array([_find_eq_current(g) for g in _FF_GAP_LUT])
_FF_PWM_LUT = np.clip(curr_lut * coil_r / v_supply, -1.0, 1.0)
return _FF_GAP_LUT, _FF_PWM_LUT
def feedforward_pwm(gap_mm: float) -> float:
"""Interpolate equilibrium PWM for gap height [mm]. Builds LUT on first call."""
global _FF_GAP_LUT, _FF_PWM_LUT
if _FF_GAP_LUT is None:
build_feedforward_lut()
return float(np.interp(gap_mm, _FF_GAP_LUT, _FF_PWM_LUT))
def run_pid_simulation(
initial_gap_mm: float = 14.0,
max_steps: int = 2000,
use_gui: bool = False,
disturbance_step: int = None,
disturbance_force: float = 0.0,
disturbance_force_std: float = 0.0,
use_feedforward: bool = True,
record_video: bool = False,
record_telemetry: bool = False,
record_dir: str = "recordings",
gains: dict = None,
verbose: bool = True,
) -> dict:
"""
Run one feedforward + PID simulation. Gains can be passed for tuning (e.g. Optuna).
Args:
initial_gap_mm: Starting gap height (mm).
max_steps: Max simulation steps.
use_gui: PyBullet GUI (avoid in notebooks).
disturbance_step: Step for impulse (None = none).
disturbance_force: Impulse force (N).
disturbance_force_std: Noise std (N).
use_feedforward: Use MaglevPredictor feedforward.
record_video: Save MP4.
record_telemetry: Save telemetry PNG.
record_dir: Output directory.
gains: Dict with keys height_kp, height_ki, height_kd, roll_kp, roll_ki, roll_kd,
pitch_kp, pitch_ki, pitch_kd. If None, uses DEFAULT_GAINS.
verbose: Print progress.
Returns:
data: dict of arrays (time, gap_avg, roll_deg, pitch_deg, currents, pwms, ...).
"""
g = gains if gains is not None else DEFAULT_GAINS
env = LevPodEnv(
use_gui=use_gui,
initial_gap_mm=initial_gap_mm,
max_steps=max_steps,
disturbance_force_std=disturbance_force_std,
record_video=record_video,
record_telemetry=record_telemetry,
record_dir=record_dir,
)
dt = env.dt
height_pid = PIDController(
g["height_kp"], g["height_ki"], g["height_kd"],
output_min=-1.0, output_max=1.0,
)
roll_pid = PIDController(
g["roll_kp"], g["roll_ki"], g["roll_kd"],
output_min=-0.5, output_max=0.5,
)
pitch_pid = PIDController(
g["pitch_kp"], g["pitch_ki"], g["pitch_kd"],
output_min=-0.5, output_max=0.5,
)
data = {
"time": [], "gap_front": [], "gap_back": [], "gap_avg": [],
"roll_deg": [], "pitch_deg": [],
"current_FL": [], "current_FR": [], "current_BL": [], "current_BR": [],
"current_total": [], "pwm_FL": [], "pwm_FR": [], "pwm_BL": [], "pwm_BR": [],
"ff_pwm": [],
}
obs, _ = env.reset()
target_gap = TARGET_GAP
y_distance = 0.1016
x_distance = 0.2518
if verbose:
print(f"Starting simulation: initial_gap={initial_gap_mm}mm, target={target_gap*1000:.2f}mm")
if disturbance_step is not None:
print(f" Impulse disturbance: {disturbance_force}N at step {disturbance_step}")
if disturbance_force_std > 0:
print(f" Stochastic noise: std={disturbance_force_std}N")
print(f" Feedforward: {'enabled' if use_feedforward else 'disabled'}")
if record_video or record_telemetry:
print(f" Recording: video={record_video}, telemetry={record_telemetry} → {record_dir}/")
for step in range(max_steps):
t = step * dt
gaps_normalized = obs[:4]
gaps = gaps_normalized * env.gap_scale + TARGET_GAP
gap_front = gaps[2]
gap_back = gaps[3]
gap_left = gaps[1]
gap_right = gaps[0]
gap_avg = (gap_front + gap_back + gap_left + gap_right) / 4
roll_angle = np.arcsin(np.clip((gap_left - gap_right) / y_distance, -1, 1))
pitch_angle = np.arcsin(np.clip((gap_back - gap_front) / x_distance, -1, 1))
ff_base = feedforward_pwm(gap_avg * 1000) if use_feedforward else 0.0
height_error = target_gap - gap_avg
roll_error = -roll_angle
pitch_error = -pitch_angle
height_correction = height_pid.update(height_error, dt)
roll_correction = roll_pid.update(roll_error, dt)
pitch_correction = pitch_pid.update(pitch_error, dt)
pwm_FL = np.clip(ff_base + height_correction - roll_correction - pitch_correction, -1, 1)
pwm_FR = np.clip(ff_base + height_correction + roll_correction - pitch_correction, -1, 1)
pwm_BL = np.clip(ff_base + height_correction - roll_correction + pitch_correction, -1, 1)
pwm_BR = np.clip(ff_base + height_correction + roll_correction + pitch_correction, -1, 1)
action = np.array([pwm_FL, pwm_FR, pwm_BL, pwm_BR], dtype=np.float32)
if disturbance_step is not None and step == disturbance_step:
env.apply_impulse(disturbance_force)
# Coupled torque: random direction, magnitude = 0.5 * |impulse force| (N·m)
torque_mag = 0.5 * abs(disturbance_force)
direction = np.random.randn(3)
direction = direction / (np.linalg.norm(direction) + 1e-12)
torque_nm = (torque_mag * direction).tolist()
env.apply_torque_impulse(torque_nm)
if verbose:
print(f" Applied {disturbance_force}N impulse and {torque_mag:.2f} N·m torque at step {step}")
obs, _, terminated, truncated, info = env.step(action)
data["time"].append(t)
data["gap_front"].append(info["gap_front_yoke"] * 1000)
data["gap_back"].append(info["gap_back_yoke"] * 1000)
data["gap_avg"].append(info["gap_avg"] * 1000)
data["roll_deg"].append(np.degrees(info["roll"]))
data["pitch_deg"].append(np.degrees(info["pitch"]))
data["current_FL"].append(info["curr_front_L"])
data["current_FR"].append(info["curr_front_R"])
data["current_BL"].append(info["curr_back_L"])
data["current_BR"].append(info["curr_back_R"])
data["current_total"].append(
abs(info["curr_front_L"]) + abs(info["curr_front_R"])
+ abs(info["curr_back_L"]) + abs(info["curr_back_R"])
)
data["ff_pwm"].append(ff_base)
data["pwm_FL"].append(pwm_FL)
data["pwm_FR"].append(pwm_FR)
data["pwm_BL"].append(pwm_BL)
data["pwm_BR"].append(pwm_BR)
if terminated or truncated:
if verbose:
print(f" Simulation ended at step {step} (terminated={terminated})")
break
env.close()
for key in data:
data[key] = np.array(data[key])
if verbose:
print(f"Simulation complete: {len(data['time'])} steps, {data['time'][-1]:.2f}s")
print(f" Final gap: {data['gap_avg'][-1]:.2f}mm (target: {target_gap*1000:.2f}mm)")
print(f" Final roll: {data['roll_deg'][-1]:.3f}°, pitch: {data['pitch_deg'][-1]:.3f}°")
return data

86
lev_sim/pod.xml Normal file
View File

@@ -0,0 +1,86 @@
<?xml version="1.0"?>
<robot name="lev_pod">
<link name="base_link">
<inertial>
<mass value="9.4"/>
<inertia ixx="0.03162537654" ixy="0.0" ixz="0.0" iyy="0.21929017831" iyz="0.0" izz="0.21430205089"/>
</inertial>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.009525"/>
<geometry> <box size="0.6096 0.0862 0.01905"/> </geometry>
</collision>
Bolts
<collision>
<origin rpy="0 0 0" xyz="0.285 0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.285 -0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.285 0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.285 -0.03 0.09085"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<!-- BottomStops
<collision>
<origin rpy="0 0 0" xyz="0.285 0.03 0.14500"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.285 -0.03 0.14500"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.285 0.03 0.14500"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.285 -0.03 0.14500"/>
<geometry><box size="0.01 0.01 0.01"/></geometry>
</collision> -->
<!-- Yoke Tops -->
<collision>
<origin rpy="0 0 0" xyz="0.1259 0.0508 0.08585"/>
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.1259 -0.0508 0.08585"/>
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.1259 0.0508 0.08585"/>
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.1259 -0.0508 0.08585"/>
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
</collision>
<!-- Sensor Tops -->
<collision>
<origin rpy="0 0 0" xyz="0 0.0508 0.08585"/>
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0 -0.0508 0.08585"/>
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="0.2366 0 0.08585"/>
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
</collision>
<collision>
<origin rpy="0 0 0" xyz="-0.2366 0 0.08585"/>
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
</collision>
</link>
</robot>

430
lev_sim/polynomial_equations.txt Normal file
View File

@@ -0,0 +1,430 @@
Magnetic Levitation Force & Torque — Explicit Polynomial Equations
Polynomial Degree: 6
Variables:
currL [A] = Left coil current
currR [A] = Right coil current
rollDeg [deg] = Roll angle
invGap = 1 / GapHeight [1/mm]
================================================================================
FORCE [N] =
-1.3837763236e+01
+ (+3.4900958018e-01) * currL [A]
+ (+3.4444782070e-01) * currR [A]
+ (+1.3948921341e-07) * rollDeg [deg]
+ (+6.3404397040e+02) * invGap
+ (+2.0808153074e-02) * currL [A]^2
+ (-8.2940557511e-04) * currL [A] * currR [A]
+ (+2.2629904125e-02) * currL [A] * rollDeg [deg]
+ (-1.4941727622e+01) * currL [A] * invGap
+ (+2.1137560003e-02) * currR [A]^2
+ (-2.2629886279e-02) * currR [A] * rollDeg [deg]
+ (-1.4834895226e+01) * currR [A] * invGap
+ (-7.3648244412e-01) * rollDeg [deg]^2
+ (-7.6740357314e-06) * rollDeg [deg] * invGap
+ (+1.3767217831e+03) * invGap^2
+ (-1.1531758728e-04) * currL [A]^3
+ (+7.2971576377e-05) * currL [A]^2 * currR [A]
+ (+5.4868393021e-06) * currL [A]^2 * rollDeg [deg]
+ (-1.0521912466e+00) * currL [A]^2 * invGap
+ (+5.2348894922e-05) * currL [A] * currR [A]^2
+ (+2.1794350221e-09) * currL [A] * currR [A] * rollDeg [deg]
+ (+3.2678067078e-02) * currL [A] * currR [A] * invGap
+ (-6.3017520966e-03) * currL [A] * rollDeg [deg]^2
+ (-2.6462505434e-01) * currL [A] * rollDeg [deg] * invGap
+ (-2.9510804409e+01) * currL [A] * invGap^2
+ (-7.2152022082e-05) * currR [A]^3
+ (-5.4823932308e-06) * currR [A]^2 * rollDeg [deg]
+ (-1.0708822638e+00) * currR [A]^2 * invGap
+ (-6.3914882711e-03) * currR [A] * rollDeg [deg]^2
+ (+2.6462554272e-01) * currR [A] * rollDeg [deg] * invGap
+ (-3.0234998506e+01) * currR [A] * invGap^2
+ (+4.5881294085e-09) * rollDeg [deg]^3
+ (+2.5797308313e+01) * rollDeg [deg]^2 * invGap
+ (+4.6682794236e-05) * rollDeg [deg] * invGap^2
+ (-5.3401486713e+03) * invGap^3
+ (+1.3320765542e-06) * currL [A]^4
+ (+3.2152411427e-06) * currL [A]^3 * currR [A]
+ (+1.2629467079e-05) * currL [A]^3 * rollDeg [deg]
+ (+1.7892402808e-03) * currL [A]^3 * invGap
+ (+3.0619624267e-06) * currL [A]^2 * currR [A]^2
+ (-1.5568499556e-07) * currL [A]^2 * currR [A] * rollDeg [deg]
+ (-1.0714912904e-03) * currL [A]^2 * currR [A] * invGap
+ (+8.7713928261e-05) * currL [A]^2 * rollDeg [deg]^2
+ (+4.5854056157e-03) * currL [A]^2 * rollDeg [deg] * invGap
+ (+2.0746027602e+01) * currL [A]^2 * invGap^2
+ (+3.8655549044e-06) * currL [A] * currR [A]^3
+ (+1.5545726439e-07) * currL [A] * currR [A]^2 * rollDeg [deg]
+ (-7.0411774478e-04) * currL [A] * currR [A]^2 * invGap
+ (-3.8261231547e-05) * currL [A] * currR [A] * rollDeg [deg]^2
+ (-9.5017239129e-09) * currL [A] * currR [A] * rollDeg [deg] * invGap
+ (+7.0646804762e-01) * currL [A] * currR [A] * invGap^2
+ (-2.5574383917e-03) * currL [A] * rollDeg [deg]^3
+ (-3.3355499360e-02) * currL [A] * rollDeg [deg]^2 * invGap
+ (-2.1337761184e+01) * currL [A] * rollDeg [deg] * invGap^2
+ (+1.2343471808e+02) * currL [A] * invGap^3
+ (+7.7592074199e-07) * currR [A]^4
+ (-1.2629426979e-05) * currR [A]^3 * rollDeg [deg]
+ (+8.6147973530e-04) * currR [A]^3 * invGap
+ (+8.0130692138e-05) * currR [A]^2 * rollDeg [deg]^2
+ (-4.5854241649e-03) * currR [A]^2 * rollDeg [deg] * invGap
+ (+2.1139446060e+01) * currR [A]^2 * invGap^2
+ (+2.5574380197e-03) * currR [A] * rollDeg [deg]^3
+ (-2.9857453053e-02) * currR [A] * rollDeg [deg]^2 * invGap
+ (+2.1337762097e+01) * currR [A] * rollDeg [deg] * invGap^2
+ (+1.2514911864e+02) * currR [A] * invGap^3
+ (+2.9443521553e-02) * rollDeg [deg]^4
+ (-7.9265644452e-09) * rollDeg [deg]^3 * invGap
+ (-2.3563990269e+02) * rollDeg [deg]^2 * invGap^2
+ (+1.6353835994e-04) * rollDeg [deg] * invGap^3
+ (-2.2354105572e+03) * invGap^4
+ (+3.1117360777e-07) * currL [A]^5
+ (-1.8750259301e-07) * currL [A]^4 * currR [A]
+ (-8.5198564648e-08) * currL [A]^4 * rollDeg [deg]
+ (-6.3869937977e-05) * currL [A]^4 * invGap
+ (-1.4911217505e-07) * currL [A]^3 * currR [A]^2
+ (+1.7061909929e-07) * currL [A]^3 * currR [A] * rollDeg [deg]
+ (-7.4262537249e-06) * currL [A]^3 * currR [A] * invGap
+ (+7.2402232831e-07) * currL [A]^3 * rollDeg [deg]^2
+ (+2.1841125430e-05) * currL [A]^3 * rollDeg [deg] * invGap
+ (-3.4357925518e-03) * currL [A]^3 * invGap^2
+ (-1.6416880300e-07) * currL [A]^2 * currR [A]^3
+ (+3.6385798723e-05) * currL [A]^2 * currR [A]^2 * invGap
+ (+5.2924336558e-07) * currL [A]^2 * currR [A] * rollDeg [deg]^2
+ (+8.5467922645e-05) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap
+ (+4.5573594510e-03) * currL [A]^2 * currR [A] * invGap^2
+ (-2.9680843072e-05) * currL [A]^2 * rollDeg [deg]^3
+ (-3.7951808035e-03) * currL [A]^2 * rollDeg [deg]^2 * invGap
+ (+4.7717250306e-02) * currL [A]^2 * rollDeg [deg] * invGap^2
+ (-1.4608679078e+02) * currL [A]^2 * invGap^3
+ (-1.2370955460e-07) * currL [A] * currR [A]^4
+ (-1.7060417434e-07) * currL [A] * currR [A]^3 * rollDeg [deg]
+ (-1.3936700967e-05) * currL [A] * currR [A]^3 * invGap
+ (+4.5908154078e-07) * currL [A] * currR [A]^2 * rollDeg [deg]^2
+ (-8.5469212391e-05) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap
+ (+1.6991917750e-03) * currL [A] * currR [A]^2 * invGap^2
+ (-3.7373312737e-04) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap
+ (+1.5086926031e-07) * currL [A] * currR [A] * rollDeg [deg] * invGap^2
+ (-7.5987537316e+00) * currL [A] * currR [A] * invGap^3
+ (+6.6508849418e-04) * currL [A] * rollDeg [deg]^4
+ (+9.6378055653e-02) * currL [A] * rollDeg [deg]^3 * invGap
+ (+2.4435308561e+00) * currL [A] * rollDeg [deg]^2 * invGap^2
+ (+1.2664933542e+02) * currL [A] * rollDeg [deg] * invGap^3
+ (+5.1887450366e+01) * currL [A] * invGap^4
+ (+2.3180087538e-07) * currR [A]^5
+ (+8.5217195078e-08) * currR [A]^4 * rollDeg [deg]
+ (-6.9233585420e-05) * currR [A]^4 * invGap
+ (+7.5608402383e-07) * currR [A]^3 * rollDeg [deg]^2
+ (-2.1841873013e-05) * currR [A]^3 * rollDeg [deg] * invGap
+ (+3.1131797033e-03) * currR [A]^3 * invGap^2
+ (+2.9680840171e-05) * currR [A]^2 * rollDeg [deg]^3
+ (-3.7292231655e-03) * currR [A]^2 * rollDeg [deg]^2 * invGap
+ (-4.7717023731e-02) * currR [A]^2 * rollDeg [deg] * invGap^2
+ (-1.4923564858e+02) * currR [A]^2 * invGap^3
+ (+6.6540367362e-04) * currR [A] * rollDeg [deg]^4
+ (-9.6378069105e-02) * currR [A] * rollDeg [deg]^3 * invGap
+ (+2.3836959223e+00) * currR [A] * rollDeg [deg]^2 * invGap^2
+ (-1.2664937015e+02) * currR [A] * rollDeg [deg] * invGap^3
+ (+5.2627468646e+01) * currR [A] * invGap^4
+ (+1.4646059365e-10) * rollDeg [deg]^5
+ (-1.0417153181e+00) * rollDeg [deg]^4 * invGap
+ (+1.1431957446e-06) * rollDeg [deg]^3 * invGap^2
+ (+9.2056028147e+02) * rollDeg [deg]^2 * invGap^3
+ (-3.1228040188e-04) * rollDeg [deg] * invGap^4
+ (-5.9623186232e+02) * invGap^5
+ (+2.9763214116e-09) * currL [A]^6
+ (-6.2261733547e-09) * currL [A]^5 * currR [A]
+ (-3.7175269085e-08) * currL [A]^5 * rollDeg [deg]
+ (-3.1125615720e-06) * currL [A]^5 * invGap
+ (-9.9876729109e-09) * currL [A]^4 * currR [A]^2
+ (-1.3294041423e-08) * currL [A]^4 * currR [A] * rollDeg [deg]
+ (+2.2610334440e-06) * currL [A]^4 * currR [A] * invGap
+ (+2.4101829865e-08) * currL [A]^4 * rollDeg [deg]^2
+ (+2.5320289563e-06) * currL [A]^4 * rollDeg [deg] * invGap
+ (+2.0532678993e-04) * currL [A]^4 * invGap^2
+ (-7.0411871889e-09) * currL [A]^3 * currR [A]^3
+ (+1.1956544199e-08) * currL [A]^3 * currR [A]^2 * rollDeg [deg]
+ (+1.9188820701e-06) * currL [A]^3 * currR [A]^2 * invGap
+ (+6.3643952330e-08) * currL [A]^3 * currR [A] * rollDeg [deg]^2
+ (-3.3660324359e-06) * currL [A]^3 * currR [A] * rollDeg [deg] * invGap
+ (+1.0594658745e-05) * currL [A]^3 * currR [A] * invGap^2
+ (-2.5937761805e-07) * currL [A]^3 * rollDeg [deg]^3
+ (-7.2227672945e-06) * currL [A]^3 * rollDeg [deg]^2 * invGap
+ (+9.6876113423e-05) * currL [A]^3 * rollDeg [deg] * invGap^2
+ (+8.5422144858e-03) * currL [A]^3 * invGap^3
+ (-1.0519386251e-08) * currL [A]^2 * currR [A]^4
+ (-1.1957411061e-08) * currL [A]^2 * currR [A]^3 * rollDeg [deg]
+ (+2.1222821118e-06) * currL [A]^2 * currR [A]^3 * invGap
+ (+4.4935102750e-08) * currL [A]^2 * currR [A]^2 * rollDeg [deg]^2
+ (-1.2807847845e-04) * currL [A]^2 * currR [A]^2 * invGap^2
+ (+5.7679319987e-08) * currL [A]^2 * currR [A] * rollDeg [deg]^3
+ (-1.4777475671e-05) * currL [A]^2 * currR [A] * rollDeg [deg]^2 * invGap
+ (-2.5449360510e-04) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap^2
+ (-1.2272562713e-02) * currL [A]^2 * currR [A] * invGap^3
+ (+8.8287809685e-07) * currL [A]^2 * rollDeg [deg]^4
+ (+4.4586018461e-04) * currL [A]^2 * rollDeg [deg]^3 * invGap
+ (+3.8576999819e-02) * currL [A]^2 * rollDeg [deg]^2 * invGap^2
+ (+3.4044411238e-01) * currL [A]^2 * rollDeg [deg] * invGap^3
+ (+3.5950095140e+02) * currL [A]^2 * invGap^4
+ (-8.4967268776e-09) * currL [A] * currR [A]^5
+ (+1.3295050394e-08) * currL [A] * currR [A]^4 * rollDeg [deg]
+ (+1.7032328259e-06) * currL [A] * currR [A]^4 * invGap
+ (+7.0043846279e-08) * currL [A] * currR [A]^3 * rollDeg [deg]^2
+ (+3.3657944138e-06) * currL [A] * currR [A]^3 * rollDeg [deg] * invGap
+ (+6.8900834879e-05) * currL [A] * currR [A]^3 * invGap^2
+ (-5.7674824028e-08) * currL [A] * currR [A]^2 * rollDeg [deg]^3
+ (-1.4623729019e-05) * currL [A] * currR [A]^2 * rollDeg [deg]^2 * invGap
+ (+2.5450219980e-04) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap^2
+ (-1.0593500843e-03) * currL [A] * currR [A]^2 * invGap^3
+ (+1.0484950490e-06) * currL [A] * currR [A] * rollDeg [deg]^4
+ (-3.0932389983e-10) * currL [A] * currR [A] * rollDeg [deg]^3 * invGap
+ (+5.6370243409e-03) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap^2
+ (-6.1682354633e-07) * currL [A] * currR [A] * rollDeg [deg] * invGap^3
+ (+2.2923219836e+01) * currL [A] * currR [A] * invGap^4
+ (-2.3420164800e-05) * currL [A] * rollDeg [deg]^5
+ (-9.2739975478e-03) * currL [A] * rollDeg [deg]^4 * invGap
+ (-7.4080422659e-01) * currL [A] * rollDeg [deg]^3 * invGap^2
+ (-2.1756879968e+01) * currL [A] * rollDeg [deg]^2 * invGap^3
+ (-3.5929041347e+02) * currL [A] * rollDeg [deg] * invGap^4
+ (+1.3909390562e+01) * currL [A] * invGap^5
+ (+6.1617129177e-09) * currR [A]^6
+ (+3.7175730938e-08) * currR [A]^5 * rollDeg [deg]
+ (-2.1826492898e-06) * currR [A]^5 * invGap
+ (+3.0887363778e-08) * currR [A]^4 * rollDeg [deg]^2
+ (-2.5321109476e-06) * currR [A]^4 * rollDeg [deg] * invGap
+ (+2.0057675659e-04) * currR [A]^4 * invGap^2
+ (+2.5936417813e-07) * currR [A]^3 * rollDeg [deg]^3
+ (-7.9156716870e-06) * currR [A]^3 * rollDeg [deg]^2 * invGap
+ (-9.6870862064e-05) * currR [A]^3 * rollDeg [deg] * invGap^2
+ (-1.1576514633e-02) * currR [A]^3 * invGap^3
+ (+1.0210733379e-06) * currR [A]^2 * rollDeg [deg]^4
+ (-4.4586066717e-04) * currR [A]^2 * rollDeg [deg]^3 * invGap
+ (+3.8287512141e-02) * currR [A]^2 * rollDeg [deg]^2 * invGap^2
+ (-3.4044501906e-01) * currR [A]^2 * rollDeg [deg] * invGap^3
+ (+3.6814992351e+02) * currR [A]^2 * invGap^4
+ (+2.3420159192e-05) * currR [A] * rollDeg [deg]^5
+ (-9.2337309491e-03) * currR [A] * rollDeg [deg]^4 * invGap
+ (+7.4080431520e-01) * currR [A] * rollDeg [deg]^3 * invGap^2
+ (-2.1520950367e+01) * currR [A] * rollDeg [deg]^2 * invGap^3
+ (+3.5929051935e+02) * currR [A] * rollDeg [deg] * invGap^4
+ (+1.4109361400e+01) * currR [A] * invGap^5
+ (+1.7232857807e-04) * rollDeg [deg]^6
+ (-1.5228368774e-09) * rollDeg [deg]^5 * invGap
+ (+7.4229949571e+00) * rollDeg [deg]^4 * invGap^2
+ (-4.7116044996e-06) * rollDeg [deg]^3 * invGap^3
+ (+4.0058823232e+02) * rollDeg [deg]^2 * invGap^4
+ (+7.4032294285e-04) * rollDeg [deg] * invGap^5
+ (-1.3151563792e+02) * invGap^6
================================================================================
TORQUE [mN*m] =
-2.5609643910e+00
+ (+1.2736755974e+01) * currL [A]
+ (-1.3082527996e+01) * currR [A]
+ (-1.3305641735e+01) * rollDeg [deg]
+ (+4.4151637366e+01) * invGap
+ (-6.3923649292e-02) * currL [A]^2
+ (-1.1560951475e-02) * currL [A] * currR [A]
+ (+1.8386370689e+00) * currL [A] * rollDeg [deg]
+ (-4.5922645259e+02) * currL [A] * invGap
+ (+9.8233200925e-02) * currR [A]^2
+ (+1.8386371393e+00) * currR [A] * rollDeg [deg]
+ (+4.7236896866e+02) * currR [A] * invGap
+ (+9.1214755261e-01) * rollDeg [deg]^2
+ (-1.0749540976e+03) * rollDeg [deg] * invGap
+ (-1.7435331556e+02) * invGap^2
+ (+1.3682740756e-03) * currL [A]^3
+ (+1.9533070101e-03) * currL [A]^2 * currR [A]
+ (+1.9657240805e-02) * currL [A]^2 * rollDeg [deg]
+ (+2.0525258396e+00) * currL [A]^2 * invGap
+ (-2.0087168696e-03) * currL [A] * currR [A]^2
+ (-7.7704180912e-03) * currL [A] * currR [A] * rollDeg [deg]
+ (+4.6783332660e-01) * currL [A] * currR [A] * invGap
+ (-1.5697787523e-01) * currL [A] * rollDeg [deg]^2
+ (-2.5279026405e+01) * currL [A] * rollDeg [deg] * invGap
+ (-2.6796288784e+03) * currL [A] * invGap^2
+ (-6.9397567186e-04) * currR [A]^3
+ (+1.9657238531e-02) * currR [A]^2 * rollDeg [deg]
+ (-3.1136966908e+00) * currR [A]^2 * invGap
+ (+1.8298169417e-01) * currR [A] * rollDeg [deg]^2
+ (-2.5279027176e+01) * currR [A] * rollDeg [deg] * invGap
+ (+2.5386184669e+03) * currR [A] * invGap^2
+ (+6.2974733858e-01) * rollDeg [deg]^3
+ (-1.3210284466e+01) * rollDeg [deg]^2 * invGap
+ (+4.7007171607e+04) * rollDeg [deg] * invGap^2
+ (-3.3555302551e+02) * invGap^3
+ (-1.4305035722e-04) * currL [A]^4
+ (-1.6602474226e-05) * currL [A]^3 * currR [A]
+ (-1.2886823540e-04) * currL [A]^3 * rollDeg [deg]
+ (+6.0143039665e-03) * currL [A]^3 * invGap
+ (+1.2951511025e-05) * currL [A]^2 * currR [A]^2
+ (-5.1776437670e-04) * currL [A]^2 * currR [A] * rollDeg [deg]
+ (-3.8569371878e-02) * currL [A]^2 * currR [A] * invGap
+ (+5.2166822420e-03) * currL [A]^2 * rollDeg [deg]^2
+ (-6.2117182881e-01) * currL [A]^2 * rollDeg [deg] * invGap
+ (+9.0620919904e+01) * currL [A]^2 * invGap^2
+ (+1.3270592660e-06) * currL [A] * currR [A]^3
+ (-5.1776417343e-04) * currL [A] * currR [A]^2 * rollDeg [deg]
+ (+4.2265553856e-02) * currL [A] * currR [A]^2 * invGap
+ (-2.2041148798e-04) * currL [A] * currR [A] * rollDeg [deg]^2
+ (+2.4509139646e-01) * currL [A] * currR [A] * rollDeg [deg] * invGap
+ (-4.9160159525e+00) * currL [A] * currR [A] * invGap^2
+ (-1.7295894521e-01) * currL [A] * rollDeg [deg]^3
+ (-6.7494817486e+00) * currL [A] * rollDeg [deg]^2 * invGap
+ (-8.6427297569e+02) * currL [A] * rollDeg [deg] * invGap^2
+ (+8.5965909925e+03) * currL [A] * invGap^3
+ (-1.6550913550e-05) * currR [A]^4
+ (-1.2886821877e-04) * currR [A]^3 * rollDeg [deg]
+ (-3.0326211668e-02) * currR [A]^3 * invGap
+ (-5.1175783571e-03) * currR [A]^2 * rollDeg [deg]^2
+ (-6.2117181767e-01) * currR [A]^2 * rollDeg [deg] * invGap
+ (-7.5004948389e+01) * currR [A]^2 * invGap^2
+ (-1.7295894543e-01) * currR [A] * rollDeg [deg]^3
+ (+5.7289182157e+00) * currR [A] * rollDeg [deg]^2 * invGap
+ (-8.6427297453e+02) * currR [A] * rollDeg [deg] * invGap^2
+ (-8.1554262787e+03) * currR [A] * invGap^3
+ (-8.0890042154e-02) * rollDeg [deg]^4
+ (+1.7462417614e+02) * rollDeg [deg]^3 * invGap
+ (+5.9548102453e+01) * rollDeg [deg]^2 * invGap^2
+ (-9.1016550502e+04) * rollDeg [deg] * invGap^3
+ (-1.2462037728e+02) * invGap^4
+ (-1.2219304153e-05) * currL [A]^5
+ (-6.3133366268e-06) * currL [A]^4 * currR [A]
+ (+5.1514166444e-07) * currL [A]^4 * rollDeg [deg]
+ (+1.1879557083e-03) * currL [A]^4 * invGap
+ (-1.0954778098e-06) * currL [A]^3 * currR [A]^2
+ (-5.8420851161e-06) * currL [A]^3 * currR [A] * rollDeg [deg]
+ (+2.5831185234e-03) * currL [A]^3 * currR [A] * invGap
+ (+7.9254089698e-05) * currL [A]^3 * rollDeg [deg]^2
+ (-1.7220995748e-03) * currL [A]^3 * rollDeg [deg] * invGap
+ (-4.0028155701e-01) * currL [A]^3 * invGap^2
+ (+1.7079834365e-06) * currL [A]^2 * currR [A]^3
+ (-7.8935650691e-06) * currL [A]^2 * currR [A]^2 * rollDeg [deg]
+ (-3.2330488590e-04) * currL [A]^2 * currR [A]^2 * invGap
+ (+9.9753661971e-05) * currL [A]^2 * currR [A] * rollDeg [deg]^2
+ (+7.2169993632e-03) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap
+ (+5.4494940800e-01) * currL [A]^2 * currR [A] * invGap^2
+ (-1.5381149512e-03) * currL [A]^2 * rollDeg [deg]^3
+ (-1.9907607357e-01) * currL [A]^2 * rollDeg [deg]^2 * invGap
+ (+1.0517964842e+01) * currL [A]^2 * rollDeg [deg] * invGap^2
+ (-5.9716223372e+02) * currL [A]^2 * invGap^3
+ (+5.7682329953e-06) * currL [A] * currR [A]^4
+ (-5.8420830129e-06) * currL [A] * currR [A]^3 * rollDeg [deg]
+ (-3.1387669824e-03) * currL [A] * currR [A]^3 * invGap
+ (-9.2627696858e-05) * currL [A] * currR [A]^2 * rollDeg [deg]^2
+ (+7.2169983158e-03) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap
+ (-5.4112374537e-01) * currL [A] * currR [A]^2 * invGap^2
+ (-3.1504005406e-05) * currL [A] * currR [A] * rollDeg [deg]^3
+ (-1.7005015703e-04) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap
+ (-2.3976914010e+00) * currL [A] * currR [A] * rollDeg [deg] * invGap^2
+ (+2.0162876449e+01) * currL [A] * currR [A] * invGap^3
+ (+3.0541236561e-02) * currL [A] * rollDeg [deg]^4
+ (+6.0909890705e+00) * currL [A] * rollDeg [deg]^3 * invGap
+ (+1.8686476537e+02) * currL [A] * rollDeg [deg]^2 * invGap^2
+ (+5.4569961037e+03) * currL [A] * rollDeg [deg] * invGap^3
+ (+3.6521334233e+03) * currL [A] * invGap^4
+ (+1.2356741024e-05) * currR [A]^5
+ (+5.1514541610e-07) * currR [A]^4 * rollDeg [deg]
+ (-3.2093123475e-04) * currR [A]^4 * invGap
+ (-9.5040930219e-05) * currR [A]^3 * rollDeg [deg]^2
+ (-1.7221002130e-03) * currR [A]^3 * rollDeg [deg] * invGap
+ (+6.5615872016e-01) * currR [A]^3 * invGap^2
+ (-1.5381149487e-03) * currR [A]^2 * rollDeg [deg]^3
+ (+2.0395948760e-01) * currR [A]^2 * rollDeg [deg]^2 * invGap
+ (+1.0517964822e+01) * currR [A]^2 * rollDeg [deg] * invGap^2
+ (+4.9837046412e+02) * currR [A]^2 * invGap^3
+ (-3.0694304201e-02) * currR [A] * rollDeg [deg]^4
+ (+6.0909890702e+00) * currR [A] * rollDeg [deg]^3 * invGap
+ (-1.7712281328e+02) * currR [A] * rollDeg [deg]^2 * invGap^2
+ (+5.4569961045e+03) * currR [A] * rollDeg [deg] * invGap^3
+ (-3.4643372364e+03) * currR [A] * invGap^4
+ (-3.1967283950e-01) * rollDeg [deg]^5
+ (+7.2465373581e-01) * rollDeg [deg]^4 * invGap
+ (-3.7694440105e+03) * rollDeg [deg]^3 * invGap^2
+ (+4.4429019352e+01) * rollDeg [deg]^2 * invGap^3
+ (-3.9730669147e+04) * rollDeg [deg] * invGap^4
+ (-3.1813627757e+01) * invGap^5
+ (+2.0551811986e-07) * currL [A]^6
+ (-2.2810121436e-07) * currL [A]^5 * currR [A]
+ (-1.3383146324e-07) * currL [A]^5 * rollDeg [deg]
+ (+1.6049756900e-04) * currL [A]^5 * invGap
+ (-5.6453359321e-07) * currL [A]^4 * currR [A]^2
+ (+2.6031671041e-07) * currL [A]^4 * currR [A] * rollDeg [deg]
+ (+1.2744387021e-05) * currL [A]^4 * currR [A] * invGap
+ (+2.8978174669e-06) * currL [A]^4 * rollDeg [deg]^2
+ (-9.3438872511e-05) * currL [A]^4 * rollDeg [deg] * invGap
+ (-3.2491823205e-03) * currL [A]^4 * invGap^2
+ (+3.7946676912e-09) * currL [A]^3 * currR [A]^3
+ (+1.7599444391e-07) * currL [A]^3 * currR [A]^2 * rollDeg [deg]
+ (+2.4965108992e-05) * currL [A]^3 * currR [A]^2 * invGap
+ (+1.1286476678e-06) * currL [A]^3 * currR [A] * rollDeg [deg]^2
+ (+4.5300600434e-05) * currL [A]^3 * currR [A] * rollDeg [deg] * invGap
+ (-1.5200582693e-02) * currL [A]^3 * currR [A] * invGap^2
+ (+2.7490628440e-06) * currL [A]^3 * rollDeg [deg]^3
+ (-8.2493098857e-04) * currL [A]^3 * rollDeg [deg]^2 * invGap
+ (+2.3944496913e-02) * currL [A]^3 * rollDeg [deg] * invGap^2
+ (+1.2946953968e+00) * currL [A]^3 * invGap^3
+ (+5.9211265580e-07) * currL [A]^2 * currR [A]^4
+ (+1.7599464286e-07) * currL [A]^2 * currR [A]^3 * rollDeg [deg]
+ (-3.7773169680e-05) * currL [A]^2 * currR [A]^3 * invGap
+ (-7.7324017411e-08) * currL [A]^2 * currR [A]^2 * rollDeg [deg]^2
+ (+1.6224376321e-04) * currL [A]^2 * currR [A]^2 * rollDeg [deg] * invGap
+ (+1.1125393841e-03) * currL [A]^2 * currR [A]^2 * invGap^2
+ (-1.8213825115e-08) * currL [A]^2 * currR [A] * rollDeg [deg]^3
+ (-1.1582951117e-03) * currL [A]^2 * currR [A] * rollDeg [deg]^2 * invGap
+ (-2.7423615740e-02) * currL [A]^2 * currR [A] * rollDeg [deg] * invGap^2
+ (-1.8375761343e+00) * currL [A]^2 * currR [A] * invGap^3
+ (+1.8148082155e-05) * currL [A]^2 * rollDeg [deg]^4
+ (+2.5582361106e-02) * currL [A]^2 * rollDeg [deg]^3 * invGap
+ (+1.8719567169e+00) * currL [A]^2 * rollDeg [deg]^2 * invGap^2
+ (-7.9375540256e+00) * currL [A]^2 * rollDeg [deg] * invGap^3
+ (+1.3851610060e+03) * currL [A]^2 * invGap^4
+ (+3.6572413364e-07) * currL [A] * currR [A]^5
+ (+2.6031864309e-07) * currL [A] * currR [A]^4 * rollDeg [deg]
+ (-1.3740560576e-05) * currL [A] * currR [A]^4 * invGap
+ (-1.1634704791e-06) * currL [A] * currR [A]^3 * rollDeg [deg]^2
+ (+4.5300591111e-05) * currL [A] * currR [A]^3 * rollDeg [deg] * invGap
+ (+1.7827349035e-02) * currL [A] * currR [A]^3 * invGap^2
+ (-1.8220930542e-08) * currL [A] * currR [A]^2 * rollDeg [deg]^3
+ (+1.0311063391e-03) * currL [A] * currR [A]^2 * rollDeg [deg]^2 * invGap
+ (-2.7423615867e-02) * currL [A] * currR [A]^2 * rollDeg [deg] * invGap^2
+ (+1.8202040056e+00) * currL [A] * currR [A]^2 * invGap^3
+ (+1.4693588980e-05) * currL [A] * currR [A] * rollDeg [deg]^4
+ (+2.0391970869e-03) * currL [A] * currR [A] * rollDeg [deg]^3 * invGap
+ (-5.1289752136e-03) * currL [A] * currR [A] * rollDeg [deg]^2 * invGap^2
+ (+1.3585046026e+01) * currL [A] * currR [A] * rollDeg [deg] * invGap^3
+ (-2.5312909994e+01) * currL [A] * currR [A] * invGap^4
+ (-1.2892918352e-03) * currL [A] * rollDeg [deg]^5
+ (-4.2744856185e-01) * currL [A] * rollDeg [deg]^4 * invGap
+ (-4.5230140146e+01) * currL [A] * rollDeg [deg]^3 * invGap^2
+ (-1.2691925261e+03) * currL [A] * rollDeg [deg]^2 * invGap^3
+ (-1.7946912044e+04) * currL [A] * rollDeg [deg] * invGap^4
+ (+9.8200368161e+02) * currL [A] * invGap^5
+ (+1.4402604620e-07) * currR [A]^6
+ (-1.3383078112e-07) * currR [A]^5 * rollDeg [deg]
+ (-1.5877721700e-04) * currR [A]^5 * invGap
+ (-1.8463799734e-06) * currR [A]^4 * rollDeg [deg]^2
+ (-9.3438911534e-05) * currR [A]^4 * rollDeg [deg] * invGap
+ (-2.0062450751e-03) * currR [A]^4 * invGap^2
+ (+2.7490636967e-06) * currR [A]^3 * rollDeg [deg]^3
+ (+1.0293575893e-03) * currR [A]^3 * rollDeg [deg]^2 * invGap
+ (+2.3944496992e-02) * currR [A]^3 * rollDeg [deg] * invGap^2
+ (-2.1505766179e+00) * currR [A]^3 * invGap^3
+ (-4.8050087258e-05) * currR [A]^2 * rollDeg [deg]^4
+ (+2.5582361094e-02) * currR [A]^2 * rollDeg [deg]^3 * invGap
+ (-1.8925775930e+00) * currR [A]^2 * rollDeg [deg]^2 * invGap^2
+ (-7.9375540232e+00) * currR [A]^2 * rollDeg [deg] * invGap^3
+ (-1.1457282555e+03) * currR [A]^2 * invGap^4
+ (-1.2892918417e-03) * currR [A] * rollDeg [deg]^5
+ (+4.3570132351e-01) * currR [A] * rollDeg [deg]^4 * invGap
+ (-4.5230140146e+01) * currR [A] * rollDeg [deg]^3 * invGap^2
+ (+1.2356182202e+03) * currR [A] * rollDeg [deg]^2 * invGap^3
+ (-1.7946912045e+04) * currR [A] * rollDeg [deg] * invGap^4
+ (-9.3157391357e+02) * currR [A] * invGap^5
+ (+1.9887971069e-03) * rollDeg [deg]^6
+ (+4.1532416818e+00) * rollDeg [deg]^5 * invGap
+ (-3.7128109697e+00) * rollDeg [deg]^4 * invGap^2
+ (+1.9150570779e+04) * rollDeg [deg]^3 * invGap^3
+ (+1.9562107621e+01) * rollDeg [deg]^2 * invGap^4
+ (-1.0765823661e+04) * rollDeg [deg] * invGap^5
+ (-6.8540115783e+00) * invGap^6

BIN
lev_sim/recordings/sim_20260221_134810.mp4 Normal file
View File

Binary file not shown.

BIN
lev_sim/recordings/sim_20260221_142649.mp4 Normal file
View File

Binary file not shown.

BIN
lev_sim/recordings/sim_20260221_212028.mp4 Normal file
View File

Binary file not shown.

BIN
lev_sim/recordings/sim_20260221_223407.mp4 Normal file
View File

Binary file not shown.

BIN
lev_sim/recordings/sim_20260221_225018.mp4 Normal file
View File

Binary file not shown.

11
lev_sim/temp.json Normal file
View File

@@ -0,0 +1,11 @@
{
"height_kp": 80.05607483893696,
"height_ki": 0,
"height_kd": 7.09266287860531,
"roll_kp": -0.600856607986966,
"roll_ki": 0,
"roll_kd": -0.1,
"pitch_kp": 50.3415835489009009,
"pitch_ki": 0.02319184022898008,
"pitch_kd": 0.017632648760979346
}

63
lev_sim/test_env.py Normal file
View File

@@ -0,0 +1,63 @@
# The following was generated by AI - see [18]
"""
Test script for LevPodEnv
Runs a simple episode with constant actions to verify the environment works
"""
from lev_pod_env import LevPodEnv
import numpy as np
import time
# Create environment with GUI for visualization
env = LevPodEnv(use_gui=True, initial_gap_mm=15)
print("=" * 60)
print("Testing LevPodEnv")
print("=" * 60)
print(f"Action space: {env.action_space}")
print(f" 4 PWM duty cycles: [front_L, front_R, back_L, back_R]")
print(f"Observation space: {env.observation_space}")
print(f" 8 values: [gaps(4), velocities(4)]")
print("=" * 60)
# Reset environment
obs, info = env.reset()
print(f"\nInitial observation:")
print(f" Gaps: CR={obs[0]*1000:.2f}mm, CL={obs[1]*1000:.2f}mm, F={obs[2]*1000:.2f}mm, B={obs[3]*1000:.2f}mm")
print(f" Velocities: CR={obs[4]*1000:.2f}mm/s, CL={obs[5]*1000:.2f}mm/s, F={obs[6]*1000:.2f}mm/s, B={obs[7]*1000:.2f}mm/s")
print(f" Average gap: {np.mean(obs[:4])*1000:.2f} mm")
# Run a few steps with constant action to test force application
print("\nRunning test episode...")
for step in range(500):
# Apply constant moderate PWM to all 4 coils
# 50% PWM should generate current that produces upward force
action = np.array([0,0,0,0], dtype=np.float32)
obs, reward, terminated, truncated, info = env.step(action)
if step % 5 == 0:
print(f"\nStep {step} (t={step/240:.2f}s):")
print(f" Sensor gaps: CR={obs[0]*1000:.2f}mm, CL={obs[1]*1000:.2f}mm, " +
f"F={obs[2]*1000:.2f}mm, B={obs[3]*1000:.2f}mm")
print(f" Velocities: CR={obs[4]*1000:.2f}mm/s, CL={obs[5]*1000:.2f}mm/s, " +
f"F={obs[6]*1000:.2f}mm/s, B={obs[7]*1000:.2f}mm/s")
print(f" Yoke gaps: front={info['gap_front_yoke']*1000:.2f}mm, back={info['gap_back_yoke']*1000:.2f}mm")
print(f" Roll: {np.degrees(info['roll']):.2f}°")
print(f" Currents: FL={info['curr_front_L']:.2f}A, FR={info['curr_front_R']:.2f}A, " +
f"BL={info['curr_back_L']:.2f}A, BR={info['curr_back_R']:.2f}A")
print(f" Forces: front={info['force_front']:.2f}N, back={info['force_back']:.2f}N")
print(f" Torques: front={info['torque_front']:.2f}mN·m, back={info['torque_back']:.2f}mN·m")
print(f" Reward: {reward:.2f}")
if terminated or truncated:
print(f"\nEpisode terminated at step {step}")
break
time.sleep(0.01)
print("\n" + "=" * 60)
print("Test complete!")
print("=" * 60)
env.close()