built environment
This commit is contained in:
176
RL Testing/ENV_INTEGRATION.md
Normal file
176
RL Testing/ENV_INTEGRATION.md
Normal file
@@ -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
|
||||
208
RL Testing/ENV_UPDATE.md
Normal file
208
RL Testing/ENV_UPDATE.md
Normal file
@@ -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
|
||||
@@ -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": {
|
||||
|
||||
111
RL Testing/find_equilibrium.py
Normal file
111
RL Testing/find_equilibrium.py
Normal file
@@ -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.")
|
||||
1
RL Testing/lev_PPO.py
Normal file
1
RL Testing/lev_PPO.py
Normal file
@@ -0,0 +1 @@
|
||||
import gymnasium as gym
|
||||
446
RL Testing/lev_pod_env.py
Normal file
446
RL Testing/lev_pod_env.py
Normal file
@@ -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
|
||||
23
RL Testing/mag_lev_coil.py
Normal file
23
RL Testing/mag_lev_coil.py
Normal file
@@ -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
|
||||
@@ -1,6 +0,0 @@
|
||||
from maglev_predictor import MaglevPredictor
|
||||
class MaglevSim:
|
||||
def __init__(self):
|
||||
self.pred = MaglevPredictor()
|
||||
|
||||
|
||||
68
RL Testing/pod.xml
Normal file
68
RL Testing/pod.xml
Normal file
@@ -0,0 +1,68 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="lev_pod">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<mass value="5.8"/>
|
||||
<inertia ixx="0.0192942414" ixy="0.0" ixz="0.0" iyy="0.130582305" iyz="0.0" izz="0.13760599326"/>
|
||||
</inertial>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.009525"/>
|
||||
<geometry> <box size="0.6096 0.0862 0.01905"/> </geometry>
|
||||
</collision>
|
||||
|
||||
<!-- Bolts -->
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.285 0.03 0.09585"/>
|
||||
<geometry><box size="0.01 0.01 0.01"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.285 -0.03 0.09585"/>
|
||||
<geometry><box size="0.01 0.01 0.01"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.285 0.03 0.09585"/>
|
||||
<geometry><box size="0.01 0.01 0.01"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.285 -0.03 0.09585"/>
|
||||
<geometry><box size="0.01 0.01 0.01"/></geometry>
|
||||
</collision>
|
||||
|
||||
<!-- Yoke Tops -->
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.1259 0.0508 0.08585"/>
|
||||
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.1259 -0.0508 0.08585"/>
|
||||
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.1259 0.0508 0.08585"/>
|
||||
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.1259 -0.0508 0.08585"/>
|
||||
<geometry><box size="0.0254 0.0254 0.01"/></geometry>
|
||||
</collision>
|
||||
|
||||
<!-- Sensor Tops -->
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0.0508 0.08585"/>
|
||||
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 -0.0508 0.08585"/>
|
||||
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.2366 0 0.08585"/>
|
||||
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="-0.2366 0 0.08585"/>
|
||||
<geometry><cylinder length="0.01" radius="0.015"/></geometry>
|
||||
</collision>
|
||||
</link>
|
||||
</robot>
|
||||
59
RL Testing/test_env.py
Normal file
59
RL Testing/test_env.py
Normal file
@@ -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()
|
||||
241
RL Testing/visualize_urdf.py
Normal file
241
RL Testing/visualize_urdf.py
Normal file
@@ -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.")
|
||||
Reference in New Issue
Block a user