perhaps optimal

RL Testing/lev_pod_env.py

@@ -4,13 +4,15 @@ 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 = 16.491741 / 1000 # target gap height for 5.8 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):
+    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
@@ -20,6 +22,17 @@ class LevPodEnv(gym.Env):
 
         # 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 ---
@@ -143,7 +156,8 @@ class LevPodEnv(gym.Env):
                     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)
-                            label = "Center_Right" if local_pos[1] > 0 else "Center_Left"
+                            # +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)
@@ -156,7 +170,49 @@ class LevPodEnv(gym.Env):
         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]
@@ -295,7 +351,7 @@ class LevPodEnv(gym.Env):
             physicsClientId=self.client
         )
 
-        # --- 5b. Apply Stochastic Disturbance Force (if enabled) ---
+        # --- 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(
@@ -305,6 +361,17 @@ class LevPodEnv(gym.Env):
                 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)
@@ -390,7 +457,43 @@ class LevPodEnv(gym.Env):
             '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):
@@ -411,6 +514,20 @@ class LevPodEnv(gym.Env):
             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):
         """
@@ -511,12 +628,103 @@ class LevPodEnv(gym.Env):
         
         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 = 5.8 * 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

RL Testing/maglev_predictor.py

@@ -14,17 +14,22 @@ 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='maglev_model.pkl'):
+    def __init__(self, model_path=None):
         """
         Initialize predictor by loading the pickle file and extracting
         raw matrices for fast inference.
         """
-        if not os.path.exists(model_path):
-            raise FileNotFoundError(f"Model file '{model_path}' not found. Please train and save the model first.")
+        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 {model_path}...")
-        data = joblib.load(model_path)
+        print(f"Loading maglev model from {path}...")
+        data = joblib.load(path)
         
         # 1. Extract Scikit-Learn Objects
         poly_transformer = data['poly_features']

RL Testing/optuna_pid_tune.py

@@ -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 = [12.0, 18.0]  # Two conditions for robustness
+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,
+        )

RL Testing/pid_best_params.json

@@ -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
+}

RL Testing/pid_best_params_1500.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 112.05607483893696,
+  "height_ki": 1.3231206601751908,
+  "height_kd": 14.09266287860531,
+  "roll_kp": 3.600856607986966,
+  "roll_ki": 0.10314962498074487,
+  "roll_kd": 0.013792861414632316,
+  "pitch_kp": 0.1415835489009009,
+  "pitch_ki": 0.02319184022898008,
+  "pitch_kd": 0.017632648760979346
+}

RL Testing/pid_best_params_3000.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 7.886377342450826,
+  "height_ki": 0.5011631292801424,
+  "height_kd": 6.281854346104332,
+  "roll_kp": 0.5834114273785317,
+  "roll_ki": 4.958217971237515,
+  "roll_kd": 0.020111264395006163,
+  "pitch_kp": 1.7442062254360877,
+  "pitch_ki": 0.03376218829632256,
+  "pitch_kd": 2.7713413800138587
+}

RL Testing/pid_best_params_6000.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 6.387288593598476,
+  "height_ki": 3.8692474098125285,
+  "height_kd": 8.326592570264028,
+  "roll_kp": 2.5896160034319817,
+  "roll_ki": 0.26391970508131646,
+  "roll_kd": 0.10133339760273202,
+  "pitch_kp": 0.5612446354489615,
+  "pitch_ki": 0.014113041384510265,
+  "pitch_kd": 1.100684970603034
+}

RL Testing/pid_controller.py

@@ -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"]

RL Testing/pid_simulation.py

@@ -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 = 5.8,
+    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

RL Testing/requirements.txt

@@ -1,156 +0,0 @@
-annotated-types==0.7.0
-anyio==4.10.0
-appnope==0.1.4
-argon2-cffi==25.1.0
-argon2-cffi-bindings==25.1.0
-arrow==1.3.0
-asttokens==3.0.0
-async-lru==2.0.5
-attrs==25.3.0
-babel==2.17.0
-beautifulsoup4==4.13.5
-bleach==6.2.0
-cachetools==6.2.1
-certifi==2025.8.3
-cffi==1.17.1
-charset-normalizer==3.4.3
-click==8.3.0
-comm==0.2.3
-contourpy==1.3.3
-cycler==0.12.1
-debugpy==1.8.16
-decorator==5.2.1
-defusedxml==0.7.1
-executing==2.2.1
-fastapi==0.115.0
-fastjsonschema==2.21.2
-fonttools==4.59.2
-fqdn==1.5.1
-future==1.0.0
-google-ai-generativelanguage==0.6.10
-google-api-core==2.26.0
-google-api-python-client==2.185.0
-google-auth==2.41.1
-google-auth-httplib2==0.2.0
-google-generativeai==0.8.3
-googleapis-common-protos==1.70.0
-grpcio==1.75.1
-grpcio-status==1.71.2
-h11==0.16.0
-httpcore==1.0.9
-httplib2==0.31.0
-httpx==0.27.2
-idna==3.10
-ipykernel==6.30.1
-ipython==9.5.0
-ipython_pygments_lexers==1.1.1
-ipywidgets==8.1.7
-iso8601==2.1.0
-isoduration==20.11.0
-jedi==0.19.2
-Jinja2==3.1.6
-json5==0.12.1
-jsonpointer==3.0.0
-jsonschema==4.25.1
-jsonschema-specifications==2025.4.1
-jupyter==1.1.1
-jupyter-console==6.6.3
-jupyter-events==0.12.0
-jupyter-lsp==2.3.0
-jupyter_client==8.6.3
-jupyter_core==5.8.1
-jupyter_server==2.17.0
-jupyter_server_terminals==0.5.3
-jupyterlab==4.4.7
-jupyterlab_pygments==0.3.0
-jupyterlab_server==2.27.3
-jupyterlab_widgets==3.0.15
-kiwisolver==1.4.9
-lark==1.2.2
-lazy_loader==0.4
-MarkupSafe==3.0.2
-matplotlib==3.10.6
-matplotlib-inline==0.1.7
-mistune==3.1.4
-mne==1.10.2
-mpmath==1.3.0
-nbclient==0.10.2
-nbconvert==7.16.6
-nbformat==5.10.4
-nest-asyncio==1.6.0
-notebook==7.4.5
-notebook_shim==0.2.4
-numpy==2.3.2
-packaging==25.0
-pandas==2.3.3
-pandocfilters==1.5.1
-parso==0.8.5
-pexpect==4.9.0
-pillow==11.3.0
-platformdirs==4.4.0
-pooch==1.8.2
-prometheus_client==0.22.1
-prompt_toolkit==3.0.52
-proto-plus==1.26.1
-protobuf==5.29.5
-psutil==7.0.0
-ptyprocess==0.7.0
-pure_eval==0.2.3
-pyasn1==0.6.1
-pyasn1_modules==0.4.2
-pycparser==2.22
-pydantic==2.9.2
-pydantic-settings==2.6.0
-pydantic_core==2.23.4
-Pygments==2.19.2
-pyparsing==3.2.3
-PyQt6==6.10.0
-PyQt6-Qt6==6.10.0
-PyQt6_sip==13.10.2
-pyqtgraph==0.13.7
-pyserial==3.5
-PySide6==6.10.0
-PySide6_Addons==6.10.0
-PySide6_Essentials==6.10.0
-python-dateutil==2.9.0.post0
-python-dotenv==1.0.1
-python-json-logger==3.3.0
-pytz==2025.2
-PyYAML==6.0.2
-pyzmq==27.0.2
-referencing==0.36.2
-requests==2.32.5
-rfc3339-validator==0.1.4
-rfc3986-validator==0.1.1
-rfc3987-syntax==1.1.0
-rpds-py==0.27.1
-rsa==4.9.1
-scipy==1.16.3
-Send2Trash==1.8.3
-serial==0.0.97
-setuptools==80.9.0
-shiboken6==6.10.0
-six==1.17.0
-sniffio==1.3.1
-soupsieve==2.8
-stack-data==0.6.3
-starlette==0.38.6
-sympy==1.14.0
-terminado==0.18.1
-tinycss2==1.4.0
-tornado==6.5.2
-tqdm==4.67.1
-traitlets==5.14.3
-types-python-dateutil==2.9.0.20250822
-typing_extensions==4.15.0
-tzdata==2025.2
-uri-template==1.3.0
-uritemplate==4.2.0
-urllib3==2.5.0
-uvicorn==0.32.0
-wcwidth==0.2.13
-webcolors==24.11.1
-webencodings==0.5.1
-websocket-client==1.8.0
-websockets==15.0.1
-widgetsnbextension==4.0.14

RL Testing/temp.json

@@ -0,0 +1,11 @@
+{
+  "height_kp": 6.387288593598476,
+  "height_ki": 3.8692474098125285,
+  "height_kd": 8.326592570264028,
+  "roll_kp": 2.5896160034319817,
+  "roll_ki": 0.26391970508131646,
+  "roll_kd": 0.10133339760273202,
+  "pitch_kp": 0.5612446354489615,
+  "pitch_ki": 0.014113041384510265,
+  "pitch_kd": 1.100684970603034
+}