I guess it kinda works ish

.gitignore

@@ -4,3 +4,4 @@ __pycache__/
 sim_results/
 sim_results_multi/
 tuningTrials/
+RL_Trials/

RL Testing/ENV_INTEGRATION.md

@@ -1,176 +0,0 @@
-# LevPodEnv Integration Summary
-
-## Overview
-`LevPodEnv` now fully interfaces with PyBullet simulation and uses the `maglev_predictor` to apply electromagnetic forces based on real-time gap heights and coil currents.
-
-## Architecture
-
-### System Configuration (From pod.xml and visualize_urdf.py)
-- **Inverted Maglev System**: Pod hangs BELOW track (like a monorail)
-- **Track**: Bottom surface at Z=0, 2m × 0.4m × 0.02m
-- **Pod Mass**: 5.8 kg (from pod.xml inertial)
-- **Yoke Positions** (local coordinates):
-  - Front Right: (+0.1259m, +0.0508m, +0.08585m)
-  - Front Left: (+0.1259m, -0.0508m, +0.08585m)
-  - Back Right: (-0.1259m, +0.0508m, +0.08585m)
-  - Back Left: (-0.1259m, -0.0508m, +0.08585m)
-- **Y-axis distance between left/right**: 0.1016m
-
-### Coil Configuration
-- **Two Coils**: 
-  - `coilL`: Left side (+Y), controls all +Y yokes
-  - `coilR`: Right side (-Y), controls all -Y yokes
-- **Parameters** (preserved from original):
-  - Resistance: 1.1Ω
-  - Inductance: 0.0025H (2.5mH)
-  - Source Voltage: 12V
-  - Max Current: 10.2A
-
-## Action Space
-- **Type**: Box(2)
-- **Range**: [-1, 1] for each coil
-- **Mapping**:
-  - `action[0]`: PWM duty cycle for left coil (+Y side)
-  - `action[1]`: PWM duty cycle for right coil (-Y side)
-
-## Observation Space
-- **Type**: Box(5)
-- **Components**:
-  1. `avg_gap_front` (m): Average gap height of front left & right yokes
-  2. `avg_gap_back` (m): Average gap height of back left & right yokes
-  3. `roll` (rad): Roll angle about X-axis (calculated from yoke Z positions)
-  4. `roll_rate` (rad/s): Angular velocity about X-axis
-  5. `z_velocity` (m/s): Vertical velocity
-
-## Physics Pipeline (per timestep)
-
-### 1. Coil Current Update
-```python
-currL = self.coilL.update(pwm_L, dt)  # First-order RL circuit model
-currR = self.coilR.update(pwm_R, dt)
-```
-
-### 2. Gap Height Calculation
-For each of 4 yokes:
-- Transform local position to world coordinates using rotation matrix
-- Add 5mm (half-height of 10mm yoke box) to get top surface
-- Gap height = -yoke_top_z (track at Z=0, yoke below)
-- Separate into front and back averages
-
-### 3. Roll Angle Calculation
-```python
-roll = arctan2((right_z_avg - left_z_avg) / y_distance)
-```
-- Uses Z-position difference between left (+Y) and right (-Y) yokes
-- Y-distance = 0.1016m (distance between yoke centerlines)
-
-### 4. Force/Torque Prediction
-```python
-# Convert to Ansys convention (negative currents)
-currL_ansys = -abs(currL)
-currR_ansys = -abs(currR)
-
-# Predict for front and back independently
-force_front, torque_front = predictor.predict(currL_ansys, currR_ansys, roll_deg, gap_front_mm)
-force_back, torque_back = predictor.predict(currL_ansys, currR_ansys, roll_deg, gap_back_mm)
-```
-
-### 5. Force Application
-- **Front Force**: Applied at [+0.1259, 0, 0.08585] in local frame
-- **Back Force**: Applied at [-0.1259, 0, 0.08585] in local frame
-- **Roll Torque**: Average of front/back torques, applied about X-axis
-  - Converted from mN·m to N·m: `torque_Nm = avg_torque / 1000`
-
-### 6. Simulation Step
-```python
-p.stepSimulation()  # 240 Hz (dt = 1/240s)
-```
-
-## Reward Function
-```python
-reward = 1.0
-reward -= gap_error * 100      # Target: 10mm gap
-reward -= roll_error * 50      # Keep level
-reward -= z_vel_penalty * 10   # Minimize oscillation
-reward -= power * 0.01         # Efficiency
-```
-
-## Termination Conditions
-- Gap outside [2mm, 30mm] range
-- Roll angle exceeds ±10°
-
-## Info Dictionary
-Each step returns:
-```python
-{
-    'currL': float,          # Left coil current (A)
-    'currR': float,          # Right coil current (A)
-    'gap_front': float,      # Front average gap (m)
-    'gap_back': float,       # Back average gap (m)
-    'roll': float,           # Roll angle (rad)
-    'force_front': float,    # Front force prediction (N)
-    'force_back': float,     # Back force prediction (N)
-    'torque': float          # Average torque (mN·m)
-}
-```
-
-## Key Design Decisions
-
-### Why Two Coils Instead of Four?
-- Physical system has one coil per side (left/right)
-- Each coil's magnetic field affects both front and back yokes on that side
-- Simplifies control: differential current creates roll torque
-
-### Why Separate Front/Back Predictions?
-- Gap heights can differ due to pitch angle
-- More accurate force modeling
-- Allows pitch control if needed in future
-
-### Roll Angle from Yoke Positions
-As requested: `roll = arctan((right_z - left_z) / y_distance)`
-- Uses actual yoke Z positions in world frame
-- More accurate than quaternion-based roll (accounts for deformation)
-- Matches physical sensor measurements
-
-### Current Sign Convention
-- Coils produce positive current (0 to +10.2A)
-- Ansys model expects negative currents (-15A to 0A)
-- Conversion: `currL_ansys = -abs(currL)`
-
-## Usage Example
-
-```python
-from lev_pod_env import LevPodEnv
-
-# Create environment
-env = LevPodEnv(use_gui=True)  # Set False for training
-
-# Reset
-obs, info = env.reset()
-# obs = [gap_front, gap_back, roll, roll_rate, z_vel]
-
-# Step
-action = [0.5, 0.5]  # 50% PWM on both coils
-obs, reward, terminated, truncated, info = env.step(action)
-
-# Check results
-print(f"Gaps: {info['gap_front']*1000:.2f}mm, {info['gap_back']*1000:.2f}mm")
-print(f"Forces: {info['force_front']:.2f}N, {info['force_back']:.2f}N")
-print(f"Currents: {info['currL']:.2f}A, {info['currR']:.2f}A")
-
-env.close()
-```
-
-## Testing
-Run `test_env.py` to verify integration:
-```bash
-cd "/Users/adipu/Documents/lev_control_4pt_small/RL Testing"
-/opt/miniconda3/envs/RLenv/bin/python test_env.py
-```
-
-## Next Steps for RL Training
-1. Test environment with random actions (test_env.py)
-2. Verify force magnitudes are reasonable (should see ~50-100N upward)
-3. Check that roll control works (differential currents produce torque)
-4. Train RL agent (PPO, SAC, or TD3 recommended)
-5. Tune reward function weights based on training results

