diff --git a/RL Testing/ENV_INTEGRATION.md b/RL Testing/ENV_INTEGRATION.md new file mode 100644 index 0000000..2ac2af6 --- /dev/null +++ b/RL Testing/ENV_INTEGRATION.md @@ -0,0 +1,176 @@ +# 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 diff --git a/RL Testing/ENV_UPDATE.md b/RL Testing/ENV_UPDATE.md new file mode 100644 index 0000000..810cb10 --- /dev/null +++ b/RL Testing/ENV_UPDATE.md @@ -0,0 +1,208 @@ +# 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 diff --git a/RL Testing/Function Fitting.ipynb b/RL Testing/Function Fitting.ipynb index 3eaf2b6..9307e27 100644 --- a/RL Testing/Function Fitting.ipynb +++ b/RL Testing/Function Fitting.ipynb @@ -1922,6 +1922,106 @@ "print(\"✓ All tests passed! Standalone predictor matches original model perfectly.\")\n", "print(\"\\nThe standalone predictor is ready for integration into your simulator!\")" ] + }, + { + "cell_type": "markdown", + "id": "21babeb3", + "metadata": {}, + "source": [ + "### Now, let's find the equilibrium height for our pod, given mass of 5.8 kg. \n", + "\n", + "5.8 kg * 9.81 $m/s^2$ = 56.898 N" + ] + }, + { + "cell_type": "code", + "execution_count": 279, + "id": "badbc379", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "======================================================================\n", + "EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\n", + "======================================================================\n", + "Pod mass: 5.8 kg\n", + "Total weight: 56.898 N\n", + "Target force per yoke: 28.449 N\n", + "Parameters: currL = 0 A, currR = 0 A, roll = 0°\n", + "\n", + "using scipy.optimize.fsolve:\n", + " Gap: 16.491741 mm → Force: 28.449000 N\n", + "\n" + ] + } + ], + "source": [ + "# Find equilibrium gap height for 5.8 kg pod using polynomial root finding\n", + "import numpy as np\n", + "from maglev_predictor import MaglevPredictor\n", + "from scipy.optimize import fsolve\n", + "\n", + "# Initialize predictor\n", + "predictor = MaglevPredictor()\n", + "\n", + "# Target force for 5.8 kg pod (total force = weight)\n", + "# Since we have TWO yokes (front and back), each produces this force\n", + "target_force_per_yoke = 5.8 * 9.81 / 2 # 28.449 N per yoke\n", + "\n", + "print(\"=\" * 70)\n", + "print(\"EQUILIBRIUM GAP HEIGHT FINDER (Analytical Solution)\")\n", + "print(\"=\" * 70)\n", + "print(f\"Pod mass: 5.8 kg\")\n", + "print(f\"Total weight: {5.8 * 9.81:.3f} N\")\n", + "print(f\"Target force per yoke: {target_force_per_yoke:.3f} N\")\n", + "print(f\"Parameters: currL = 0 A, currR = 0 A, roll = 0°\")\n", + "print()\n", + "\n", + "# Method 2: Use scipy.optimize.fsolve for verification\n", + "def force_error(gap_height):\n", + " # Handle array input from fsolve (convert to scalar)\n", + " gap_height = float(np.atleast_1d(gap_height)[0])\n", + " force, _ = predictor.predict(currL=0, currR=0, roll=0, gap_height=gap_height)\n", + " return force - target_force_per_yoke\n", + "\n", + "# Try multiple initial guesses to find all solutions\n", + "initial_guesses = [8, 10, 15, 20, 25]\n", + "scipy_solutions = []\n", + "\n", + "print(\"using scipy.optimize.fsolve:\")\n", + "for guess in initial_guesses:\n", + " solution = fsolve(force_error, guess)[0]\n", + " if 5 <= solution <= 30: # Valid range\n", + " force, torque = predictor.predict(currL=0, currR=0, roll=0, gap_height=solution)\n", + " error = abs(force - target_force_per_yoke)\n", + " if error < 0.01: # Valid solution (within 10 mN)\n", + " scipy_solutions.append((solution, force))\n", + "\n", + "# Remove duplicates (solutions within 0.1 mm)\n", + "unique_solutions = []\n", + "for sol, force in scipy_solutions:\n", + " is_duplicate = False\n", + " for unique_sol, _ in unique_solutions:\n", + " if abs(sol - unique_sol) < 0.1:\n", + " is_duplicate = True\n", + " break\n", + " if not is_duplicate:\n", + " unique_solutions.append((sol, force))\n", + "\n", + "for gap_val, force in unique_solutions:\n", + " print(f\" Gap: {gap_val:.6f} mm → Force: {force:.6f} N\")\n", + "print()" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "3ec4bb72", + "metadata": {}, + "outputs": [], + "source": [] } ], "metadata": { diff --git a/RL Testing/find_equilibrium.py b/RL Testing/find_equilibrium.py new file mode 100644 index 0000000..373ac4b --- /dev/null +++ b/RL Testing/find_equilibrium.py @@ -0,0 +1,111 @@ +""" +Find equilibrium gap height for magnetic levitation system. + +Given: +- Pod mass: 5.8 kg +- Required force: 5.8 * 9.81 = 56.898 N +- All currents: 0 A +- Roll angle: 0 degrees + +Find: Gap height (mm) that produces this force +""" + +import numpy as np +from maglev_predictor import MaglevPredictor + +# Initialize predictor +predictor = MaglevPredictor() + +# Target force +target_force = 5.8 * 9.81 # 56.898 N + +print("=" * 70) +print("EQUILIBRIUM GAP HEIGHT FINDER") +print("=" * 70) +print(f"Target Force: {target_force:.3f} N (for 5.8 kg pod)") +print(f"Parameters: currL = 0 A, currR = 0 A, roll = 0°") +print() + +# Define objective function +def force_error(gap_height): + """Calculate difference between predicted force and target force.""" + force, _ = predictor.predict(currL=0, currR=0, roll=0, gap_height=gap_height) + return force - target_force + +# First, let's scan across gap heights to understand the behavior +print("Scanning gap heights from 5 to 30 mm...") +print("-" * 70) +gap_range = np.linspace(5, 30, 26) +forces = [] + +for gap in gap_range: + force, torque = predictor.predict(currL=0, currR=0, roll=0, gap_height=gap) + forces.append(force) + if abs(force - target_force) < 5: # Close to target + print(f"Gap: {gap:5.1f} mm → Force: {force:7.3f} N ← CLOSE!") + else: + print(f"Gap: {gap:5.1f} mm → Force: {force:7.3f} N") + +forces = np.array(forces) + +print() +print("-" * 70) + +# Check if target force is within the range +min_force = forces.min() +max_force = forces.max() + +if target_force < min_force or target_force > max_force: + print(f"⚠ WARNING: Target force {target_force:.3f} N is outside the range!") + print(f" Force range: {min_force:.3f} N to {max_force:.3f} N") + print(f" Cannot achieve equilibrium with 0A currents.") +else: + # Find the gap height using root finding + # Use brentq for robust bracketing (requires sign change) + + # Find bracketing interval + idx_above = np.where(forces > target_force)[0] + idx_below = np.where(forces < target_force)[0] + + if len(idx_above) > 0 and len(idx_below) > 0: + # Find the transition point + gap_low = gap_range[idx_below[-1]] + gap_high = gap_range[idx_above[0]] + + print(f"✓ Target force is achievable!") + print(f" Bracketing interval: {gap_low:.1f} mm to {gap_high:.1f} mm") + print() + + # Use simple bisection method for accurate root finding + tol = 1e-6 + while (gap_high - gap_low) > tol: + gap_mid = (gap_low + gap_high) / 2 + force_mid, _ = predictor.predict(currL=0, currR=0, roll=0, gap_height=gap_mid) + + if force_mid > target_force: + gap_low = gap_mid + else: + gap_high = gap_mid + + equilibrium_gap = (gap_low + gap_high) / 2 + + # Verify the result + final_force, final_torque = predictor.predict( + currL=0, currR=0, roll=0, gap_height=equilibrium_gap + ) + + print("=" * 70) + print("EQUILIBRIUM FOUND!") + print("=" * 70) + print(f"Equilibrium Gap Height: {equilibrium_gap:.6f} mm") + print(f"Predicted Force: {final_force:.6f} N") + print(f"Target Force: {target_force:.6f} N") + print(f"Error: {abs(final_force - target_force):.9f} N") + print(f"Torque at equilibrium: {final_torque:.6f} mN·m") + print() + print(f"✓ Pod will levitate at {equilibrium_gap:.3f} mm gap height") + print(f" with no current applied (permanent magnets only)") + print("=" * 70) + else: + print("⚠ Could not find bracketing interval for bisection.") + print(" Target force may not be achievable in the scanned range.") diff --git a/RL Testing/lev_PPO.py b/RL Testing/lev_PPO.py new file mode 100644 index 0000000..d892bfa --- /dev/null +++ b/RL Testing/lev_PPO.py @@ -0,0 +1 @@ +import gymnasium as gym \ No newline at end of file diff --git a/RL Testing/lev_pod_env.py b/RL Testing/lev_pod_env.py new file mode 100644 index 0000000..286dbe2 --- /dev/null +++ b/RL Testing/lev_pod_env.py @@ -0,0 +1,446 @@ +import gymnasium as gym +from gymnasium import spaces +import pybullet as p +import pybullet_data +import numpy as np +import os +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): + super(LevPodEnv, self).__init__() + + # Store initial gap height parameter + self.initial_gap_mm = initial_gap_mm + + # --- 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 + # [sensor_center_right, sensor_center_left, sensor_front, sensor_back, + # vel_center_right, vel_center_left, vel_front, vel_back] + 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 + # 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) + + # Sensor noise parameters + self.sensor_noise_std = 0.0001 # 0.1mm standard deviation + + # 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) + + # 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 + # Pod center Z = -(90.85mm + initial_gap_mm) / 1000 + start_z = -(0.09085 + self.initial_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) + + # 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) + + # 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 + + return self._get_obs(), {} + + def step(self, action): + # --- 1. Update Coil Currents from PWM Actions --- + pwm_front_L = action[0] # Front yoke, +Y end + pwm_front_R = action[1] # Front yoke, -Y end + pwm_back_L = action[2] # Back yoke, +Y end + pwm_back_R = action[3] # Back yoke, -Y end + + curr_front_L = self.coil_front_L.update(pwm_front_L, self.dt) + curr_front_R = self.coil_front_R.update(pwm_front_R, self.dt) + curr_back_L = self.coil_back_L.update(pwm_back_L, self.dt) + curr_back_R = self.coil_back_R.update(pwm_back_R, self.dt) + + # --- 2. Get Current Pod State --- + pos, orn = p.getBasePositionAndOrientation(self.podId, physicsClientId=self.client) + lin_vel, ang_vel = p.getBaseVelocity(self.podId, physicsClientId=self.client) + + # Convert quaternion to rotation matrix + rot_matrix = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3) + + # --- 3. Calculate Gap Heights at Yoke Positions (for force prediction) --- + # Calculate world positions of the 4 yokes (ends of U-yokes) + yoke_gap_heights_dict = {} # Store by label for easy access + + for i, yoke_idx in enumerate(self.yoke_indices): + local_pos = self.collision_local_positions[yoke_idx] + local_vec = np.array(local_pos) + world_offset = rot_matrix @ local_vec + world_pos = np.array(pos) + world_offset + + # Top surface of yoke box (add half-height = 5mm) + yoke_top_z = world_pos[2] + 0.005 + + # Gap height: track bottom (Z=0) to yoke top (negative Z) + gap_height = -yoke_top_z # Convert to positive gap in meters + yoke_gap_heights_dict[self.yoke_labels[i]] = gap_height + + # Average gap heights for each U-shaped yoke (average left and right ends) + # Front yoke: average of Front_Left and Front_Right + # Back yoke: average of Back_Left and Back_Right + avg_gap_front = (yoke_gap_heights_dict.get('Front_Left', 0.010) + + yoke_gap_heights_dict.get('Front_Right', 0.010)) / 2 + avg_gap_back = (yoke_gap_heights_dict.get('Back_Left', 0.010) + + yoke_gap_heights_dict.get('Back_Right', 0.010)) / 2 + + # 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) + 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 + # When right side has larger gap (higher), roll is negative (right-side-up convention) + 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 (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 --- + # 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 + + # Roll angle in degrees + roll_deg = np.degrees(roll_angle) + + # Predict force and torque for each U-shaped yoke + # Front yoke (at X = +0.1259m) + force_front, torque_front = self.predictor.predict( + curr_front_L, curr_front_R, roll_deg, gap_front_mm + ) + + # Back yoke (at X = -0.1259m) + force_back, torque_back = self.predictor.predict( + curr_back_L, curr_back_R, roll_deg, gap_back_mm + ) + + # --- 5. Apply Forces and Torques to Pod --- + # Forces are applied at Y=0 (center of U-yoke) at each X position + # This is where the actual magnetic force acts on the U-shaped yoke + + # Apply force at front yoke center (X=+0.1259, Y=0) + front_yoke_center = [0.1259, 0, 0.08585] # From pod.xml yoke positions + p.applyExternalForce( + self.podId, -1, + forceObj=[0, 0, force_front], + posObj=front_yoke_center, + flags=p.LINK_FRAME, + physicsClientId=self.client + ) + + # Apply force at back yoke center (X=-0.1259, Y=0) + back_yoke_center = [-0.1259, 0, 0.08585] + p.applyExternalForce( + self.podId, -1, + forceObj=[0, 0, force_back], + posObj=back_yoke_center, + flags=p.LINK_FRAME, + physicsClientId=self.client + ) + + + + # Apply roll torques + # Each yoke produces its own torque about X axis + torque_front_Nm = torque_front / 1000 # Convert from mN·m to N·m + torque_back_Nm = torque_back / 1000 + + # Apply torques at respective yoke positions + p.applyExternalTorque( + self.podId, -1, + torqueObj=[torque_front_Nm, 0, 0], + flags=p.LINK_FRAME, + physicsClientId=self.client + ) + p.applyExternalTorque( + self.podId, -1, + torqueObj=[torque_back_Nm, 0, 0], + flags=p.LINK_FRAME, + physicsClientId=self.client + ) + + # --- 6. Step Simulation --- + p.stepSimulation(physicsClientId=self.client) + + # --- 7. Get New Observation --- + obs = self._get_obs() + + # --- 8. Calculate Reward --- + # Goal: Hover at target gap (10mm = 0.010m), minimize roll, minimize power + target_gap = TARGET_GAP # 10mm 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 + + curr_front_R**2 * self.coil_front_R.R + + curr_back_L**2 * self.coil_back_L.R + + curr_back_R**2 * self.coil_back_R.R) + + # Reward function + reward = 1.0 + 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 --- + terminated = False + truncated = False + + # Check if pod crashed or flew away + if avg_gap < 0.002 or avg_gap > 0.030: # Outside 2-30mm range + terminated = True + reward -= 100 + + # Check if pod rolled or pitched too much + if abs(roll_angle) > np.radians(10): # More than 10 degrees roll + terminated = True + reward -= 50 + + if abs(pitch_angle) > np.radians(10): # More than 10 degrees pitch + terminated = True + reward -= 50 + + info = { + 'curr_front_L': curr_front_L, + 'curr_front_R': curr_front_R, + 'curr_back_L': curr_back_L, + 'curr_back_R': curr_back_R, + 'gap_front_yoke': avg_gap_front, + 'gap_back_yoke': avg_gap_back, + 'roll': roll_angle, + 'force_front': force_front, + 'force_back': force_back, + 'torque_front': torque_front, + 'torque_back': torque_back + } + + return obs, reward, terminated, truncated, info + + def _get_obs(self): + """ + Returns observation: [gaps(4), velocities(4)] + Uses noisy sensor readings + computed velocities for microcontroller-friendly deployment + """ + pos, orn = p.getBasePositionAndOrientation(self.podId, physicsClientId=self.client) + + # Convert quaternion to rotation matrix + rot_matrix = np.array(p.getMatrixFromQuaternion(orn)).reshape(3, 3) + + # Calculate sensor gap heights with noise + sensor_gap_heights = {} + + for i, sensor_idx in enumerate(self.sensor_indices): + local_pos = self.collision_local_positions[sensor_idx] + local_vec = np.array(local_pos) + world_offset = rot_matrix @ local_vec + world_pos = np.array(pos) + world_offset + + # Top surface of sensor (add half-height = 5mm) + sensor_top_z = world_pos[2] + 0.005 + + # Gap height: track bottom (Z=0) to sensor top + gap_height = -sensor_top_z + + # Add measurement noise + noisy_gap = gap_height + np.random.normal(0, self.sensor_noise_std) + sensor_gap_heights[self.sensor_labels[i]] = noisy_gap + + # Pack sensor measurements in consistent order + # [center_right, center_left, front, back] + gaps = np.array([ + sensor_gap_heights.get('Center_Right', 0.010), + sensor_gap_heights.get('Center_Left', 0.010), + sensor_gap_heights.get('Front', 0.010), + sensor_gap_heights.get('Back', 0.010) + ], dtype=np.float32) + + # Compute velocities (d_gap/dt) + if self.prev_sensor_gaps is None: + # First observation - no velocity information yet + velocities = np.zeros(4, dtype=np.float32) + else: + # Compute velocity as finite difference + velocities = (gaps - self.prev_sensor_gaps) / self.dt + + # Store for next step + self.prev_sensor_gaps = gaps.copy() + + # Concatenate: [gaps, velocities] + obs = np.concatenate([gaps, velocities]) + + return obs + + def _create_modified_urdf(self): + """ + Create a modified URDF with bolt positions adjusted based on initial gap height. + Bolts are at Z = 0.08585 + gap_mm/1000 (relative to pod origin). + Yokes and sensors remain at Z = 0.08585 (relative to pod origin). + """ + import tempfile + + # Calculate bolt Z position + bolt_z = 0.08585 + self.initial_gap_mm / 1000.0 + + # Read the original URDF template + urdf_template_path = os.path.join(os.path.dirname(__file__), "pod.xml") + with open(urdf_template_path, 'r') as f: + urdf_content = f.read() + + # Replace the bolt Z positions (originally at 0.09585) + # There are 4 bolts at different X,Y positions but same Z + urdf_modified = urdf_content.replace( + 'xyz="0.285 0.03 0.09585"', + f'xyz="0.285 0.03 {bolt_z:.6f}"' + ).replace( + 'xyz="0.285 -0.03 0.09585"', + f'xyz="0.285 -0.03 {bolt_z:.6f}"' + ).replace( + 'xyz="-0.285 0.03 0.09585"', + f'xyz="-0.285 0.03 {bolt_z:.6f}"' + ).replace( + 'xyz="-0.285 -0.03 0.09585"', + f'xyz="-0.285 -0.03 {bolt_z:.6f}"' + ) + + # Write to a temporary file + with tempfile.NamedTemporaryFile(mode='w', suffix='.urdf', delete=False) as f: + f.write(urdf_modified) + temp_urdf_path = f.name + + return temp_urdf_path + + def close(self): + p.disconnect(physicsClientId=self.client) + + def render(self): + """Rendering is handled by PyBullet GUI mode""" + pass \ No newline at end of file diff --git a/RL Testing/mag_lev_coil.py b/RL Testing/mag_lev_coil.py new file mode 100644 index 0000000..db4e7b8 --- /dev/null +++ b/RL Testing/mag_lev_coil.py @@ -0,0 +1,23 @@ +class MagLevCoil: + def __init__(self, r_resistance, l_inductance, source_voltage, maxCurrent): + self.R = r_resistance + self.L = l_inductance + self.current = 0.0 + self.Vs = source_voltage + self.Imax = maxCurrent + + def update(self, pwm_duty_cycle, dt): + """ + Simulates the coil circuit and force generation. + pwm_duty_cycle: -1.0 to 1.0 + """ + # Simple First-order RL circuit approximation + # V_in = Duty * V_source + voltage = pwm_duty_cycle * self.Vs + + # di/dt = (V - I*R) / L + di = (voltage - self.current * self.R) / self.L + self.current += di * dt + self.current = min(max(-self.Imax, self.current), self.Imax) + + return self.current \ No newline at end of file diff --git a/RL Testing/maglev_sim.py b/RL Testing/maglev_sim.py deleted file mode 100644 index 21c0584..0000000 --- a/RL Testing/maglev_sim.py +++ /dev/null @@ -1,6 +0,0 @@ -from maglev_predictor import MaglevPredictor -class MaglevSim: - def __init__(self): - self.pred = MaglevPredictor() - - diff --git a/RL Testing/pod.xml b/RL Testing/pod.xml new file mode 100644 index 0000000..fa0fc21 --- /dev/null +++ b/RL Testing/pod.xml @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/RL Testing/test_env.py b/RL Testing/test_env.py new file mode 100644 index 0000000..6e7e740 --- /dev/null +++ b/RL Testing/test_env.py @@ -0,0 +1,59 @@ +""" +Test script for LevPodEnv +Runs a simple episode with constant actions to verify the environment works +""" + +from lev_pod_env import LevPodEnv +import numpy as np + +# Create environment with GUI for visualization +env = LevPodEnv(use_gui=True, initial_gap_mm=15) + +print("=" * 60) +print("Testing LevPodEnv") +print("=" * 60) +print(f"Action space: {env.action_space}") +print(f" 4 PWM duty cycles: [front_L, front_R, back_L, back_R]") +print(f"Observation space: {env.observation_space}") +print(f" 8 values: [gaps(4), velocities(4)]") +print("=" * 60) + +# Reset environment +obs, info = env.reset() +print(f"\nInitial observation:") +print(f" Gaps: CR={obs[0]*1000:.2f}mm, CL={obs[1]*1000:.2f}mm, F={obs[2]*1000:.2f}mm, B={obs[3]*1000:.2f}mm") +print(f" Velocities: CR={obs[4]*1000:.2f}mm/s, CL={obs[5]*1000:.2f}mm/s, F={obs[6]*1000:.2f}mm/s, B={obs[7]*1000:.2f}mm/s") +print(f" Average gap: {np.mean(obs[:4])*1000:.2f} mm") + +# Run a few steps with constant action to test force application +print("\nRunning test episode...") +for step in range(500): + # Apply constant moderate PWM to all 4 coils + # 50% PWM should generate current that produces upward force + action = np.array([0.95,0.95,-0.95,-0.95], dtype=np.float32) + + obs, reward, terminated, truncated, info = env.step(action) + + if step % 5 == 0: + print(f"\nStep {step} (t={step/240:.2f}s):") + print(f" Sensor gaps: CR={obs[0]*1000:.2f}mm, CL={obs[1]*1000:.2f}mm, " + + f"F={obs[2]*1000:.2f}mm, B={obs[3]*1000:.2f}mm") + print(f" Velocities: CR={obs[4]*1000:.2f}mm/s, CL={obs[5]*1000:.2f}mm/s, " + + f"F={obs[6]*1000:.2f}mm/s, B={obs[7]*1000:.2f}mm/s") + print(f" Yoke gaps: front={info['gap_front_yoke']*1000:.2f}mm, back={info['gap_back_yoke']*1000:.2f}mm") + print(f" Roll: {np.degrees(info['roll']):.2f}°") + print(f" Currents: FL={info['curr_front_L']:.2f}A, FR={info['curr_front_R']:.2f}A, " + + f"BL={info['curr_back_L']:.2f}A, BR={info['curr_back_R']:.2f}A") + print(f" Forces: front={info['force_front']:.2f}N, back={info['force_back']:.2f}N") + print(f" Torques: front={info['torque_front']:.2f}mN·m, back={info['torque_back']:.2f}mN·m") + print(f" Reward: {reward:.2f}") + + if terminated or truncated: + print(f"\nEpisode terminated at step {step}") + break + +print("\n" + "=" * 60) +print("Test complete!") +print("=" * 60) + +env.close() \ No newline at end of file diff --git a/RL Testing/visualize_urdf.py b/RL Testing/visualize_urdf.py new file mode 100644 index 0000000..976864e --- /dev/null +++ b/RL Testing/visualize_urdf.py @@ -0,0 +1,241 @@ +""" +URDF Structure Visualizer for Lev Pod using PyBullet +Loads and displays the pod.urdf file in PyBullet's GUI +""" + +import pybullet as p +import pybullet_data +import time +import os + +# Initialize PyBullet in GUI mode +physicsClient = p.connect(p.GUI) + +# Set up the simulation environment +p.setAdditionalSearchPath(pybullet_data.getDataPath()) +p.setGravity(0, 0, 0) + +# Configure camera view - looking at inverted maglev system +p.resetDebugVisualizerCamera( + cameraDistance=0.5, + cameraYaw=45, + cameraPitch=1, # Look up at the hanging pod + cameraTargetPosition=[0, 0, 0] +) + +# Create the maglev track with collision physics (ABOVE, like a monorail) +# The track BOTTOM surface is at Z=0, pod hangs below +track_collision = p.createCollisionShape( + shapeType=p.GEOM_BOX, + halfExtents=[1.0, 0.2, 0.010] # 2m long × 0.4m wide × 2cm thick +) +track_visual = p.createVisualShape( + shapeType=p.GEOM_BOX, + halfExtents=[1.0, 0.2, 0.010], + rgbaColor=[0.3, 0.3, 0.3, 0.8] # Gray, semi-transparent +) +trackId = p.createMultiBody( + baseMass=0, # Static object + baseCollisionShapeIndex=track_collision, + baseVisualShapeIndex=track_visual, + basePosition=[0, 0, 0.010] # Track bottom at Z=0, center at Z=10mm +) +# Set track surface properties (steel) +p.changeDynamics(trackId, -1, + lateralFriction=0.3, # Steel-on-steel + restitution=0.1) # Minimal bounce + +# Load the lev pod URDF +urdf_path = "pod.xml" +if not os.path.exists(urdf_path): + print(f"Error: Could not find {urdf_path}") + print(f"Current directory: {os.getcwd()}") + p.disconnect() + exit(1) + +# INVERTED SYSTEM: Pod hangs BELOW track +# Track bottom is at Z=0 +# Gap height = distance from track bottom DOWN to magnetic yoke (top of pod body) +# URDF collision spheres at +25mm from center = bolts that contact track from below +# URDF box top at +25mm from center = magnetic yoke +# +# For 10mm gap: +# - Yoke (top of pod at +25mm from center) should be at Z = 0 - 10mm = -10mm +# - Therefore: pod center = -10mm - 25mm = -35mm +# - Bolts (at +25mm from center) end up at: -35mm + 25mm = -10mm (touching track at Z=0) +start_pos = [0, 0, -0.10085] # Pod center 100.85mm BELOW track → yoke at 10mm gap +start_orientation = p.getQuaternionFromEuler([0, 0, 0]) +podId = p.loadURDF(urdf_path, start_pos, start_orientation) + +print("\n" + "=" * 60) +print("PyBullet URDF Visualizer") +print("=" * 60) +print(f"Loaded: {urdf_path}") +print(f"Position: {start_pos}") +print("\nControls:") +print(" • Mouse: Rotate view (left drag), Pan (right drag), Zoom (scroll)") +print(" • Ctrl+Mouse: Apply forces to the pod") +print(" • Press ESC or close window to exit") +print("=" * 60) + +# Get and display URDF information +num_joints = p.getNumJoints(podId) +print(f"\nNumber of joints: {num_joints}") + +# Get base info +base_mass, base_lateral_friction = p.getDynamicsInfo(podId, -1)[:2] +print(f"\nBase link:") +print(f" Mass: {base_mass} kg") +print(f" Lateral friction: {base_lateral_friction}") + +# Get collision shape info +collision_shapes = p.getCollisionShapeData(podId, -1) +print(f"\nCollision shapes: {len(collision_shapes)}") +for i, shape in enumerate(collision_shapes): + shape_type = shape[2] + dimensions = shape[3] + local_pos = shape[5] + shape_names = { + p.GEOM_BOX: "Box", + p.GEOM_SPHERE: "Sphere", + p.GEOM_CAPSULE: "Capsule", + p.GEOM_CYLINDER: "Cylinder", + p.GEOM_MESH: "Mesh" + } + print(f" Shape {i}: {shape_names.get(shape_type, 'Unknown')}") + print(f" Dimensions: {dimensions}") + print(f" Position: {local_pos}") + +# Enable visualization of collision shapes +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) +p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) +p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0) + +# Add coordinate frame visualization at the pod's origin +axis_length = 0.1 +p.addUserDebugLine([0, 0, 0], [axis_length, 0, 0], [1, 0, 0], lineWidth=3, parentObjectUniqueId=podId, parentLinkIndex=-1) +p.addUserDebugLine([0, 0, 0], [0, axis_length, 0], [0, 1, 0], lineWidth=3, parentObjectUniqueId=podId, parentLinkIndex=-1) +p.addUserDebugLine([0, 0, 0], [0, 0, axis_length], [0, 0, 1], lineWidth=3, parentObjectUniqueId=podId, parentLinkIndex=-1) + +print("\n" + "=" * 60) +print("Visualization is running. Interact with the viewer...") +print("Close the PyBullet window to exit.") +print("=" * 60 + "\n") + +# Store collision shape local positions and types for tracking +collision_local_positions = [] +collision_types = [] +for shape in collision_shapes: + collision_local_positions.append(shape[5]) # Local position (x, y, z) + collision_types.append(shape[2]) # Shape type (BOX, CYLINDER, etc.) + +# Identify the 4 yoke top collision boxes and 4 sensor cylinders +# Both are at Z ≈ 0.08585m, but yokes are BOXES and sensors are CYLINDERS +yoke_indices = [] +yoke_labels = [] +sensor_indices = [] +sensor_labels = [] + +for i, (local_pos, shape_type) in enumerate(zip(collision_local_positions, collision_types)): + # Check if at sensor/yoke height (Z ≈ 0.08585m) + if abs(local_pos[2] - 0.08585) < 0.001: # Within 1mm tolerance + if shape_type == p.GEOM_BOX: + # Yoke collision boxes (BOX shapes) + yoke_indices.append(i) + x_pos = "Front" if local_pos[0] > 0 else "Back" + y_pos = "Right" if local_pos[1] > 0 else "Left" + yoke_labels.append(f"{x_pos} {y_pos}") + elif shape_type == p.GEOM_CYLINDER or shape_type == p.GEOM_MESH: + # Sensor cylinders (may be loaded as MESH by PyBullet) + # Distinguish from yokes by X or Y position patterns + # Yokes have both X and Y non-zero, sensors have one coordinate near zero + if abs(local_pos[0]) < 0.06 or abs(local_pos[1]) < 0.02: + sensor_indices.append(i) + # Label sensors by position + if abs(local_pos[0]) < 0.001: # X ≈ 0 (center sensors) + label = "Center Right" if local_pos[1] > 0 else "Center Left" + else: + label = "Front" if local_pos[0] > 0 else "Back" + sensor_labels.append(label) + +print(f"\nIdentified {len(yoke_indices)} yoke collision boxes for gap height tracking") +print(f"Identified {len(sensor_indices)} sensor cylinders for gap height tracking") + +# Run the simulation with position tracking +import numpy as np +step_count = 0 +print_interval = 240 # Print every second (at 240 Hz) + +try: + while p.isConnected(): + p.stepSimulation() + + # Extract positions periodically + if step_count % print_interval == 0: + # Get pod base position and orientation + pos, orn = p.getBasePositionAndOrientation(podId) + + # Convert quaternion to rotation matrix + rot_matrix = p.getMatrixFromQuaternion(orn) + rot_matrix = np.array(rot_matrix).reshape(3, 3) + + # Calculate world positions of yoke tops and sensors + print(f"\n--- Time: {step_count/240:.2f}s ---") + print(f"Pod center: [{pos[0]*1000:.1f}, {pos[1]*1000:.1f}, {pos[2]*1000:.1f}] mm") + + print("\nYoke Gap Heights:") + yoke_gap_heights = [] + for i, yoke_idx in enumerate(yoke_indices): + local_pos = collision_local_positions[yoke_idx] + + # Transform local position to world coordinates + local_vec = np.array(local_pos) + world_offset = rot_matrix @ local_vec + world_pos = np.array(pos) + world_offset + + # Add 0.005m (5mm) to get top surface of yoke box (half-height of 10mm box) + yoke_top_z = world_pos[2] + 0.005 + + # Gap height: distance from track bottom (Z=0) down to yoke top + gap_height = 0.0 - yoke_top_z # Negative means below track + gap_height_mm = gap_height * 1000 + yoke_gap_heights.append(gap_height_mm) + + print(f" {yoke_labels[i]} yoke: pos=[{world_pos[0]*1000:.1f}, {world_pos[1]*1000:.1f}, {yoke_top_z*1000:.1f}] mm | Gap: {gap_height_mm:.2f} mm") + + # Calculate average yoke gap height (for Ansys model input) + avg_yoke_gap = np.mean(yoke_gap_heights) + print(f" Average yoke gap: {avg_yoke_gap:.2f} mm") + + print("\nSensor Gap Heights:") + sensor_gap_heights = [] + for i, sensor_idx in enumerate(sensor_indices): + local_pos = collision_local_positions[sensor_idx] + + # Transform local position to world coordinates + local_vec = np.array(local_pos) + world_offset = rot_matrix @ local_vec + world_pos = np.array(pos) + world_offset + + # Add 0.005m (5mm) to get top surface of cylinder (half-length of 10mm cylinder) + sensor_top_z = world_pos[2] + 0.005 + + # Gap height: distance from track bottom (Z=0) down to sensor top + gap_height = 0.0 - sensor_top_z + gap_height_mm = gap_height * 1000 + sensor_gap_heights.append(gap_height_mm) + + print(f" {sensor_labels[i]} sensor: pos=[{world_pos[0]*1000:.1f}, {world_pos[1]*1000:.1f}, {sensor_top_z*1000:.1f}] mm | Gap: {gap_height_mm:.2f} mm") + + # Calculate average sensor gap height + avg_sensor_gap = np.mean(sensor_gap_heights) + print(f" Average sensor gap: {avg_sensor_gap:.2f} mm") + + step_count += 1 + time.sleep(1./240.) # Run at 240 Hz + +except KeyboardInterrupt: + print("\nExiting...") + +p.disconnect() +print("PyBullet session closed.")