adipu/guadaloop_lev_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
a098ec546281f852cd1185747a613194028c4b8f
guadaloop_lev_control/lev_sim/lev_PID.ipynb

1033 lines
319 KiB
Plaintext
Raw Normal View History

rename to levSim, requirements, readme
2026-03-20 19:05:50 -05:00
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# PID Control Testing for Maglev Pod\n",
"\n",
"This notebook provides a feedforward + PID control interface for the `LevPodEnv` simulation environment.\n",
"\n",
"## Control Architecture\n",
"- **Feedforward**: `MaglevPredictor` estimates the equilibrium PWM needed to hover at the current gap height\n",
"- **Height PID**: Corrects residual gap error → additive PWM correction for all coils\n",
"- **Roll PID**: Corrects roll angle → differential left/right adjustment\n",
"- **Pitch PID**: Corrects pitch angle → differential front/back adjustment\n",
"\n",
"The outputs combine: `coil_pwm = feedforward + height_correction ± roll_adjustment ± pitch_adjustment`"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Target Gap Height: 11.86 mm\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"pybullet build time: Feb 21 2026 12:08:04\n"
]
}
],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"from lev_pod_env import LevPodEnv, TARGET_GAP\n",
"from pid_controller import PIDController, DEFAULT_GAINS\n",
"from pid_simulation import run_pid_simulation, feedforward_pwm, build_feedforward_lut\n",
"\n",
"# Set plot style for better aesthetics\n",
"plt.style.use('seaborn-v0_8-whitegrid')\n",
"plt.rcParams['figure.facecolor'] = 'white'\n",
"plt.rcParams['axes.facecolor'] = 'white'\n",
"plt.rcParams['font.size'] = 11\n",
"plt.rcParams['axes.titlesize'] = 14\n",
"plt.rcParams['axes.labelsize'] = 12\n",
"\n",
"print(f\"Target Gap Height: {TARGET_GAP * 1000:.2f} mm\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Load PID gains (JSON or defaults)\n",
"\n",
"Try to load `pid_best_params.json` (saved by Optuna in this folder). If missing, use `DEFAULT_GAINS`. You can override any value in the **PID Gains Configuration** cell below."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Loaded PID gains from /Users/adipu/Documents/guadaloop_lev_control/lev_sim/pid_best_params.json\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"/Users/adipu/Documents/guadaloop_lev_control/.venv/lib/python3.13/site-packages/tqdm/auto.py:21: TqdmWarning: IProgress not found. Please update jupyter and ipywidgets. See https://ipywidgets.readthedocs.io/en/stable/user_install.html\n",
" from .autonotebook import tqdm as notebook_tqdm\n"
]
}
],
"source": [
"import os\n",
"import json\n",
"\n",
"try:\n",
" import optuna_pid_tune as _optuna_tune\n",
" _best_path = os.path.join(os.path.dirname(os.path.abspath(_optuna_tune.__file__)), \"pid_best_params.json\")\n",
"except Exception:\n",
" _best_path = \"pid_best_params.json\"\n",
"\n",
"if os.path.isfile(_best_path):\n",
" with open(_best_path) as f:\n",
" _best = json.load(f)\n",
" HEIGHT_KP = _best[\"height_kp\"]\n",
" HEIGHT_KI = _best[\"height_ki\"]\n",
" HEIGHT_KD = _best[\"height_kd\"]\n",
" ROLL_KP = _best[\"roll_kp\"]\n",
" ROLL_KI = _best[\"roll_ki\"]\n",
" ROLL_KD = _best[\"roll_kd\"]\n",
" PITCH_KP = _best[\"pitch_kp\"]\n",
" PITCH_KI = _best[\"pitch_ki\"]\n",
" PITCH_KD = _best[\"pitch_kd\"]\n",
" gains = dict(_best)\n",
" print(f\"Loaded PID gains from {_best_path}\")\n",
"else:\n",
" HEIGHT_KP = DEFAULT_GAINS[\"height_kp\"]\n",
" HEIGHT_KI = DEFAULT_GAINS[\"height_ki\"]\n",
" HEIGHT_KD = DEFAULT_GAINS[\"height_kd\"]\n",
" ROLL_KP = DEFAULT_GAINS[\"roll_kp\"]\n",
" ROLL_KI = DEFAULT_GAINS[\"roll_ki\"]\n",
" ROLL_KD = DEFAULT_GAINS[\"roll_kd\"]\n",
" PITCH_KP = DEFAULT_GAINS[\"pitch_kp\"]\n",
" PITCH_KI = DEFAULT_GAINS[\"pitch_ki\"]\n",
" PITCH_KD = DEFAULT_GAINS[\"pitch_kd\"]\n",
" gains = dict(DEFAULT_GAINS)\n",
" print(\"Using DEFAULT_GAINS (no pid_best_params.json found). Run Optuna to save best params.\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## PID Controller Class"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"# PID controller and default gains live in pid_controller.py (imported above).\n",
"# Override gains in the next cell; they are passed to run_pid_simulation via the gains dict."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## PID Gains Configuration (optional overrides)\n",
"\n",
"Constants are already set above from `pid_best_params.json` or defaults. To **override**, uncomment and edit a line in the cell below (e.g. `HEIGHT_KP = 50.0`) and re-run that cell; then run the simulation."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"PID Gains (used by simulation):\n",
" Height: Kp=100.96239556037482, Ki=0.0, Kd=8.173809263716658\n",
" Roll: Kp=0.600856607986966, Ki=0.0, Kd=-0.1\n",
" Pitch: Kp=50.011470879925824, Ki=0, Kd=1.8990306608320433\n"
]
}
],
"source": [
"# Optional manual overrides: set any constant below and re-run this cell to use it.\n",
"# Otherwise the values from the \"Load PID gains\" cell (JSON or DEFAULT_GAINS) are used.\n",
"# HEIGHT_KP = 50.0\n",
"# HEIGHT_KI = 5.0\n",
"# HEIGHT_KD = 10.0\n",
"# ROLL_KP = 2.0\n",
"# ROLL_KI = 0.5\n",
"# ROLL_KD = 0.5\n",
"# PITCH_KP = 2.0\n",
"# PITCH_KI = 0.5\n",
"# PITCH_KD = 0.5\n",
"\n",
"# Build gains dict from current constants (used by run_pid_simulation)\n",
"gains = {\n",
" \"height_kp\": HEIGHT_KP, \"height_ki\": HEIGHT_KI, \"height_kd\": HEIGHT_KD,\n",
" \"roll_kp\": ROLL_KP, \"roll_ki\": ROLL_KI, \"roll_kd\": ROLL_KD,\n",
" \"pitch_kp\": PITCH_KP, \"pitch_ki\": PITCH_KI, \"pitch_kd\": PITCH_KD,\n",
"}\n",
"\n",
"print(\"PID Gains (used by simulation):\")\n",
"print(f\" Height: Kp={HEIGHT_KP}, Ki={HEIGHT_KI}, Kd={HEIGHT_KD}\")\n",
"print(f\" Roll: Kp={ROLL_KP}, Ki={ROLL_KI}, Kd={ROLL_KD}\")\n",
"print(f\" Pitch: Kp={PITCH_KP}, Ki={PITCH_KI}, Kd={PITCH_KD}\")"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Loading maglev model from /Users/adipu/Documents/guadaloop_lev_control/lev_sim/maglev_model.pkl...\n"
]
},
{
"name": "stderr",
"output_type": "stream",
"text": [
"/Users/adipu/Documents/guadaloop_lev_control/.venv/lib/python3.13/site-packages/sklearn/base.py:463: InconsistentVersionWarning: Trying to unpickle estimator PolynomialFeatures from version 1.7.2 when using version 1.8.0. This might lead to breaking code or invalid results. Use at your own risk. For more info please refer to:\n",
"https://scikit-learn.org/stable/model_persistence.html#security-maintainability-limitations\n",
" warnings.warn(\n",
"/Users/adipu/Documents/guadaloop_lev_control/.venv/lib/python3.13/site-packages/sklearn/base.py:463: InconsistentVersionWarning: Trying to unpickle estimator LinearRegression from version 1.7.2 when using version 1.8.0. This might lead to breaking code or invalid results. Use at your own risk. For more info please refer to:\n",
"https://scikit-learn.org/stable/model_persistence.html#security-maintainability-limitations\n",
" warnings.warn(\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"Model loaded. Degree: 6\n",
"Force R2: 1.0000\n",
"Torque R2: 0.9999\n",
"Feedforward LUT ready. At target gap (11.86 mm): PWM = -0.0006\n"
]
}
],
"source": [
"# ============================================================\n",
"# FEEDFORWARD USING MAGLEV PREDICTOR\n",
"# ============================================================\n",
"# Builds gap → equilibrium PWM LUT in pid_simulation (used by run_pid_simulation).\n",
"build_feedforward_lut()\n",
"print(f\"Feedforward LUT ready. At target gap ({TARGET_GAP*1000:.2f} mm): PWM = {feedforward_pwm(TARGET_GAP * 1000):.4f}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Simulation Runner"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [],
"source": [
"# run_pid_simulation is imported from pid_simulation (see imports).\n",
"# It accepts gains=... (dict with height_kp, height_ki, ...), plus initial_gap_mm, max_steps, etc.\n",
"# The gains dict is built in the \"PID Gains Configuration\" cell above."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Plotting Functions"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"def plot_results(data: dict, title_suffix: str = \"\"):\n",
" \"\"\"\n",
" Create aesthetic plots of simulation results.\n",
" \n",
" Args:\n",
" data: Dictionary from run_pid_simulation()\n",
" title_suffix: Optional suffix for plot titles\n",
" \"\"\"\n",
" fig, axes = plt.subplots(2, 2, figsize=(14, 10))\n",
" fig.suptitle(f'PID Control Performance{title_suffix}', fontsize=16, fontweight='bold', y=1.02)\n",
" \n",
" target_gap_mm = TARGET_GAP * 1000\n",
" time = data['time']\n",
" \n",
" # Color palette\n",
" colors = {\n",
" 'primary': '#2563eb', # Blue\n",
" 'secondary': '#7c3aed', # Purple\n",
" 'accent1': '#059669', # Green\n",
" 'accent2': '#dc2626', # Red\n",
" 'target': '#f59e0b', # Orange\n",
" 'grid': '#e5e7eb'\n",
" }\n",
" \n",
" # ========== Gap Height Plot ==========\n",
" ax1 = axes[0, 0]\n",
" ax1.plot(time, data['gap_avg'], color=colors['primary'], linewidth=2, label='Average Gap')\n",
" ax1.plot(time, data['gap_front'], color=colors['secondary'], linewidth=1, alpha=0.6, label='Front Yoke')\n",
" ax1.plot(time, data['gap_back'], color=colors['accent1'], linewidth=1, alpha=0.6, label='Back Yoke')\n",
" ax1.axhline(y=target_gap_mm, color=colors['target'], linestyle='--', linewidth=2, label=f'Target ({target_gap_mm:.1f}mm)')\n",
" \n",
" ax1.set_xlabel('Time (s)')\n",
" ax1.set_ylabel('Gap Height (mm)')\n",
" ax1.set_title('Gap Height Over Time', fontweight='bold')\n",
" ax1.legend(loc='best', framealpha=0.9)\n",
" ax1.set_ylim([0, 20])\n",
" ax1.grid(True, alpha=0.3)\n",
" \n",
" # Add settling info\n",
" final_error = abs(data['gap_avg'][-1] - target_gap_mm)\n",
" ax1.text(0.98, 0.02, f'Final error: {final_error:.2f}mm', \n",
" transform=ax1.transAxes, ha='right', va='bottom',\n",
" fontsize=10, bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))\n",
" \n",
" # ========== Roll Angle Plot ==========\n",
" ax2 = axes[0, 1]\n",
" ax2.plot(time, data['roll_deg'], color=colors['primary'], linewidth=2)\n",
" ax2.axhline(y=0, color=colors['target'], linestyle='--', linewidth=1.5, alpha=0.7)\n",
" ax2.fill_between(time, data['roll_deg'], 0, alpha=0.2, color=colors['primary'])\n",
" \n",
" ax2.set_xlabel('Time (s)')\n",
" ax2.set_ylabel('Roll Angle (degrees)')\n",
" ax2.set_title('Roll Angle Over Time', fontweight='bold')\n",
" ax2.grid(True, alpha=0.3)\n",
" \n",
" ax2.set_ylim([-4, 4])\n",
" \n",
" # ========== Pitch Angle Plot ==========\n",
" ax3 = axes[1, 0]\n",
" ax3.plot(time, data['pitch_deg'], color=colors['secondary'], linewidth=2)\n",
" ax3.axhline(y=0, color=colors['target'], linestyle='--', linewidth=1.5, alpha=0.7)\n",
" ax3.fill_between(time, data['pitch_deg'], 0, alpha=0.2, color=colors['secondary'])\n",
" \n",
" ax3.set_xlabel('Time (s)')\n",
" ax3.set_ylabel('Pitch Angle (degrees)')\n",
" ax3.set_title('Pitch Angle Over Time', fontweight='bold')\n",
" ax3.grid(True, alpha=0.3)\n",
" \n",
" ax3.set_ylim([-6, 6])\n",
" \n",
" # ========== Current Draw Plot ==========\n",
" ax4 = axes[1, 1]\n",
" ax4.plot(time, data['current_FL'], linewidth=1.5, label='Front Left', alpha=0.8)\n",
" ax4.plot(time, data['current_FR'], linewidth=1.5, label='Front Right', alpha=0.8)\n",
" ax4.plot(time, data['current_BL'], linewidth=1.5, label='Back Left', alpha=0.8)\n",
" ax4.plot(time, data['current_BR'], linewidth=1.5, label='Back Right', alpha=0.8)\n",
" ax4.plot(time, data['current_total'], color='black', linewidth=2, label='Total', linestyle='--')\n",
" \n",
" ax4.set_xlabel('Time (s)')\n",
" ax4.set_ylabel('Current (A)')\n",
" ax4.set_title('Coil Current Draw Over Time', fontweight='bold')\n",
" ax4.legend(loc='best', framealpha=0.9, ncol=2)\n",
" ax4.grid(True, alpha=0.3)\n",
" \n",
" # Add average power info\n",
" avg_total_current = np.mean(data['current_total'])\n",
" ax4.text(0.98, 0.98, f'Avg total: {avg_total_current:.2f}A', \n",
" transform=ax4.transAxes, ha='right', va='top',\n",
" fontsize=10, bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))\n",
" \n",
" plt.tight_layout()\n",
" plt.show()\n",
" \n",
" return fig\n",
"\n",
"\n",
"def show_current_slice(data: dict, t_start: float, t_end: float):\n",
" \"\"\"\n",
" Plot coil current draw over a time slice (same as bottom-right graph in plot_results()).\n",
"\n",
" Args:\n",
" data: Dictionary from run_pid_simulation()\n",
" t_start: Start time in seconds (inclusive)\n",
" t_end: End time in seconds (inclusive)\n",
" \"\"\"\n",
" time = np.asarray(data['time'])\n",
" mask = (time >= t_start) & (time <= t_end)\n",
" if not np.any(mask):\n",
" print(f'No data in range [{t_start}, {t_end}] s.')\n",
" return\n",
" t = time[mask]\n",
" fig, ax = plt.subplots(figsize=(10, 4))\n",
" ax.plot(t, np.asarray(data['current_FL'])[mask], linewidth=1.5, label='Front Left', alpha=0.8)\n",
" ax.plot(t, np.asarray(data['current_FR'])[mask], linewidth=1.5, label='Front Right', alpha=0.8)\n",
" ax.plot(t, np.asarray(data['current_BL'])[mask], linewidth=1.5, label='Back Left', alpha=0.8)\n",
" ax.plot(t, np.asarray(data['current_BR'])[mask], linewidth=1.5, label='Back Right', alpha=0.8)\n",
" ax.plot(t, np.asarray(data['current_total'])[mask], color='black', linewidth=2, label='Total', linestyle='--')\n",
" ax.set_xlabel('Time (s)')\n",
" ax.set_ylabel('Current (A)')\n",
" ax.set_title(f'Coil Current Draw ({t_start}–{t_end} s)', fontweight='bold')\n",
" ax.legend(loc='best', framealpha=0.9, ncol=2)\n",
" ax.grid(True, alpha=0.3)\n",
" avg_total = np.mean(np.asarray(data['current_total'])[mask])\n",
" ax.text(0.98, 0.98, f'Avg total: {avg_total:.2f}A', transform=ax.transAxes, ha='right', va='top',\n",
" fontsize=10, bbox=dict(boxstyle='round', facecolor='white', alpha=0.8))\n",
" plt.tight_layout()\n",
" plt.show()\n",
" return fig"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [],
"source": [
"def plot_control_signals(data: dict):\n",
" \"\"\"\n",
" Plot the PWM control signals sent to each coil.\n",
" \n",
" Args:\n",
" data: Dictionary from run_pid_simulation()\n",
" \"\"\"\n",
" fig, ax = plt.subplots(figsize=(12, 5))\n",
" \n",
" time = data['time']\n",
" \n",
" ax.plot(time, data['pwm_FL'], label='Front Left', linewidth=1.5, alpha=0.8)\n",
" ax.plot(time, data['pwm_FR'], label='Front Right', linewidth=1.5, alpha=0.8)\n",
" ax.plot(time, data['pwm_BL'], label='Back Left', linewidth=1.5, alpha=0.8)\n",
" ax.plot(time, data['pwm_BR'], label='Back Right', linewidth=1.5, alpha=0.8)\n",
" \n",
" if 'ff_pwm' in data and len(data.get('ff_pwm', [])) > 0:\n",
" ff = data['ff_pwm'] if isinstance(data['ff_pwm'], np.ndarray) else np.array(data['ff_pwm'])\n",
" ax.plot(time, ff, label='Feedforward', color='black', linewidth=2, linestyle='-.', alpha=0.7)\n",
" \n",
" ax.axhline(y=0, color='gray', linestyle='-', linewidth=0.5)\n",
" ax.axhline(y=1, color='red', linestyle='--', linewidth=1, alpha=0.5, label='Saturation')\n",
" ax.axhline(y=-1, color='red', linestyle='--', linewidth=1, alpha=0.5)\n",
" \n",
" ax.set_xlabel('Time (s)')\n",
" ax.set_ylabel('PWM Duty Cycle')\n",
" ax.set_title('Control Signals (PWM)', fontsize=14, fontweight='bold')\n",
" ax.legend(loc='best', framealpha=0.9)\n",
" ax.set_ylim([-1.2, 1.2])\n",
" ax.grid(True, alpha=0.3)\n",
" \n",
" plt.tight_layout()\n",
" plt.show()\n",
" \n",
" return fig"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Run Simulation\n",
"\n",
"**Configure the simulation parameters below and run the cell.**"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Loading maglev model from /Users/adipu/Documents/guadaloop_lev_control/lev_sim/maglev_model.pkl...\n",
"Model loaded. Degree: 6\n",
"Force R2: 1.0000\n",
"Torque R2: 0.9999\n",
"Version = 4.1 Metal - 90.5\n",
"Vendor = Apple\n",
"Renderer = Apple M2\n",
"b3Printf: Selected demo: Physics Server\n",
"startThreads creating 1 threads.\n",
"starting thread 0\n",
"started thread 0 \n",
"MotionThreadFunc thread started\n",
"Starting simulation: initial_gap=9.0mm, target=11.86mm\n",
" Impulse disturbance: -2N at step 5000\n",
" Feedforward: enabled\n",
" Applied -2N impulse and 1.00 N·m torque at step 5000\n",
"numActiveThreads = 0\n",
"Simulation complete: 10000 steps, 41.66s\n",
" Final gap: 10.88mm (target: 11.86mm)\n",
" Final roll: 0.000°, pitch: -0.003°\n",
"stopping threads\n",
"Thread with taskId 0 exiting\n",
"Thread TERMINATED\n",
"destroy semaphore\n",
"semaphore destroyed\n",
"destroy main semaphore\n",
"main semaphore destroyed\n"
]
}
],
"source": [
"# ============================================================\n",
"# SIMULATION PARAMETERS\n",
"# ============================================================\n",
"\n",
"INITIAL_GAP_MM = 9.0 # Starting gap height (mm). Target is ~11.86mm\n",
"MAX_STEPS = 10000 # Simulation steps (240 steps = 1 second)\n",
"USE_GUI = True # PyBullet GUI often hangs in notebooks; keep False. Use recording instead.\n",
"USE_FEEDFORWARD = True # Use MaglevPredictor feedforward for base PWM\n",
"\n",
"# Recording (headless: no GUI window; files saved to RECORD_DIR)\n",
"RECORD_VIDEO = False # Set True to save MP4 of each run (e.g. recordings/sim_YYYYMMDD_HHMMSS.mp4)\n",
"RECORD_TELEMETRY = False # Set True to save 4-panel telemetry PNG per run\n",
"RECORD_DIR = \"recordings\" # Output folder for video and telemetry (created if missing)\n",
"\n",
"# Impulse disturbance (one-time force at a specific step)\n",
"DISTURBANCE_STEP = 5000 # Step at which to apply impulse (e.g., 500). None = disabled\n",
"DISTURBANCE_FORCE = -2 # Impulse force in Newtons (positive = upward push)\n",
"\n",
"# Stochastic disturbance (continuous random noise each step)\n",
"DISTURBANCE_FORCE_STD = 0 # Std dev of random force noise (Newtons). 0 = disabled\n",
"\n",
"# ============================================================\n",
"\n",
"# Run simulation (gains from \"PID Gains Configuration\" cell)\n",
"results = run_pid_simulation(\n",
" initial_gap_mm=INITIAL_GAP_MM,\n",
" max_steps=MAX_STEPS,\n",
" use_gui=USE_GUI,\n",
" disturbance_step=DISTURBANCE_STEP,\n",
" disturbance_force=DISTURBANCE_FORCE,\n",
" disturbance_force_std=DISTURBANCE_FORCE_STD,\n",
" use_feedforward=USE_FEEDFORWARD,\n",
" record_video=RECORD_VIDEO,\n",
" record_telemetry=RECORD_TELEMETRY,\n",
" record_dir=RECORD_DIR,\n",
" gains=gains,\n",
" verbose=True\n",
")"
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"{'time': array([0.00000000e+00, 4.16666667e-03, 8.33333333e-03, ...,\n",
" 4.16541667e+01, 4.16583333e+01, 4.16625000e+01], shape=(10000,)), 'gap_front': array([ 9. , 9.02630146, 9.07173494, ..., 10.88719188,\n",
" 10.87327881, 10.88719196], shape=(10000,)), 'gap_back': array([ 9. , 9.02630146, 9.07173494, ..., 10.87327859,\n",
" 10.88719174, 10.87327877], shape=(10000,)), 'gap_avg': array([ 9. , 9.02630146, 9.07173494, ..., 10.88023523,\n",
" 10.88023528, 10.88023537], shape=(10000,)), 'roll_deg': array([-0.00000000e+00, -5.27915552e-06, -2.13414205e-05, ...,\n",
" 1.30725108e-05, 1.30788079e-05, 1.30724589e-05], shape=(10000,)), 'pitch_deg': array([ 0. , 0. , 0. , ..., -0.0031659 ,\n",
" 0.00316582, -0.00316587], shape=(10000,)), 'current_FL': array([10.2 , 9.29838848, 8.99919415, ..., -3.97452164,\n",
" 10.2 , -3.97452164], shape=(10000,)), 'current_FR': array([10.2 , 9.29830265, 8.9989996 , ..., -3.97452831,\n",
" 10.2 , -3.97452831], shape=(10000,)), 'current_BL': array([10.2 , 9.29838848, 8.99919415, ..., 10.2 ,\n",
" -3.97451782, 10.2 ], shape=(10000,)), 'current_BR': array([10.2 , 9.29830265, 8.9989996 , ..., 10.2 ,\n",
" -3.97452259, 10.2 ], shape=(10000,)), 'current_total': array([40.8 , 37.19338226, 35.99638748, ..., 28.34904861,\n",
" 28.34904099, 28.34904861], shape=(10000,)), 'pwm_FL': array([0.95018876, 0.8899194 , 0.83739257, ..., 0.226274 , 0.42596412,\n",
" 0.226274 ], shape=(10000,), dtype=float32), 'pwm_FR': array([0.95018876, 0.8899151 , 0.83737934, ..., 0.2262737 , 0.42596382,\n",
" 0.2262737 ], shape=(10000,), dtype=float32), 'pwm_BL': array([0.95018876, 0.8899194 , 0.83739257, ..., 0.4259643 , 0.22627419,\n",
" 0.4259643 ], shape=(10000,), dtype=float32), 'pwm_BR': array([0.95018876, 0.8899151 , 0.83737934, ..., 0.425964 , 0.2262739 ,\n",
" 0.425964 ], shape=(10000,), dtype=float32), 'ff_pwm': array([0.66143623, 0.65541619, 0.64500579, ..., 0.22719963, 0.22719963,\n",
" 0.22719963], shape=(10000,))}\n"
]
}
],
"source": [
"print(results)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## View Results"
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1400x1000 with 4 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1000x400 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1000x400 with 1 Axes>"
]
},
"execution_count": 11,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Plot main performance metrics\n",
"plot_results(results)\n",
"show_current_slice(results, 20, 25)"
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1200x500 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1200x500 with 1 Axes>"
]
},
"execution_count": 12,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Plot control signals (optional)\n",
"plot_control_signals(results)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Compare Multiple Initial Conditions\n",
"\n",
"Run this cell to test the controller with different starting heights."
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {},
"outputs": [],
"source": [
"def compare_initial_conditions(initial_gaps_mm: list, max_steps: int = 2000):\n",
" \"\"\"\n",
" Compare PID performance across different initial conditions.\n",
" \n",
" Args:\n",
" initial_gaps_mm: List of starting gap heights to test\n",
" max_steps: Maximum steps per simulation\n",
" \"\"\"\n",
" fig, axes = plt.subplots(2, 2, figsize=(14, 10))\n",
" fig.suptitle('PID Performance Comparison: Different Initial Conditions', \n",
" fontsize=16, fontweight='bold', y=1.02)\n",
" \n",
" target_gap_mm = TARGET_GAP * 1000\n",
" colors = plt.cm.viridis(np.linspace(0, 0.8, len(initial_gaps_mm)))\n",
" \n",
" all_results = []\n",
" \n",
" for i, gap in enumerate(initial_gaps_mm):\n",
" print(f\"Running simulation {i+1}/{len(initial_gaps_mm)}: initial_gap={gap}mm\")\n",
" data = run_pid_simulation(initial_gap_mm=gap, max_steps=max_steps, gains=gains, verbose=False)\n",
" all_results.append((gap, data))\n",
" \n",
" label = f'{gap}mm'\n",
" \n",
" # Gap height\n",
" axes[0, 0].plot(data['time'], data['gap_avg'], color=colors[i], \n",
" linewidth=2, label=label)\n",
" \n",
" # Roll\n",
" axes[0, 1].plot(data['time'], data['roll_deg'], color=colors[i], \n",
" linewidth=2, label=label)\n",
" \n",
" # Pitch\n",
" axes[1, 0].plot(data['time'], data['pitch_deg'], color=colors[i], \n",
" linewidth=2, label=label)\n",
" \n",
" # Total current\n",
" axes[1, 1].plot(data['time'], data['current_total'], color=colors[i], \n",
" linewidth=2, label=label)\n",
" \n",
" # Add target line to gap plot\n",
" axes[0, 0].axhline(y=target_gap_mm, color='red', linestyle='--', \n",
" linewidth=2, label=f'Target ({target_gap_mm:.1f}mm)')\n",
" \n",
" # Configure axes\n",
" axes[0, 0].set_xlabel('Time (s)')\n",
" axes[0, 0].set_ylabel('Gap Height (mm)')\n",
" axes[0, 0].set_title('Average Gap Height', fontweight='bold')\n",
" axes[0, 0].legend(loc='best')\n",
" axes[0, 0].set_ylim([0, 20])\n",
" axes[0, 0].grid(True, alpha=0.3)\n",
" \n",
" axes[0, 1].axhline(y=0, color='gray', linestyle='--', linewidth=1)\n",
" axes[0, 1].set_xlabel('Time (s)')\n",
" axes[0, 1].set_ylabel('Roll Angle (degrees)')\n",
" axes[0, 1].set_title('Roll Angle', fontweight='bold')\n",
" axes[0, 1].legend(loc='best')\n",
" axes[0, 1].set_ylim([-4, 4])\n",
" axes[0, 1].grid(True, alpha=0.3)\n",
" \n",
" axes[1, 0].axhline(y=0, color='gray', linestyle='--', linewidth=1)\n",
" axes[1, 0].set_xlabel('Time (s)')\n",
" axes[1, 0].set_ylabel('Pitch Angle (degrees)')\n",
" axes[1, 0].set_title('Pitch Angle', fontweight='bold')\n",
" axes[1, 0].legend(loc='best')\n",
" axes[1, 0].set_ylim([-6, 6])\n",
" axes[1, 0].grid(True, alpha=0.3)\n",
" \n",
" axes[1, 1].set_xlabel('Time (s)')\n",
" axes[1, 1].set_ylabel('Total Current (A)')\n",
" axes[1, 1].set_title('Total Current Draw', fontweight='bold')\n",
" axes[1, 1].legend(loc='best')\n",
" axes[1, 1].grid(True, alpha=0.3)\n",
" \n",
" plt.tight_layout()\n",
" plt.show()\n",
" \n",
" return all_results"
]
},
{
"cell_type": "code",
"execution_count": 14,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"============================================================\n",
"Data structure: results (from run_pid_simulation)\n",
"============================================================\n",
"Number of time steps: 10000\n",
"\n",
"Key Shape Dtype Min Max \n",
"------------------------------------------------------------\n",
"current_BL (10000,) float64 -9.416 10.2 \n",
"current_BR (10000,) float64 -9.326 10.2 \n",
"current_FL (10000,) float64 -9.667 10.2 \n",
"current_FR (10000,) float64 -9.618 10.2 \n",
"current_total (10000,) float64 0.0007697 40.8 \n",
"ff_pwm (10000,) float64 -0.003738 0.6614 \n",
"gap_avg (10000,) float64 9 11.87 \n",
"gap_back (10000,) float64 9 11.86 \n",
"gap_front (10000,) float64 9 11.89 \n",
"pitch_deg (10000,) float64 -0.007633 0.003622 \n",
"pwm_BL (10000,) float32 -0.06703 0.9502 \n",
"pwm_BR (10000,) float32 -0.06227 0.9502 \n",
"pwm_FL (10000,) float32 -0.0651 0.9502 \n",
"pwm_FR (10000,) float32 -0.06026 0.9502 \n",
"roll_deg (10000,) float64 -0.05699 0.03849 \n",
"time (10000,) float64 0 41.66 \n",
"------------------------------------------------------------\n",
"\n",
"Sample (first and last time steps):\n",
"\n",
" Step 0: t = 0.0000 s\n",
" gap_avg=9.000 mm roll=-0.000° pitch=0.000°\n",
" pwm: FL=0.950 FR=0.950 BL=0.950 BR=0.950\n",
" ff_pwm=0.661 current_total=40.800 A\n",
"\n",
" Step 5000: t = 20.8333 s\n",
" gap_avg=11.860 mm roll=0.002° pitch=0.000°\n",
" pwm: FL=-0.001 FR=-0.001 BL=-0.001 BR=-0.001\n",
" ff_pwm=-0.001 current_total=0.019 A\n",
"\n",
" Step 9999: t = 41.6625 s\n",
" gap_avg=10.880 mm roll=0.000° pitch=-0.003°\n",
" pwm: FL=0.226 FR=0.226 BL=0.426 BR=0.426\n",
" ff_pwm=0.227 current_total=28.349 A\n"
]
}
],
"source": [
"# Show simulation results data structure in a readable format\n",
"def show_data_structure(data: dict, name: str = \"results\", sample_rows: int = 3):\n",
" \"\"\"Print keys, shapes, dtypes, and a small sample of the simulation data dict.\"\"\"\n",
" print(\"=\" * 60)\n",
" print(f\"Data structure: {name} (from run_pid_simulation)\")\n",
" print(\"=\" * 60)\n",
" n = len(data[\"time\"]) if \"time\" in data else 0\n",
" print(f\"Number of time steps: {n}\\n\")\n",
" print(f\"{'Key':<18} {'Shape':<14} {'Dtype':<12} {'Min':<10} {'Max':<10}\")\n",
" print(\"-\" * 60)\n",
" for key in sorted(data.keys()):\n",
" arr = data[key]\n",
" if isinstance(arr, np.ndarray):\n",
" sh = str(arr.shape)\n",
" dt = str(arr.dtype)\n",
" mn = f\"{np.min(arr):.4g}\" if arr.size else \"—\"\n",
" mx = f\"{np.max(arr):.4g}\" if arr.size else \"—\"\n",
" else:\n",
" sh, dt, mn, mx = \"—\", \"—\", \"—\", \"—\"\n",
" print(f\"{key:<18} {sh:<14} {dt:<12} {mn:<10} {mx:<10}\")\n",
" print(\"-\" * 60)\n",
" print(\"\\nSample (first and last time steps):\")\n",
" if n >= 1:\n",
" sample_idx = [0, n // 2, n - 1] if n >= 3 else list(range(n))\n",
" for i in sample_idx:\n",
" t = data[\"time\"][i]\n",
" print(f\"\\n Step {i}: t = {t:.4f} s\")\n",
" print(f\" gap_avg={data['gap_avg'][i]:.3f} mm roll={data['roll_deg'][i]:.3f}° pitch={data['pitch_deg'][i]:.3f}°\")\n",
" print(f\" pwm: FL={data['pwm_FL'][i]:.3f} FR={data['pwm_FR'][i]:.3f} BL={data['pwm_BL'][i]:.3f} BR={data['pwm_BR'][i]:.3f}\")\n",
" if \"ff_pwm\" in data:\n",
" print(f\" ff_pwm={data['ff_pwm'][i]:.3f} current_total={data['current_total'][i]:.3f} A\")\n",
"\n",
"show_data_structure(results, \"results\")"
]
},
{
"cell_type": "code",
"execution_count": 15,
"metadata": {},
"outputs": [],
"source": [
"# Compare different starting heights\n",
"# Uncomment and run to test multiple initial conditions\n",
"\n",
"# comparison_results = compare_initial_conditions(\n",
"# initial_gaps_mm=[10.0, 14.0, 18.0, 22.0],\n",
"# max_steps=2000\n",
"# )"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## PID Tuning with Optuna\n",
"\n",
"Bayesian-style optimization (Optuna TPE sampler) tunes all **nine** gains (height, roll, pitch × Kp, Ki, Kd) **jointly** so loop coupling is respected.\n",
"\n",
"- **Run optimizer** (script or cell below): saves `pid_best_params.json` in this folder.\n",
"- **Use new params**: re-run the **Load PID gains** cell at the top, then the **PID Gains** cell and the simulation."
]
},
{
"cell_type": "code",
"execution_count": 16,
"metadata": {},
"outputs": [],
"source": [
"# Option A: Run Optuna optimization (slow). Saves pid_best_params.json next to optuna_pid_tune.py.\n",
"# from optuna_pid_tune import run_optimization\n",
"# study = run_optimization(n_trials=50, timeout=1800)\n",
"# print(\"Best:\", study.best_params)\n",
"\n",
"# After running Optuna: re-run the \"Load PID gains\" cell at the top of the notebook\n",
"# to pick up the new pid_best_params.json, then re-run the PID Gains cell and the simulation."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Test Disturbance Rejection\n",
"\n",
"Apply a sudden force disturbance and observe recovery."
]
},
{
"cell_type": "code",
"execution_count": 17,
"metadata": {},
"outputs": [],
"source": [
"# Test disturbance rejection\n",
"# Uncomment and run to test disturbance response\n",
"\n",
"# Example 1: Impulse disturbance (one-time force)\n",
"# disturbance_results = run_pid_simulation(\n",
"# initial_gap_mm=11.86, # Start near target\n",
"# max_steps=3000,\n",
"# use_gui=False,\n",
"# disturbance_step=720, # Apply at 3 seconds\n",
"# disturbance_force=-20.0, # 20N downward push\n",
"# verbose=True\n",
"# )\n",
"# plot_results(disturbance_results, title_suffix=' (with 20N impulse at t=3s)')\n",
"\n",
"# Example 2: Continuous stochastic noise\n",
"# noisy_results = run_pid_simulation(\n",
"# initial_gap_mm=11.86,\n",
"# max_steps=3000,\n",
"# use_gui=False,\n",
"# disturbance_force_std=2.0, # 2N standard deviation continuous noise\n",
"# verbose=True\n",
"# )\n",
"# plot_results(noisy_results, title_suffix=' (with 2N std continuous noise)')"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"---\n",
"## Summary Statistics"
]
},
{
"cell_type": "code",
"execution_count": 18,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"==================================================\n",
"SIMULATION SUMMARY\n",
"==================================================\n",
"Duration: 41.66 seconds (10000 steps)\n",
"Target gap: 11.86 mm\n",
"\n",
"Gap Height:\n",
" Initial: 9.00 mm\n",
" Final: 10.88 mm\n",
" Error: 0.980 mm\n",
" Max: 11.87 mm\n",
" Min: 9.00 mm\n",
" Settling time (2%): 0.179 s\n",
"\n",
"Orientation (final):\n",
" Roll: +0.000 degrees\n",
" Pitch: -0.003 degrees\n",
"\n",
"Current Draw:\n",
" Average total: 14.23 A\n",
" Peak total: 40.80 A\n",
"==================================================\n"
]
}
],
"source": [
"def print_summary(data: dict):\n",
" \"\"\"Print summary statistics for a simulation run.\"\"\"\n",
" target_gap_mm = TARGET_GAP * 1000\n",
" \n",
" # Calculate settling time (within 2% of target)\n",
" tolerance = 0.02 * target_gap_mm\n",
" settled_idx = None\n",
" for i in range(len(data['gap_avg'])):\n",
" if abs(data['gap_avg'][i] - target_gap_mm) < tolerance:\n",
" # Check if it stays settled\n",
" if all(abs(data['gap_avg'][j] - target_gap_mm) < tolerance \n",
" for j in range(i, min(i+100, len(data['gap_avg'])))):\n",
" settled_idx = i\n",
" break\n",
" \n",
" print(\"=\" * 50)\n",
" print(\"SIMULATION SUMMARY\")\n",
" print(\"=\" * 50)\n",
" print(f\"Duration: {data['time'][-1]:.2f} seconds ({len(data['time'])} steps)\")\n",
" print(f\"Target gap: {target_gap_mm:.2f} mm\")\n",
" print()\n",
" print(\"Gap Height:\")\n",
" print(f\" Initial: {data['gap_avg'][0]:.2f} mm\")\n",
" print(f\" Final: {data['gap_avg'][-1]:.2f} mm\")\n",
" print(f\" Error: {abs(data['gap_avg'][-1] - target_gap_mm):.3f} mm\")\n",
" print(f\" Max: {max(data['gap_avg']):.2f} mm\")\n",
" print(f\" Min: {min(data['gap_avg']):.2f} mm\")\n",
" if settled_idx:\n",
" print(f\" Settling time (2%): {data['time'][settled_idx]:.3f} s\")\n",
" else:\n",
" print(f\" Settling time: Not settled within tolerance\")\n",
" print()\n",
" print(\"Orientation (final):\")\n",
" print(f\" Roll: {data['roll_deg'][-1]:+.3f} degrees\")\n",
" print(f\" Pitch: {data['pitch_deg'][-1]:+.3f} degrees\")\n",
" print()\n",
" print(\"Current Draw:\")\n",
" print(f\" Average total: {np.mean(data['current_total']):.2f} A\")\n",
" print(f\" Peak total: {max(data['current_total']):.2f} A\")\n",
" print(\"=\" * 50)\n",
"\n",
"# Print summary for last simulation\n",
"print_summary(results)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": ".venv",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.9"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
Reference in New Issue Copy Permalink