RL Testing/ENV_UPDATE.md

@@ -1,208 +0,0 @@
-# Updated LevPodEnv - Physical System Clarification
-
-## System Architecture
-
-### Physical Configuration
-
-**Two U-Shaped Magnetic Yokes:**
-- **Front Yoke**: Located at X = +0.1259m
-  - Has two ends: Left (+Y = +0.0508m) and Right (-Y = -0.0508m)
-  - Force is applied at center: X = +0.1259m, Y = 0m
-  
-- **Back Yoke**: Located at X = -0.1259m
-  - Has two ends: Left (+Y = +0.0508m) and Right (-Y = -0.0508m)
-  - Force is applied at center: X = -0.1259m, Y = 0m
-
-**Four Independent Coil Currents:**
-1. `curr_front_L`: Current around front yoke's left (+Y) end
-2. `curr_front_R`: Current around front yoke's right (-Y) end
-3. `curr_back_L`: Current around back yoke's left (+Y) end
-4. `curr_back_R`: Current around back yoke's right (-Y) end
-
-**Current Range:** -15A to +15A (from Ansys CSV data)
-- Negative current: Strengthens permanent magnet field → stronger attraction
-- Positive current: Weakens permanent magnet field → weaker attraction
-
-### Collision Geometry in URDF
-
-**Yoke Ends (4 boxes):** Represent the tips of the U-yokes where gap is measured
-- Front Left: (+0.1259m, +0.0508m, +0.08585m)
-- Front Right: (+0.1259m, -0.0508m, +0.08585m)
-- Back Left: (-0.1259m, +0.0508m, +0.08585m)
-- Back Right: (-0.1259m, -0.0508m, +0.08585m)
-
-**Sensors (4 cylinders):** Physical gap sensors at different locations
-- Center Right: (0m, +0.0508m, +0.08585m)
-- Center Left: (0m, -0.0508m, +0.08585m)
-- Front: (+0.2366m, 0m, +0.08585m)
-- Back: (-0.2366m, 0m, +0.08585m)
-
-## RL Environment Interface
-
-### Action Space
-**Type:** `Box(4)`, Range: [-1, 1]
-
-**Actions:** `[pwm_front_L, pwm_front_R, pwm_back_L, pwm_back_R]`
-- PWM duty cycles for the 4 independent coils
-- Converted to currents via RL circuit model: `di/dt = (V_pwm - I*R) / L`
-
-### Observation Space
-**Type:** `Box(4)`, Range: [-inf, inf]
-
-**Observations:** `[sensor_center_right, sensor_center_left, sensor_front, sensor_back]`
-- **Noisy sensor readings** (not direct yoke measurements)
-- Noise: Gaussian with σ = 0.1mm (0.0001m)
-- Agent must learn system dynamics from sensor data alone
-- Velocities not directly provided - agent can learn from temporal sequence if needed
-
-### Force Application Physics
-
-For each timestep:
-
-1. **Measure yoke end gap heights** (from 4 yoke collision boxes)
-2. **Average left/right ends** for each U-yoke:
-   - `avg_gap_front = (gap_front_L + gap_front_R) / 2`
-   - `avg_gap_back = (gap_back_L + gap_back_R) / 2`
-
-3. **Calculate roll angle** from yoke end positions:
-   ```python
-   roll_front = arctan((gap_right - gap_left) / y_distance)
-   roll_back = arctan((gap_right - gap_left) / y_distance)
-   roll = (roll_front + roll_back) / 2
-   ```
-
-4. **Predict forces** using maglev_predictor:
-   ```python
-   force_front, torque_front = predictor.predict(
-       curr_front_L, curr_front_R, roll_deg, gap_front_mm
-   )
-   force_back, torque_back = predictor.predict(
-       curr_back_L, curr_back_R, roll_deg, gap_back_mm
-   )
-   ```
-
-5. **Apply forces at Y=0** (center of each U-yoke):
-   - Front force at: `[+0.1259, 0, 0.08585]`
-   - Back force at: `[-0.1259, 0, 0.08585]`
-
-6. **Apply roll torques** from each yoke independently
-
-### Key Design Decisions
-
-**Why 4 actions instead of 2?**
-- Physical system has 4 independent electromagnets (one per yoke end)
-- Allows fine control of roll torque
-- Left/right current imbalance on each yoke creates torque
-
-**Why sensor observations instead of yoke measurements?**
-- Realistic: sensors are at different positions than yokes
-- Adds partial observability challenge
-- Agent must learn system dynamics to infer unmeasured states
-- Sensor noise simulates real measurement uncertainty
-
-**Why not include velocities in observation?**
-- Agent can learn velocities from temporal sequence (frame stacking)
-- Reduces observation dimensionality
-- Tests if agent can learn dynamic behavior from gap measurements alone
-
-**Current sign convention:**
-- No conversion needed - currents fed directly to predictor
-- Range: -15A to +15A (from Ansys model)
-- Coil RL circuit naturally produces currents in this range
-
-### Comparison with Original Design
-
-| Feature | Original | Updated |
-|---------|----------|---------|
-| **Actions** | 2 (left/right coils) | 4 (front_L, front_R, back_L, back_R) |
-| **Observations** | 5 (gaps, roll, velocities) | 4 (noisy sensor gaps) |
-| **Gap Measurement** | Direct yoke positions | Noisy sensor positions |
-| **Force Application** | Front & back yoke centers | Front & back yoke centers ✓ |
-| **Current Range** | Assumed negative only | -15A to +15A |
-| **Roll Calculation** | From yoke end heights | From yoke end heights ✓ |
-
-## Physics Pipeline (Per Timestep)
-
-1. **Action → Currents**
-   ```
-   PWM[4] → RL Circuit Model → Currents[4]
-   ```
-
-2. **State Measurement**
-   ```
-   Yoke End Positions[4] → Gap Heights[4] → Average per Yoke[2]
-   ```
-
-3. **Roll Calculation**
-   ```
-   (Gap_Right - Gap_Left) / Y_distance → Roll Angle
-   ```
-
-4. **Force Prediction**
-   ```
-   (currL, currR, roll, gap) → Maglev Predictor → (force, torque)
-   Applied separately for front and back yokes
-   ```
-
-5. **Force Application**
-   ```
-   Forces at Y=0 for each yoke + Roll torques
-   ```
-
-6. **Observation Generation**
-   ```
-   Sensor Positions[4] → Gap Heights[4] → Add Noise → Observation[4]
-   ```
-
-## Info Dictionary
-
-Each `env.step()` returns comprehensive diagnostics:
-
-```python
-{
-    'curr_front_L': float,      # Front left coil current (A)
-    'curr_front_R': float,      # Front right coil current (A)
-    'curr_back_L': float,       # Back left coil current (A)
-    'curr_back_R': float,       # Back right coil current (A)
-    'gap_front_yoke': float,    # Front yoke average gap (m)
-    'gap_back_yoke': float,     # Back yoke average gap (m)
-    'roll': float,              # Roll angle (rad)
-    'force_front': float,       # Front yoke force (N)
-    'force_back': float,        # Back yoke force (N)
-    'torque_front': float,      # Front yoke torque (mN·m)
-    'torque_back': float        # Back yoke torque (mN·m)
-}
-```
-
-## Testing
-
-Run the updated test script:
-```bash
-cd "/Users/adipu/Documents/lev_control_4pt_small/RL Testing"
-/opt/miniconda3/envs/RLenv/bin/python test_env.py
-```
-
-Expected behavior:
-- 4 sensors report gap heights with small noise variations
-- Yoke gaps (in info) match sensor gaps approximately
-- All 4 coils build up current over time (RL circuit dynamics)
-- Forces should be ~50-100N upward at 10mm gap with moderate currents
-- Pod should begin to levitate if forces overcome gravity (5.8kg × 9.81 = 56.898 N needed)
-
-## Next Steps for RL Training
-
-1. **Frame Stacking**: Use 3-5 consecutive observations to give agent velocity information
-   ```python
-   from stable_baselines3.common.vec_env import VecFrameStack
-   env = VecFrameStack(env, n_stack=4)
-   ```
-
-2. **Algorithm Selection**: PPO or SAC recommended
-   - PPO: Good for continuous control, stable training
-   - SAC: Better sample efficiency, handles stochastic dynamics
-
-3. **Reward Tuning**: Current reward weights may need adjustment based on training performance
-
-4. **Curriculum Learning**: Start with smaller gap errors, gradually increase difficulty
-
-5. **Domain Randomization**: Vary sensor noise, mass, etc. for robust policy

RL Testing/Use of AI.txt

@@ -0,0 +1,121 @@
+[1]. Tool: Gemini
+     Prompt:  Could you please set up my RL training environment to load the URDF file I created for my maglev pod and 
+              put a track above it? This should all be done in PyBullet simulation, using the maglev_predictor and 
+              mag_lev_coil files I have provided. The agent should have access to an observation space with 4 gap
+              heights and 4 gap height velocities from the sim, and should be able to output 4 PWM values as actions.
+     Output:
+        # --- 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)  # From your specs
+        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)
+
+        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)
+
+[2]. Tool: Gemini
+      Prompt: 
+      Output:
+        # 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)
+                            label = "Center_Right" if local_pos[1] > 0 else "Center_Left"
+                        else:  # Front/back sensors (Y ≈ 0)
+                            label = "Front" if local_pos[0] > 0 else "Back"
+                        self.sensor_labels.append(label)

RL Testing/lev_PPO.py

@@ -1 +0,0 @@
-import gymnasium as gym

RL Testing/lev_pod_env.py

@@ -10,28 +10,27 @@ 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):
+    def __init__(self, use_gui=False, initial_gap_mm=10.0, max_steps=2000):
         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
         
+        # 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 noisy sensor gap heights + 4 velocities
+        # Observation: 4 normalized noisy sensor gap heights + 4 normalized velocities
-        # [sensor_center_right, sensor_center_left, sensor_front, sensor_back,
+        # Gaps normalized by 0.030m, velocities by 0.1 m/s
-        #  vel_center_right, vel_center_left, vel_front, vel_back]
+        self.observation_space = spaces.Box(low=-5.0, high=5.0, shape=(8,), dtype=np.float32)
-        self.observation_space = spaces.Box(low=-np.inf, high=np.inf, shape=(8,), dtype=np.float32)
         
         # --- 2. Setup Physics & Actuators ---
         self.dt = 1./240.  # PyBullet default timestep
-        # Four coils: one for each end of the two U-shaped yokes
+        self.coil_front_L = MagLevCoil(1.1, 0.0025, 12, 10.2)
-        # Front yoke: left (+Y) and right (-Y) ends
-        # Back yoke: left (+Y) and right (-Y) ends
-        self.coil_front_L = MagLevCoil(1.1, 0.0025, 12, 10.2)  # From your specs
         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)
@@ -39,6 +38,10 @@ class LevPodEnv(gym.Env):
         # 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()
         
@@ -89,17 +92,23 @@ class LevPodEnv(gym.Env):
                          restitution=0.1,
                          physicsClientId=self.client)
         
-        # Load pod URDF - hangs below track
-        # Create modified URDF with gap-dependent bolt positions
         urdf_path = self._create_modified_urdf()
         
-        # Calculate starting position based on desired initial gap
+        # Determine start condition
-        # Pod center Z = -(90.85mm + initial_gap_mm) / 1000
+        if np.random.rand() > 0.5:
-        start_z = -(0.09085 + self.initial_gap_mm / 1000.0)
+            # 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 = []
@@ -135,23 +144,30 @@ class LevPodEnv(gym.Env):
                             label = "Front" if local_pos[0] > 0 else "Back"
                         self.sensor_labels.append(label)
         
-        # Reset coil states
         self.coil_front_L.current = 0
         self.coil_front_R.current = 0
         self.coil_back_L.current = 0
         self.coil_back_R.current = 0
         
-        # Reset velocity tracking
         self.prev_sensor_gaps = None
+        obs = self._get_obs(initial_reset=True)
+        self.current_step = 0
         
-        return self._get_obs(), {}
+        return obs, {}
 
     def step(self, action):
-        # --- 1. Update Coil Currents from PWM Actions ---
+        # Check if PyBullet connection is still active (GUI might be closed)
-        pwm_front_L = action[0]  # Front yoke, +Y end
+        try:
-        pwm_front_R = action[1]  # Front yoke, -Y end
+            p.getConnectionInfo(physicsClientId=self.client)
-        pwm_back_L = action[2]   # Back yoke, +Y end
+        except p.error:
-        pwm_back_R = action[3]   # Back yoke, -Y end
+            # 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)
@@ -190,34 +206,27 @@ class LevPodEnv(gym.Env):
         avg_gap_back = (yoke_gap_heights_dict.get('Back_Left', 0.010) + 
                        yoke_gap_heights_dict.get('Back_Right', 0.010)) / 2
         
-        # Calculate roll angle from yoke end positions
-        # Roll = arctan((right_z - left_z) / y_distance)
-        # Average front and back measurements
         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)
         
-        # Rigid body distances (hypotenuses - don't change with rotation)
+        # 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 (rotation about X-axis) - using arcsin since y_distance is the hypotenuse
+        # Roll angle
-        # When right side has larger gap (higher), roll is negative (right-side-up convention)
+        # 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
         
-        # Pitch angle (rotation about Y-axis) - using arcsin since x_distance is the hypotenuse
+        # When back has larger gap, pitch is positive
-        # When back has larger gap (higher), pitch is positive (nose-down convention)
         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
         
-        # --- 4. Predict Forces and Torques using Maglev Predictor ---
+        # Predict Forces and Torques using Maglev Predictor
-        # Currents can be positive or negative (as seen in CSV data)
-        # Positive weakens permanent magnet field, negative strengthens it
-        
         # Gap heights in mm
         gap_front_mm = avg_gap_front * 1000
         gap_back_mm = avg_gap_back * 1000
@@ -226,12 +235,12 @@ class LevPodEnv(gym.Env):
         roll_deg = np.degrees(roll_angle)
         
         # Predict force and torque for each U-shaped yoke
-        # Front yoke (at X = +0.1259m)
+        # Front yoke
         force_front, torque_front = self.predictor.predict(
             curr_front_L, curr_front_R, roll_deg, gap_front_mm
         )
         
-        # Back yoke (at X = -0.1259m)
+        # Back yoke
         force_back, torque_back = self.predictor.predict(
             curr_back_L, curr_back_R, roll_deg, gap_back_mm
         )
@@ -283,19 +292,21 @@ class LevPodEnv(gym.Env):
         
         # --- 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 (10mm = 0.010m), minimize roll, minimize power
+        # Goal: Hover at target gap (16.5mm), minimize roll/pitch, minimize power
-        target_gap = TARGET_GAP  # 10mm in meters
+        target_gap = TARGET_GAP  # 16.5mm in meters
         avg_gap = (avg_gap_front + avg_gap_back) / 2
         
         gap_error = abs(avg_gap - target_gap)
-        roll_error = abs(roll_angle)
-        pitch_error = abs(pitch_angle)
-        z_vel_penalty = abs(lin_vel[2])
         
         # Power dissipation (all 4 coils)
         power = (curr_front_L**2 * self.coil_front_L.R + 
@@ -303,31 +314,49 @@ class LevPodEnv(gym.Env):
                 curr_back_L**2 * self.coil_back_L.R +
                 curr_back_R**2 * self.coil_back_R.R)
         
-        # Reward function
+        # --- Improved Reward Function ---
-        reward = 1.0
+        # Use reward shaping with reasonable scales to enable learning
-        reward -= gap_error * 100  # Penalize gap error heavily
-        reward -= roll_error * 50   # Penalize roll (about X-axis)
-        reward -= pitch_error * 50  # Penalize pitch (about Y-axis)
-        reward -= z_vel_penalty * 10  # Penalize vertical motion
-        reward -= power * 0.01  # Small penalty for power usage
         
-        # --- 9. Check Termination ---
+        # 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
         
-        # Check if pod crashed or flew away
+        # Terminate if gap is too small (crash) or too large (lost)
-        if avg_gap < 0.002 or avg_gap > 0.030:  # Outside 2-30mm range
+        if avg_gap < 0.003 or avg_gap > 0.035:
             terminated = True
-            reward -= 100
+            reward = -10.0  # Failure penalty (scaled down)
             
-        # Check if pod rolled or pitched too much
+        # Terminate if orientation is too extreme
-        if abs(roll_angle) > np.radians(10):  # More than 10 degrees roll
+        if abs(roll_angle) > np.radians(15) or abs(pitch_angle) > np.radians(15):
             terminated = True
-            reward -= 50
+            reward = -10.0
             
-        if abs(pitch_angle) > np.radians(10):  # More than 10 degrees pitch
+        # Success bonus for stable hovering near target
-            terminated = True
+        if gap_error_mm < 1.0 and abs(np.degrees(roll_angle)) < 2.0 and abs(np.degrees(pitch_angle)) < 2.0:
-            reward -= 50
+            reward += 2.0  # Bonus for excellent control
         
         info = {
             'curr_front_L': curr_front_L,
@@ -345,7 +374,7 @@ class LevPodEnv(gym.Env):
         
         return obs, reward, terminated, truncated, info
 
-    def _get_obs(self):
+    def _get_obs(self, initial_reset=False):
         """
         Returns observation: [gaps(4), velocities(4)]
         Uses noisy sensor readings + computed velocities for microcontroller-friendly deployment
@@ -372,7 +401,8 @@ class LevPodEnv(gym.Env):
             
             # 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]] = noisy_gap
+            sensor_gap_heights[self.sensor_labels[i]] = gap_height
         
         # Pack sensor measurements in consistent order
         # [center_right, center_left, front, back]
@@ -384,7 +414,7 @@ class LevPodEnv(gym.Env):
         ], dtype=np.float32)
         
         # Compute velocities (d_gap/dt)
-        if self.prev_sensor_gaps is None:
+        if initial_reset or (self.prev_sensor_gaps is None):
             # First observation - no velocity information yet
             velocities = np.zeros(4, dtype=np.float32)
         else:
@@ -394,8 +424,12 @@ class LevPodEnv(gym.Env):
         # Store for next step
         self.prev_sensor_gaps = gaps.copy()
         
-        # Concatenate: [gaps, velocities]
+        # Normalize observations
-        obs = np.concatenate([gaps, velocities])
+        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
     
@@ -439,7 +473,10 @@ class LevPodEnv(gym.Env):
         return temp_urdf_path
 
     def close(self):
+        try:
             p.disconnect(physicsClientId=self.client)
+        except p.error:
+            pass  # Already disconnected
     
     def render(self):
         """Rendering is handled by PyBullet GUI mode"""

RL Testing/maglev_predictor.py

@@ -1,143 +1,116 @@
 """
 Magnetic Levitation Force and Torque Predictor
-Generated from polynomial regression model (degree 6)
+Optimized Inference using Pre-Trained Scikit-Learn Model
 
-Performance:
+This module loads a saved .pkl model (PolynomialFeatures + LinearRegression)
-  - Force R²: 0.999967
+and executes predictions using optimized NumPy vectorization for high-speed simulation.
-  - Torque R²: 0.999895
 
 Usage:
-    predictor = MaglevPredictor()
+    predictor = MaglevPredictor("maglev_model.pkl")
     force, torque = predictor.predict(currL=-15, currR=-15, roll=1.0, gap_height=10.0)
 """
 
 import numpy as np
-from itertools import combinations_with_replacement
+import joblib
+import os
 
 class MaglevPredictor:
-    def __init__(self):
+    def __init__(self, model_path='maglev_model.pkl'):
-        """Initialize the magnetic levitation force/torque predictor."""
-        self.degree = 6
-        self.n_features = 4  # currL, currR, roll, invGap
-
-        # Force model coefficients
-        self.force_intercept = -13.86435113666004
-        self.force_coef = np.array([0.02658759005681468, 0.34900946839988667, 0.34444750510058214, 1.2230036394034724e-06, 634.0439835577899, 0.02080813487961144, -0.0008293737853954218, 0.022629965277837213, -14.941724689144115, 0.021137542187404268, -0.022629909674164422, -14.83489069081782, -0.73648243491197, -5.086269823986137e-06, 1376.7215457108907, -0.00011531764430729397, 7.297141435376426e-05, 5.485819913861908e-06, -1.0521911302922329, 5.2347676613358446e-05, 4.3360048841353804e-11, 0.03267811924240215, -0.006301758632290206, -0.26462568844868134, -29.51081695655865, -7.215211603545774e-05, -5.4850515695552184e-06, -1.0708823138271693, -0.00639147818385613, 0.2646251877739306, -30.235017153007778, -1.876003092129465e-09, 25.797307227256603, -9.502166033167048e-06, -5340.1478288789385, 1.3321592142734318e-06, 3.215276208834439e-06, 1.2629338780878463e-05, 0.0017892414904374793, 3.0619588116120866e-06, -1.5533773078946211e-07, -0.0010714898932605993, 8.771394384172321e-05, 0.0045854174470491334, 20.746027467741882, 3.865546883474735e-06, 1.5529474017839107e-07, -0.0007041149350807824, -3.826159128179907e-05, -4.0568953404984676e-10, 0.706467757753534, -0.0025574385229880613, -0.03335549183229827, -21.337761439975633, 123.43467940890098, 7.760241165222226e-07, -1.2629374732231469e-05, 0.0008614797170750643, 8.013084143393412e-05, -0.004585417820981331, 21.139446361522204, 0.0025574386461146674, -0.02985744313710574, 21.337761466031953, 125.14914334885628, 0.029443521274208746, -1.4431913766257617e-09, -235.63989509335445, 5.908485145948768e-05, -2235.4096760821767, 3.111734585559134e-07, -1.8749393149164462e-07, -8.520634864339627e-08, -6.386995447682509e-05, -1.4911356771563078e-07, 1.7060130552692954e-07, -7.426223970874446e-06, 7.239854511453814e-07, 2.184161596530121e-05, -0.003435810422541666, -1.641718938572012e-07, 5.542233338928781e-13, 3.63858792575833e-05, 5.292425733216533e-07, 8.546867872594177e-05, 0.004557337090434387, -2.9680832541068014e-05, -0.0037951782802951306, 0.04771712476195161, -146.08679080513647, -1.237001185927511e-07, -1.706171968152148e-07, -1.3936677144776866e-05, 4.590760820377682e-07, -8.54684555038343e-05, 0.0016991681189930247, -4.5288217620509386e-12, -0.0003737298107907294, -1.625876064944437e-08, -7.59874998546124, 0.000665088457289631, 0.0963780640278282, 2.443530344020863, 126.64935400088703, 51.887534931799024, 2.3180072616924008e-07, 8.521306682496288e-08, -6.923360173516357e-05, 7.560505927983741e-07, -2.184142082528595e-05, 0.0031131690666277717, 2.9680844979562693e-05, -0.003729220721386678, -0.04771714589381877, -149.23564872315535, 0.0006654036357616189, -0.0963780608814031, 2.3836954373862396, -126.64935179224206, 52.62737504907853, -5.00320618268546e-12, -1.0417152979422686, -1.0339665214713118e-07, 920.5602713711495, -0.0003583524071162497, -596.231639923794, 2.976548785227351e-09, -6.226855475688353e-09, -3.717561014582316e-08, -3.1125642472318304e-06, -9.987900284613715e-09, -1.3295888834363723e-08, 2.2609761671787965e-06, 2.4095157868941897e-08, 2.5320743457513117e-06, 0.0002053267414218185, -7.040462435270456e-09, 1.1958917411902803e-08, 1.9188907920408838e-06, 6.364859927998623e-08, -3.365889105211295e-06, 1.0594078015402353e-05, -2.5936621739219845e-07, -7.222719128563426e-06, 9.687272945516079e-05, 0.008542285480039293, -1.0519471516090562e-08, -1.1958789514210366e-08, 2.1222848793911453e-06, 4.494913952157731e-08, -1.368860580441833e-11, -0.00012807995660391036, 5.7690230814699817e-08, -1.477740339117517e-05, -0.0002544987474469851, -0.012272472027901347, 8.828768538471365e-07, 0.0004458604644176356, 0.03857698340149728, 0.3404446128948046, 359.5009507741528, -8.497181624989025e-09, 1.3292961398292391e-08, 1.7031788637211775e-06, 7.004756952255775e-08, 3.3659403024799417e-06, 6.890026536654492e-05, -5.7663427810439316e-08, -1.4623648040501536e-05, 0.0002544970475037711, -0.0010592495918368677, 1.0484938357535611e-06, 1.11551434756052e-10, 0.00563700298358414, 6.777939510777681e-08, 22.92320865247384, -2.342015012568588e-05, -0.009273996980515453, -0.7408042760765321, -21.75687783472069, -359.2904707728828, 13.909419135663944, 6.161826604511589e-09, 3.7175375666720356e-08, -2.18264751694619e-06, 3.087993150074908e-08, -2.532056053716758e-06, 0.00020057671003725908, 2.5937900716144213e-07, -7.915630708232868e-06, -9.687398982230477e-05, -0.011576477637831827, 1.0210671073096478e-06, -0.0004458603903605418, 0.038287496092621776, -0.34044451889358196, 368.1499228308273, 2.3420170015442388e-05, -0.009233730397613549, 0.7408042665232676, -21.520948405309664, 359.29046253155633, 14.109330328817938, 0.00017232856282973358, 3.7743741378853946e-10, 7.422994835173395, 4.4725972783587505e-07, 400.58822663103115, 0.0007277763198683137, -131.51558882445943])
-
-        # Torque model coefficients  
-        self.torque_intercept = -2.130143560243341
-        self.torque_coef = np.array([-0.43082063184443115, 12.736757164929834, -13.08252780522135, -13.305640830162647, 44.15155732224114, -0.06392352126218698, -0.011560340466877903, 1.8386367631578204, -459.22646547585083, 0.09823332452981008, 1.8386366778415446, 472.3689699641128, 0.9121471971798599, -1074.954075326984, -174.3530855725946, 0.0013682631086306893, 0.00195323147528499, 0.01965729552466329, 2.0525264006051005, -0.002008794556361812, -0.007770222019582218, 0.4678349249240797, -0.1569777322000716, -25.27902517575704, -2679.628866357242, -0.0006939851572283426, 0.019657295060090646, -3.113696029777825, 0.18298184295583028, -25.279024891306417, 2538.618446083044, 0.6297471362593584, -13.210276906344967, 47007.171225763355, -335.5527948647765, -0.00014304959219657576, -1.6606984310385542e-05, -0.0001288652013009539, 0.00601442858932888, 1.2945205369874202e-05, -0.0005177649120771122, -0.03856858837487831, 0.005216673005071559, -0.6211725496051428, 90.62092408067234, 1.3225382957671172e-06, -0.0005177654090040562, 0.04226634605293933, -0.00022044202681437142, 0.24508905440265927, -4.916014084022514, -0.1729589768066897, -6.749482388553274, -864.2729715937271, 8596.591212657508, -1.655038732195635e-05, -0.0001288644846688669, -0.030326086023246823, -0.005117587667626822, -0.621172555106164, -75.00494387398975, -0.1729589774841953, 5.728917556637661, -864.2729717790494, -8155.426251781266, -0.0808900322736339, 174.6241758121025, 59.54805506962735, -91016.54919618473, -124.62007434011727, -1.2219265101975907e-05, -6.313054797146833e-06, 5.149063326825853e-07, 0.0011879009736119883, -1.095400741668584e-06, -5.842468624450703e-06, 0.0025831102861104682, 7.925225867211338e-05, -0.0017220993515252303, -0.40028230028630435, 1.708059588878541e-06, -7.89357554253911e-06, -0.00032317627449174324, 9.975362360137296e-05, 0.0072170441199936874, 0.5449500218329268, -0.0015381145174586663, -0.19907587927007642, 10.517966687101838, -597.1622745919876, 5.7685155923081766e-06, -5.842469306571729e-06, -0.0031387751925677776, -9.262773397722412e-05, 0.007217044114035787, -0.5411230562086828, -3.150811562946387e-05, -0.00016976667267920575, -2.397689451813074, 20.162839703934328, 0.03054125251259876, 6.090989540908742, 186.86477056827312, 5456.995941705915, 3652.133600981146, 1.2356778199773544e-05, 5.149040589458309e-07, -0.00032098590758522505, -9.50427686632338e-05, -0.0017220995243967252, 0.6561577426235301, -0.001538114514769262, 0.20395968177578583, 10.517966698254375, 498.3704223462871, -0.03069428826677889, 6.090989540411657, -177.12280909537836, 5456.995943453175, -3464.337190929767, -0.31967284320394906, 0.7246542873326733, -3769.4439964810213, 44.429029612632434, -39730.668497436134, -31.81356580437269, 2.0552249679894885e-07, -2.2809068411788758e-07, -1.3383481700657285e-07, 0.00016049741088863811, -5.645338205795269e-07, 2.603157440717041e-07, 1.2741259467929922e-05, 2.8978208774788072e-06, -9.343648642357039e-05, -0.003248922547916422, 3.8025689264031826e-09, 1.759924543875968e-07, 2.496429428333613e-05, 1.1286853833780697e-06, 4.5304974111815e-05, -0.015200557860030273, 2.7490249294714886e-06, -0.0008249120055054959, 0.023944368570084418, 1.2946941017176763, 5.921153842791682e-07, 1.7599268176127225e-07, -3.777398643478591e-05, -7.73167698753241e-08, 0.00016224414804355547, 0.001111905663689683, -1.8224227460450493e-08, -0.0011582951120296947, -0.02742388816863306, -1.837576623417497, 1.8148104558690648e-05, 0.025582355960430903, 1.8719555164625814, -7.937558866365559, 1385.1611007390925, 3.657347065200156e-07, 2.6031784727820195e-07, -1.3743697138579591e-05, -1.1634339003308014e-06, 4.530497474064532e-05, 0.017827373735151752, -1.8219452613266185e-08, 0.001031106311321306, -0.02742388829348119, 1.820203067929913, 1.4693896147832675e-05, 0.0020392437766894034, -0.005130454763727088, 13.585039595374047, -25.31279841776285, -0.0012892921066907093, -0.4274487957111246, -45.23013966729996, -1269.1925481953558, -17946.91153301551, 982.0037387495227, 1.4403133263840573e-07, -1.3383731811700272e-07, -0.00015877734983860137, -1.8463729247741867e-06, -9.343645731973993e-05, -0.0020059852415403867, 2.749025497905677e-06, 0.0010293765766675733, 0.023944368343614464, -2.1505766814479212, -4.805005990249356e-05, 0.025582355962653125, -1.892578794851199, -7.937558866690216, -1145.7281610448965, -0.0012892921109930455, 0.4357010899616349, -45.230139667527084, 1235.618203650514, -17946.91153617443, -931.5738949974703, 0.001988793818171075, 4.153241810706205, -3.712809616304644, 19150.57072348241, 19.562114898868295, -10765.823486744783, -6.8540018643406935])
-
-    def _polynomial_features(self, X):
         """
-        Generate polynomial features up to specified degree.
+        Initialize predictor by loading the pickle file and extracting
-        Mimics sklearn's PolynomialFeatures with include_bias=True.
+        raw matrices for fast inference.
-
-        Args:
-            X: numpy array of shape (n_samples, 4) with [currL, currR, roll, invGap]
-
-        Returns:
-            Polynomial features array
         """
-        n_samples = X.shape[0]
+        if not os.path.exists(model_path):
+            raise FileNotFoundError(f"Model file '{model_path}' not found. Please train and save the model first.")
 
-        # Start with bias term (column of ones)
+        print(f"Loading maglev model from {model_path}...")
-        features = [np.ones(n_samples)]
+        data = joblib.load(model_path)
         
-        # Add original features
+        # 1. Extract Scikit-Learn Objects
-        for i in range(self.n_features):
+        poly_transformer = data['poly_features']
-            features.append(X[:, i])
+        linear_model = data['model']
         
-        # Add polynomial combinations
+        # 2. Extract Raw Matrices for Speed (Bypasses sklearn overhead)
-        for deg in range(2, self.degree + 1):
+        # powers_: Matrix of shape (n_output_features, n_input_features)
-            for combo in combinations_with_replacement(range(self.n_features), deg):
+        # Represents the exponents for each term. e.g. x1^2 * x2^1
-                term = np.ones(n_samples)
+        self.powers = poly_transformer.powers_.T # Transpose for broadcasting
-                for idx in combo:
-                    term *= X[:, idx]
-                features.append(term)
         
-        return np.column_stack(features)
+        # 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):
         """
-        Predict force and torque for given operating conditions.
+        Fast single-sample prediction using pure NumPy.
         
         Args:
-            currL: Left coil current in Amps
+            currL, currR: Currents [A]
-            currR: Right coil current in Amps
+            roll: Roll angle [deg]
-            roll: Roll angle in degrees
+            gap_height: Gap [mm]
-            gap_height: Gap height in mm
             
         Returns:
-            tuple: (force [N], torque [mN·m])
+            (force [N], torque [mN·m])
         """
-        # Compute inverse gap (critical transformation!)
+        # 1. Pre-process Input (Must match training order: currL, currR, roll, invGap)
-        invGap = 1.0 / gap_height
+        # Clamp gap to avoid division by zero
+        safe_gap = max(gap_height, 1e-6)
+        invGap = 1.0 / safe_gap
         
-        # Create input array
+        # Input Vector: shape (4,)
-        X = np.array([[currL, currR, roll, invGap]])
+        x = np.array([currL, currR, roll, invGap])
         
-        # Generate polynomial features
+        # 2. Polynomial Expansion (Vectorized)
-        X_poly = self._polynomial_features(X)
+        # 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,)
         
-        # Compute predictions
+        # Note: This is equivalent to PolynomialFeatures.transform but 100x faster for single samples
-        force = self.force_intercept + np.dot(X_poly, self.force_coef)[0]
+        poly_features = np.prod(x[:, None] ** self.powers, axis=0)
-        torque = self.torque_intercept + np.dot(X_poly, self.torque_coef)[0]
         
-        return force, torque
+        # 3. Linear Prediction
+        # (2, n_terms) dot (n_terms,) -> (2,)
+        result = np.dot(self.coef, poly_features) + self.intercept
         
-    def predict_batch(self, currL_array, currR_array, roll_array, gap_height_array):
+        return result[0], result[1]
+
+    def predict_batch(self, currL, currR, roll, gap_height):
         """
-        Predict force and torque for multiple operating conditions.
+        Fast batch prediction for array inputs.
-
-        Args:
-            currL_array: Array of left coil currents [A]
-            currR_array: Array of right coil currents [A]
-            roll_array: Array of roll angles [deg]
-            gap_height_array: Array of gap heights [mm]
-
-        Returns:
-            tuple: (force_array [N], torque_array [mN·m])
         """
-        # Convert to numpy arrays
+        # 1. Pre-process Inputs
-        currL_array = np.asarray(currL_array)
+        gap_height = np.asarray(gap_height)
-        currR_array = np.asarray(currR_array)
+        invGap = 1.0 / np.maximum(gap_height, 1e-6)
-        roll_array = np.asarray(roll_array)
-        gap_height_array = np.asarray(gap_height_array)
         
-        # Compute inverse gaps
+        # Stack inputs: shape (N, 4)
-        invGap_array = 1.0 / gap_height_array
+        X = np.column_stack((currL, currR, roll, invGap))
         
-        # Stack into feature matrix
+        # 2. Polynomial Expansion
-        X = np.column_stack([currL_array, currR_array, roll_array, invGap_array])
+        # 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)
         
-        # Generate polynomial features
+        # 3. Linear Prediction
-        X_poly = self._polynomial_features(X)
+        # (N, n_terms) @ (n_terms, 2) + (2,)
-
+        result = np.dot(poly_features, self.coef.T) + self.intercept
-        # Compute predictions
-        force_array = self.force_intercept + np.dot(X_poly, self.force_coef)
-        torque_array = self.torque_intercept + np.dot(X_poly, self.torque_coef)
-
-        return force_array, torque_array
         
+        return result[:, 0], result[:, 1]
 
 if __name__ == "__main__":
-    # Example usage
+    # Test
-    predictor = MaglevPredictor()
+    try:
-
+        p = MaglevPredictor()
-    # Single prediction
+        f, t = p.predict(-15, -15, 1.0, 10.0)
-    force, torque = predictor.predict(currL=-15, currR=-15, roll=1.0, gap_height=10.0)
+        print(f"Force: {f:.3f}, Torque: {t:.3f}")
-    print(f"Single prediction:")
+    except FileNotFoundError as e:
-    print(f"  Force: {force:.2f} N")
+        print(e)
-    print(f"  Torque: {torque:.2f} mN·m")
-
-    # Batch prediction
-    currL = np.array([-15, -15, -10])
-    currR = np.array([-15, -10, -10])
-    roll = np.array([0, 0.5, 1.0])
-    gap = np.array([10, 12, 15])
-
-    forces, torques = predictor.predict_batch(currL, currR, roll, gap)
-    print(f"\nBatch prediction:")
-    for i in range(len(forces)):
-        print(f"  Condition {i+1}: Force={forces[i]:.2f} N, Torque={torques[i]:.2f} mN·m")

RL Testing/pod.xml

@@ -11,7 +11,7 @@
       <geometry> <box size="0.6096 0.0862 0.01905"/> </geometry>
     </collision>
     
-    <!-- Bolts -->
+    Bolts
     <collision>
       <origin rpy="0 0 0" xyz="0.285 0.03 0.09585"/>
       <geometry><box size="0.01 0.01 0.01"/></geometry>
@@ -29,6 +29,24 @@
       <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"/>

RL Testing/test_env.py

@@ -5,6 +5,7 @@ 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)
@@ -30,7 +31,7 @@ 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.95,0.95,-0.95,-0.95], dtype=np.float32)
+    action = np.array([0,0,0,0], dtype=np.float32)
     
     obs, reward, terminated, truncated, info = env.step(action)
     
@@ -52,6 +53,8 @@ for step in range(500):
         print(f"\nEpisode terminated at step {step}")
         break
     
+    time.sleep(0.01)
+
 print("\n" + "=" * 60)
 print("Test complete!")
 print("=" * 60)