Added more noise simulation and a multi-simulate file that auto-runs multiple simulations with a desired noise level.
This commit is contained in:
@@ -1,7 +1,10 @@
|
||||
# How To Use
|
||||
## Running the Simulation
|
||||
Run ```pip install -r requirements.txt``` followed by ```python topSimulate.py```. Or, if your environment is already set up, just run the python file.
|
||||
Generated files will be saved to ```sim_results/``` in the directory where the python script is run from (likely the root directory of your repository clone).
|
||||
Run ```pip install -r requirements.txt``` followed by the desired simulation file. Or, if your environment is already set up, just run the python file.
|
||||
### Single Simulation
|
||||
Run ```topSimulate.py```. You must exit the visualization window in order to see the data plots. <br> Generated files will be saved to ```sim_results/``` in the directory where the python script is run from (likely the root directory of your repository clone).
|
||||
### Multiple Simulations with parameter noise
|
||||
Set desired parameter noise level in ```simulateMultipleWithNoise.py```, then run. Noise is applied to electromagnetic characteristics, length, width, sensor position, yoke position, moment of inertia, and mass. <br> Generated files will be saved to ```sim_results_multi/```.
|
||||
## Modifying the PID control algorithm
|
||||
Modify ```controller.py```. You will see constants related to heave, pitch, and roll controllers. Will update to include current control and will make simulation much more accurate soon.
|
||||
## Modifying pod parameters
|
||||
|
||||
@@ -127,7 +127,7 @@ class DecentralizedPIDController:
|
||||
|
||||
# Apply saturation
|
||||
s = np.sign(eadesired)
|
||||
maxea = P['quadParams'].maxVoltage * np.ones(4)
|
||||
maxea = P['quadParams'].maxVoltage
|
||||
|
||||
ea = s * np.minimum(np.abs(eadesired), maxea)
|
||||
|
||||
|
||||
@@ -44,8 +44,9 @@ def quad_ode_function_hf(t, X, eaVec, distVec, P):
|
||||
Xdot : ndarray, shape (22,)
|
||||
Time derivative of the input vector X
|
||||
"""
|
||||
yokeR = P['quadParams'].yokeR # in ohms
|
||||
yokeL = P['quadParams'].yokeL # in henries
|
||||
# Use individual yoke resistances and inductances
|
||||
yokeR = P['quadParams'].yokeR_individual # 4-element array in ohms
|
||||
yokeL = P['quadParams'].yokeL_individual # 4-element array in henries
|
||||
|
||||
# Extract state variables
|
||||
currents = X[18:22] # indices 19:22 in MATLAB (1-indexed) = 18:22 in Python
|
||||
@@ -85,8 +86,8 @@ def quad_ode_function_hf(t, X, eaVec, distVec, P):
|
||||
# Calculate torques on body
|
||||
Nb = np.zeros(3)
|
||||
for i in range(4): # loop through each yoke
|
||||
# Voltage-motor modeling
|
||||
currentsdot[i] = (eaVec[i] - currents[i] * yokeR) / yokeL
|
||||
# Voltage-motor modeling using individual R and L for each yoke
|
||||
currentsdot[i] = (eaVec[i] - currents[i] * yokeR[i]) / yokeL[i]
|
||||
|
||||
NiB = np.zeros(3) # since yokes can't cause moment by themselves
|
||||
FiB = np.array([0, 0, Fm[i]])
|
||||
|
||||
@@ -6,44 +6,136 @@ Ported from quadParamsScript.m and constantsScript.m
|
||||
import numpy as np
|
||||
|
||||
|
||||
# Global parameter variations - initialized once per simulation run
|
||||
_param_variations = None
|
||||
|
||||
|
||||
def initialize_parameter_variations(noise_level=0.05, seed=None):
|
||||
"""
|
||||
Initialize mechanical and electrical parameter variations.
|
||||
Call this once at the start of a simulation to set random variations.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
noise_level : float, optional
|
||||
Standard deviation of multiplicative noise (default: 0.05 = 5%)
|
||||
seed : int, optional
|
||||
Random seed for reproducibility
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict
|
||||
Dictionary with parameter variation factors
|
||||
"""
|
||||
global _param_variations
|
||||
|
||||
if seed is not None:
|
||||
np.random.seed(seed)
|
||||
|
||||
# Generate multiplicative variation factors for each parameter
|
||||
_param_variations = {
|
||||
'mass': 1 + np.random.normal(0, noise_level),
|
||||
'Jq_components': np.array([1 + np.random.normal(0, noise_level) for _ in range(3)]), # Individual noise for each inertia component
|
||||
'frame_l': 1 + np.random.normal(0, noise_level * 0.5), # Smaller variation for dimensions
|
||||
'frame_w': 1 + np.random.normal(0, noise_level * 0.5),
|
||||
'yh': 1 + np.random.normal(0, noise_level * 0.5),
|
||||
# Individual yoke variations (4 yokes)
|
||||
'yoke_R': np.array([1 + np.random.normal(0, noise_level * 0.3) for _ in range(4)]),
|
||||
'yoke_L': np.array([1 + np.random.normal(0, noise_level * 0.3) for _ in range(4)]),
|
||||
# Position noise for yoke/rotor locations (4 locations, 3D)
|
||||
'rotor_pos_noise': np.random.normal(0, noise_level * 0.2, (3, 4)),
|
||||
# Position noise for sensor locations (4 sensors, 3D)
|
||||
'sensor_pos_noise': np.random.normal(0, noise_level * 0.2, (3, 4))
|
||||
}
|
||||
|
||||
return _param_variations.copy()
|
||||
|
||||
|
||||
def get_parameter_variations():
|
||||
"""
|
||||
Get current parameter variations. If not initialized, use nominal (no variation).
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict
|
||||
Dictionary with parameter variation factors
|
||||
"""
|
||||
global _param_variations
|
||||
|
||||
if _param_variations is None:
|
||||
# Use nominal values (no variation)
|
||||
_param_variations = {
|
||||
'mass': 1.0,
|
||||
'Jq_components': np.ones(3),
|
||||
'frame_l': 1.0,
|
||||
'frame_w': 1.0,
|
||||
'yh': 1.0,
|
||||
'yoke_R': np.ones(4),
|
||||
'yoke_L': np.ones(4),
|
||||
'rotor_pos_noise': np.zeros((3, 4)),
|
||||
'sensor_pos_noise': np.zeros((3, 4))
|
||||
}
|
||||
|
||||
return _param_variations
|
||||
|
||||
|
||||
def reset_parameter_variations():
|
||||
"""Reset parameter variations to force reinitialization"""
|
||||
global _param_variations
|
||||
_param_variations = None
|
||||
|
||||
|
||||
class QuadParams:
|
||||
"""Quadrotor/maglev pod parameters"""
|
||||
|
||||
def __init__(self):
|
||||
# Pod mechanical characteristics
|
||||
frame_l = 0.61
|
||||
frame_w = 0.149 # in meters
|
||||
self.yh = 3 * 0.0254 # yoke height
|
||||
# Get parameter variations
|
||||
pv = get_parameter_variations()
|
||||
|
||||
# Pod mechanical characteristics (with variations)
|
||||
frame_l = 0.61 * pv['frame_l']
|
||||
frame_w = 0.149 * pv['frame_w']
|
||||
self.yh = 3 * 0.0254 * pv['yh'] # yoke height
|
||||
yh = self.yh
|
||||
|
||||
# Yoke/rotor locations (at corners)
|
||||
self.rotor_loc = np.array([
|
||||
# Store dimensions for reference
|
||||
self.frame_l = frame_l
|
||||
self.frame_w = frame_w
|
||||
|
||||
# Yoke/rotor locations (at corners) with position noise
|
||||
nominal_rotor_loc = np.array([
|
||||
[frame_l/2, frame_l/2, -frame_l/2, -frame_l/2],
|
||||
[-frame_w/2, frame_w/2, frame_w/2, -frame_w/2],
|
||||
[yh, yh, yh, yh]
|
||||
])
|
||||
self.rotor_loc = nominal_rotor_loc * (1 + pv['rotor_pos_noise'])
|
||||
|
||||
# Sensor locations (independent from yoke/rotor locations, at edge centers)
|
||||
self.sensor_loc = np.array([
|
||||
# Sensor locations (independent from yoke/rotor locations, at edge centers) with position noise
|
||||
nominal_sensor_loc = np.array([
|
||||
[frame_l/2, 0, -frame_l/2, 0],
|
||||
[0, frame_w/2, 0, -frame_w/2],
|
||||
[yh, yh, yh, yh]
|
||||
])
|
||||
self.sensor_loc = nominal_sensor_loc * (1 + pv['sensor_pos_noise'])
|
||||
|
||||
self.gap_sigma = 0.5e-3 # usually on micron scale
|
||||
|
||||
# Mass of the quad, in kg
|
||||
self.m = 6
|
||||
# Mass of the quad, in kg (with variation)
|
||||
self.m = 6 * pv['mass']
|
||||
|
||||
# The quad's moment of inertia, expressed in the body frame, in kg-m^2
|
||||
self.Jq = np.diag([0.017086, 0.125965, 0.131940])
|
||||
# The quad's moment of inertia, expressed in the body frame, in kg-m^2 (with individual component variations)
|
||||
nominal_Jq = np.diag([0.017086, 0.125965, 0.131940])
|
||||
self.Jq = nominal_Jq * np.diag(pv['Jq_components']) # Apply different noise to each diagonal component
|
||||
self.invJq = np.linalg.inv(self.Jq)
|
||||
|
||||
# Quad electrical characteristics
|
||||
# Quad electrical characteristics (with variations)
|
||||
maxcurrent = 30
|
||||
self.yokeR = 2.2 # in ohms
|
||||
self.yokeL = 5e-3 # in henries (2.5mH per yoke)
|
||||
self.maxVoltage = maxcurrent * self.yokeR # max magnitude voltage supplied to each yoke
|
||||
|
||||
# Individual yoke resistances and inductances (4 yokes with individual variations)
|
||||
self.yokeR_individual = 2.2 * pv['yoke_R'] # 4-element array
|
||||
self.yokeL_individual = 5e-3 * pv['yoke_L'] # 4-element array
|
||||
|
||||
self.maxVoltage = maxcurrent * self.yokeR_individual # max magnitude voltage supplied to each yoke
|
||||
|
||||
|
||||
class Constants:
|
||||
|
||||
361
MAGLEV_DIGITALTWIN_PYTHON/simulateMultipleWithNoise.py
Normal file
361
MAGLEV_DIGITALTWIN_PYTHON/simulateMultipleWithNoise.py
Normal file
@@ -0,0 +1,361 @@
|
||||
"""
|
||||
Multiple trial simulation with randomized noise for maglev system
|
||||
Runs multiple simulations with different parameter variations
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from parameters import QuadParams, Constants, initialize_parameter_variations, reset_parameter_variations
|
||||
from utils import euler2dcm, fmag2, initialize_magnetic_characteristics, reset_magnetic_characteristics, get_magnetic_characteristics
|
||||
from simulate import simulate_maglev_control
|
||||
from visualize import visualize_quad
|
||||
import os
|
||||
|
||||
# ===== SIMULATION CONFIGURATION =====
|
||||
NUM_TRIALS = 5 # Number of trials to run
|
||||
NOISE_LEVEL = 0.1 # Standard deviation of noise (5%)
|
||||
# =====================================
|
||||
|
||||
|
||||
def generate_parameter_report(trial_num, quad_params, output_dir):
|
||||
"""Generate a text report of all parameters for this trial"""
|
||||
|
||||
# Get magnetic characteristics
|
||||
mag_chars = get_magnetic_characteristics()
|
||||
|
||||
report_filename = f'{output_dir}/trial_{trial_num:02d}_parameters.txt'
|
||||
|
||||
with open(report_filename, 'w') as f:
|
||||
f.write(f"="*70 + "\n")
|
||||
f.write(f"Parameter Report for Trial {trial_num}\n")
|
||||
f.write(f"Noise Level: {NOISE_LEVEL*100:.1f}%\n")
|
||||
f.write(f"="*70 + "\n\n")
|
||||
|
||||
# MECHANICAL PARAMETERS
|
||||
f.write(f"MECHANICAL PARAMETERS\n")
|
||||
f.write(f"-" * 70 + "\n")
|
||||
f.write(f"Mass (m): {quad_params.m:.6f} kg\n")
|
||||
f.write(f"\nMoment of Inertia (Jq): (kg⋅m²)\n")
|
||||
f.write(f" Jxx: {quad_params.Jq[0,0]:.9f}\n")
|
||||
f.write(f" Jyy: {quad_params.Jq[1,1]:.9f}\n")
|
||||
f.write(f" Jzz: {quad_params.Jq[2,2]:.9f}\n")
|
||||
|
||||
f.write(f"\nFrame Dimensions:\n")
|
||||
f.write(f" Length (frame_l): {quad_params.frame_l:.6f} m ({quad_params.frame_l*1000:.3f} mm)\n")
|
||||
f.write(f" Width (frame_w): {quad_params.frame_w:.6f} m ({quad_params.frame_w*1000:.3f} mm)\n")
|
||||
f.write(f" Yoke Height (yh): {quad_params.yh:.6f} m ({quad_params.yh*1000:.3f} mm)\n")
|
||||
|
||||
f.write(f"\nRotor/Yoke Locations (m): [x, y, z] for each corner\n")
|
||||
for i in range(4):
|
||||
f.write(f" Yoke {i+1}: [{quad_params.rotor_loc[0,i]:8.6f}, "
|
||||
f"{quad_params.rotor_loc[1,i]:8.6f}, "
|
||||
f"{quad_params.rotor_loc[2,i]:8.6f}]\n")
|
||||
|
||||
f.write(f"\nSensor Locations (m): [x, y, z] for each edge center\n")
|
||||
for i in range(4):
|
||||
f.write(f" Sensor {i+1}: [{quad_params.sensor_loc[0,i]:8.6f}, "
|
||||
f"{quad_params.sensor_loc[1,i]:8.6f}, "
|
||||
f"{quad_params.sensor_loc[2,i]:8.6f}]\n")
|
||||
|
||||
f.write(f"\nSensor Noise: {quad_params.gap_sigma*1e6:.3f} μm (std dev)\n")
|
||||
|
||||
# ELECTROMAGNETIC PARAMETERS
|
||||
f.write(f"\n\nELECTROMAGNETIC PARAMETERS\n")
|
||||
f.write(f"-" * 70 + "\n")
|
||||
|
||||
f.write(f"Yoke Electrical Characteristics (per yoke):\n")
|
||||
for i in range(4):
|
||||
f.write(f" Yoke {i+1}:\n")
|
||||
f.write(f" Resistance (R): {quad_params.yokeR_individual[i]:.6f} Ω\n")
|
||||
f.write(f" Inductance (L): {quad_params.yokeL_individual[i]:.9f} H ({quad_params.yokeL_individual[i]*1e3:.6f} mH)\n")
|
||||
f.write(f" Max Voltage: {quad_params.maxVoltage[i]:.3f} V\n")
|
||||
|
||||
f.write(f"\nMagnetic Force Model Coefficients:\n")
|
||||
f.write(f" N (turns): {mag_chars['N']:.3f}\n")
|
||||
f.write(f" const1: {mag_chars['const1']:.6e}\n")
|
||||
f.write(f" const2: {mag_chars['const2']:.6e}\n")
|
||||
f.write(f" const3: {mag_chars['const3']:.6e}\n")
|
||||
f.write(f" const4: {mag_chars['const4']:.6e}\n")
|
||||
f.write(f" const5: {mag_chars['const5']:.6e}\n")
|
||||
|
||||
f.write(f"\n" + "="*70 + "\n")
|
||||
f.write(f"End of Report\n")
|
||||
f.write(f"="*70 + "\n")
|
||||
|
||||
print(f" Saved parameter report: {report_filename}")
|
||||
|
||||
|
||||
def run_single_trial(trial_num, Tsim, delt, ref_gap, z0, output_dir):
|
||||
"""Run a single simulation trial with randomized parameters"""
|
||||
|
||||
# Reset and reinitialize noise for this trial
|
||||
reset_parameter_variations()
|
||||
reset_magnetic_characteristics()
|
||||
initialize_parameter_variations(noise_level=NOISE_LEVEL)
|
||||
initialize_magnetic_characteristics(noise_level=NOISE_LEVEL)
|
||||
|
||||
# Maglev parameters and constants (with new noise)
|
||||
quad_params = QuadParams()
|
||||
constants = Constants()
|
||||
|
||||
m = quad_params.m
|
||||
g = constants.g
|
||||
J = quad_params.Jq
|
||||
|
||||
# Time vector, in seconds
|
||||
N = int(np.floor(Tsim / delt))
|
||||
tVec = np.arange(N) * delt
|
||||
|
||||
# Matrix of disturbance forces acting on the body, in Newtons, expressed in I
|
||||
distMat = np.random.normal(0, 0, (N-1, 3))
|
||||
|
||||
# Oversampling factor
|
||||
oversampFact = 10
|
||||
|
||||
# Check nominal gap
|
||||
print(f"Force check: {4*fmag2(0, 10.830e-3) - m*g}")
|
||||
|
||||
# SET REFERENCE HERE
|
||||
ref_gap = 10.830e-3 # from python code
|
||||
z0 = ref_gap - 2e-3
|
||||
|
||||
# Create reference trajectories
|
||||
rIstar = np.zeros((N, 3))
|
||||
vIstar = np.zeros((N, 3))
|
||||
aIstar = np.zeros((N, 3))
|
||||
xIstar = np.zeros((N, 3))
|
||||
|
||||
for k in range(N):
|
||||
rIstar[k, :] = [0, 0, -ref_gap]
|
||||
vIstar[k, :] = [0, 0, 0]
|
||||
aIstar[k, :] = [0, 0, 0]
|
||||
xIstar[k, :] = [0, 1, 0]
|
||||
|
||||
# Setup reference structure
|
||||
R = {
|
||||
'tVec': tVec,
|
||||
'rIstar': rIstar,
|
||||
'vIstar': vIstar,
|
||||
'aIstar': aIstar,
|
||||
'xIstar': xIstar
|
||||
}
|
||||
|
||||
# Initial state
|
||||
state0 = {
|
||||
'r': np.array([0, 0, -(z0 + quad_params.yh)]),
|
||||
'v': np.array([0, 0, 0]),
|
||||
'e': np.array([0.01, 0.01, np.pi/2]), # xyz euler angles
|
||||
'omegaB': np.array([0.00, 0.00, 0])
|
||||
}
|
||||
|
||||
# Setup simulation structure
|
||||
S = {
|
||||
'tVec': tVec,
|
||||
'distMat': distMat,
|
||||
'oversampFact': oversampFact,
|
||||
'state0': state0
|
||||
}
|
||||
|
||||
# Setup parameters structure
|
||||
P = {
|
||||
'quadParams': quad_params,
|
||||
'constants': constants
|
||||
}
|
||||
|
||||
# Generate parameter report for this trial
|
||||
generate_parameter_report(trial_num, quad_params, output_dir)
|
||||
|
||||
# Run simulation
|
||||
print(f" Running simulation for trial {trial_num}...")
|
||||
P0 = simulate_maglev_control(R, S, P)
|
||||
print(f" Trial {trial_num} simulation complete!")
|
||||
|
||||
# Extract results
|
||||
tVec_out = P0['tVec']
|
||||
state = P0['state']
|
||||
rMat = state['rMat']
|
||||
eMat = state['eMat']
|
||||
gaps = state['gaps']
|
||||
currents = state['currents']
|
||||
|
||||
# Generate 3D visualization (GIF) without displaying
|
||||
print(f" Generating 3D visualization for trial {trial_num}...")
|
||||
S2 = {
|
||||
'tVec': tVec_out,
|
||||
'rMat': rMat,
|
||||
'eMat': eMat,
|
||||
'plotFrequency': 20,
|
||||
'makeGifFlag': True,
|
||||
'gifFileName': f'{output_dir}/trial_{trial_num:02d}_animation.gif',
|
||||
'bounds': [-1, 1, -1, 1, -300e-3, 0.000]
|
||||
}
|
||||
visualize_quad(S2)
|
||||
plt.close('all') # Close all figures to prevent display
|
||||
|
||||
# Calculate forces
|
||||
Fm = fmag2(currents[:, 0], gaps[:, 0])
|
||||
|
||||
return {
|
||||
'tVec': tVec_out,
|
||||
'gaps': gaps,
|
||||
'currents': currents,
|
||||
'Fm': Fm,
|
||||
'quad_params': quad_params
|
||||
}
|
||||
|
||||
|
||||
def main():
|
||||
"""Main simulation script - runs multiple trials"""
|
||||
|
||||
# Create output directory if it doesn't exist
|
||||
output_dir = 'sim_results_multi'
|
||||
os.makedirs(output_dir, exist_ok=True)
|
||||
|
||||
# Total simulation time, in seconds
|
||||
Tsim = 2
|
||||
|
||||
# Update interval, in seconds
|
||||
delt = 0.005 # sampling interval
|
||||
|
||||
# Reference gap and initial condition
|
||||
ref_gap = 10.830e-3
|
||||
z0 = ref_gap - 2e-3
|
||||
|
||||
print(f"\n{'='*60}")
|
||||
print(f"Running {NUM_TRIALS} trials with noise level {NOISE_LEVEL*100:.1f}%")
|
||||
print(f"{'='*60}\n")
|
||||
|
||||
# Run all trials
|
||||
trial_results = []
|
||||
for trial in range(1, NUM_TRIALS + 1):
|
||||
print(f"Trial {trial}/{NUM_TRIALS}")
|
||||
result = run_single_trial(trial, Tsim, delt, ref_gap, z0, output_dir)
|
||||
trial_results.append(result)
|
||||
print()
|
||||
|
||||
# Create individual plots for each trial
|
||||
print("Generating plots...")
|
||||
for i, result in enumerate(trial_results, 1):
|
||||
tVec_out = result['tVec']
|
||||
gaps = result['gaps']
|
||||
currents = result['currents']
|
||||
Fm = result['Fm']
|
||||
|
||||
# Create plots for this trial
|
||||
fig = plt.figure(figsize=(12, 8))
|
||||
fig.suptitle(f'Trial {i} - Noise Level {NOISE_LEVEL*100:.1f}%', fontsize=14, fontweight='bold')
|
||||
|
||||
# Plot 1: Gaps
|
||||
ax1 = plt.subplot(3, 1, 1)
|
||||
plt.plot(tVec_out, gaps * 1e3)
|
||||
plt.axhline(y=ref_gap * 1e3, color='k', linestyle='--', linewidth=1, label='Reference')
|
||||
plt.ylabel('Gap (mm)')
|
||||
plt.title('Sensor Gaps')
|
||||
plt.legend(['Sensor 1', 'Sensor 2', 'Sensor 3', 'Sensor 4', 'Reference'],
|
||||
loc='upper right', fontsize=8)
|
||||
plt.grid(True)
|
||||
plt.xticks([])
|
||||
|
||||
# Plot 2: Currents
|
||||
ax2 = plt.subplot(3, 1, 2)
|
||||
plt.plot(tVec_out, currents)
|
||||
plt.ylabel('Current (A)')
|
||||
plt.title('Yoke Currents')
|
||||
plt.legend(['Yoke 1', 'Yoke 2', 'Yoke 3', 'Yoke 4'],
|
||||
loc='upper right', fontsize=8)
|
||||
plt.grid(True)
|
||||
plt.xticks([])
|
||||
|
||||
# Plot 3: Forces
|
||||
ax3 = plt.subplot(3, 1, 3)
|
||||
plt.plot(tVec_out, Fm)
|
||||
plt.xlabel('Time (sec)')
|
||||
plt.ylabel('Force (N)')
|
||||
plt.title('Magnetic Force (Yoke 1)')
|
||||
plt.grid(True)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.savefig(f'{output_dir}/trial_{i:02d}_results.png', dpi=150)
|
||||
print(f" Saved trial {i} plot")
|
||||
plt.close()
|
||||
|
||||
# FFT for this trial
|
||||
oversampFact = 10
|
||||
Fs = 1/delt * oversampFact
|
||||
L = len(tVec_out)
|
||||
|
||||
Y = np.fft.fft(Fm)
|
||||
frequencies = Fs / L * np.arange(L)
|
||||
|
||||
fig2 = plt.figure(figsize=(10, 6))
|
||||
plt.semilogx(frequencies, np.abs(Y), linewidth=2)
|
||||
plt.title(f"FFT Spectrum - Trial {i}")
|
||||
plt.xlabel("Frequency (Hz)")
|
||||
plt.ylabel("Magnitude")
|
||||
plt.ylim([0, np.max(np.abs(Y[1:])) * 1.05])
|
||||
plt.grid(True)
|
||||
plt.savefig(f'{output_dir}/trial_{i:02d}_fft.png', dpi=150)
|
||||
print(f" Saved trial {i} FFT")
|
||||
plt.close()
|
||||
|
||||
# Create overlay comparison plots
|
||||
print("\nGenerating comparison plots...")
|
||||
|
||||
# Gaps comparison
|
||||
fig_comp = plt.figure(figsize=(14, 10))
|
||||
fig_comp.suptitle(f'All Trials Comparison ({NUM_TRIALS} trials, {NOISE_LEVEL*100:.1f}% noise)',
|
||||
fontsize=14, fontweight='bold')
|
||||
|
||||
ax1 = plt.subplot(3, 1, 1)
|
||||
for i, result in enumerate(trial_results, 1):
|
||||
avg_gap = np.mean(result['gaps'], axis=1)
|
||||
plt.plot(result['tVec'], avg_gap * 1e3, alpha=0.7, label=f'Trial {i}')
|
||||
plt.axhline(y=ref_gap * 1e3, color='k', linestyle='--', linewidth=2, label='Reference')
|
||||
plt.ylabel('Average Gap (mm)')
|
||||
plt.title('Average Gap Across All Trials')
|
||||
plt.legend(loc='upper right', fontsize=8)
|
||||
plt.grid(True)
|
||||
plt.xticks([])
|
||||
|
||||
# Currents comparison
|
||||
ax2 = plt.subplot(3, 1, 2)
|
||||
for i, result in enumerate(trial_results, 1):
|
||||
avg_current = np.mean(result['currents'], axis=1)
|
||||
plt.plot(result['tVec'], avg_current, alpha=0.7, label=f'Trial {i}')
|
||||
plt.ylabel('Average Current (A)')
|
||||
plt.title('Average Current Across All Trials')
|
||||
plt.legend(loc='upper right', fontsize=8)
|
||||
plt.grid(True)
|
||||
plt.xticks([])
|
||||
|
||||
# Forces comparison
|
||||
ax3 = plt.subplot(3, 1, 3)
|
||||
for i, result in enumerate(trial_results, 1):
|
||||
plt.plot(result['tVec'], result['Fm'], alpha=0.7, label=f'Trial {i}')
|
||||
plt.xlabel('Time (sec)')
|
||||
plt.ylabel('Force (N)')
|
||||
plt.title('Magnetic Force Across All Trials')
|
||||
plt.legend(loc='upper right', fontsize=8)
|
||||
plt.grid(True)
|
||||
|
||||
plt.tight_layout()
|
||||
plt.savefig(f'{output_dir}/comparison_all_trials.png', dpi=150)
|
||||
print(f"Saved comparison plot")
|
||||
plt.close() # Close without displaying
|
||||
|
||||
print(f"\n{'='*60}")
|
||||
print(f"All trials completed!")
|
||||
print(f"Results saved to: {output_dir}/")
|
||||
print(f" - Individual trial plots: trial_XX_results.png")
|
||||
print(f" - Individual trial animations: trial_XX_animation.gif")
|
||||
print(f" - Individual FFT plots: trial_XX_fft.png")
|
||||
print(f" - Individual parameter reports: trial_XX_parameters.txt")
|
||||
print(f" - Comparison plot: comparison_all_trials.png")
|
||||
print(f"{'='*60}\n")
|
||||
|
||||
return trial_results
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
results = main()
|
||||
print(f"\nCompleted {len(results)} trials successfully!")
|
||||
@@ -19,7 +19,6 @@ def main():
|
||||
output_dir = 'sim_results'
|
||||
os.makedirs(output_dir, exist_ok=True)
|
||||
|
||||
# SET REFERENCE HERE
|
||||
# Total simulation time, in seconds
|
||||
Tsim = 2
|
||||
|
||||
@@ -40,16 +39,17 @@ def main():
|
||||
tVec = np.arange(N) * delt
|
||||
|
||||
# Matrix of disturbance forces acting on the body, in Newtons, expressed in I
|
||||
distMat = np.random.normal(0, 10, (N-1, 3))
|
||||
distMat = np.random.normal(0, 0, (N-1, 3))
|
||||
|
||||
# Oversampling factor
|
||||
oversampFact = 10
|
||||
|
||||
# Check nominal gap
|
||||
print(f"Force check: {4*fmag2(0, 11.239e-3) - m*g}")
|
||||
print(f"Force check: {4*fmag2(0, 10.830e-3) - m*g}")
|
||||
|
||||
# SET REFERENCE HERE
|
||||
ref_gap = 10.830e-3 # from python code
|
||||
z0 = ref_gap + 2e-3
|
||||
z0 = ref_gap - 2e-3
|
||||
|
||||
# Create reference trajectories
|
||||
rIstar = np.zeros((N, 3))
|
||||
|
||||
@@ -6,6 +6,83 @@ Ported from MATLAB to Python
|
||||
import numpy as np
|
||||
|
||||
|
||||
# Global magnetic characteristics with random variation
|
||||
# These are initialized once and persist across function calls
|
||||
_mag_characteristics = None
|
||||
|
||||
|
||||
def initialize_magnetic_characteristics(noise_level=0.05, seed=None):
|
||||
"""
|
||||
Initialize magnetic force characteristics with random variation.
|
||||
Call this once at the start of a simulation to set random parameters.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
noise_level : float, optional
|
||||
Standard deviation of multiplicative noise (default: 0.05 = 5%)
|
||||
seed : int, optional
|
||||
Random seed for reproducibility
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict
|
||||
Dictionary with perturbed magnetic coefficients
|
||||
"""
|
||||
global _mag_characteristics
|
||||
|
||||
if seed is not None:
|
||||
np.random.seed(seed)
|
||||
|
||||
# Nominal coefficients from fmag2
|
||||
nominal = {
|
||||
'N': 250,
|
||||
'const1': 0.2223394555e5,
|
||||
'const2': 0.2466906550e10,
|
||||
'const3': 0.6886569338e8,
|
||||
'const4': 0.6167266375e9,
|
||||
'const5': 0.3042813963e19
|
||||
}
|
||||
|
||||
# Add multiplicative noise to each coefficient
|
||||
_mag_characteristics = {
|
||||
key: value * (1 + np.random.normal(0, noise_level))
|
||||
for key, value in nominal.items()
|
||||
}
|
||||
|
||||
return _mag_characteristics.copy()
|
||||
|
||||
|
||||
def get_magnetic_characteristics():
|
||||
"""
|
||||
Get current magnetic characteristics. If not initialized, use nominal values.
|
||||
|
||||
Returns
|
||||
-------
|
||||
dict
|
||||
Dictionary with magnetic coefficients
|
||||
"""
|
||||
global _mag_characteristics
|
||||
|
||||
if _mag_characteristics is None:
|
||||
# Use nominal values if not initialized
|
||||
_mag_characteristics = {
|
||||
'N': 250,
|
||||
'const1': 0.2223394555e5,
|
||||
'const2': 0.2466906550e10,
|
||||
'const3': 0.6886569338e8,
|
||||
'const4': 0.6167266375e9,
|
||||
'const5': 0.3042813963e19
|
||||
}
|
||||
|
||||
return _mag_characteristics
|
||||
|
||||
|
||||
def reset_magnetic_characteristics():
|
||||
"""Reset magnetic characteristics to force reinitialization"""
|
||||
global _mag_characteristics
|
||||
_mag_characteristics = None
|
||||
|
||||
|
||||
def cross_product_equivalent(u):
|
||||
"""
|
||||
Outputs the cross-product-equivalent matrix uCross such that for arbitrary
|
||||
@@ -152,6 +229,9 @@ def fmag2(i, z):
|
||||
Fm : float or ndarray
|
||||
Magnetic force in Newtons (-1 if z < 0, indicating error)
|
||||
"""
|
||||
# Get current magnetic characteristics (nominal or perturbed)
|
||||
mc = get_magnetic_characteristics()
|
||||
|
||||
# Handle scalar and array inputs
|
||||
z_scalar = np.isscalar(z)
|
||||
i_scalar = np.isscalar(i)
|
||||
@@ -160,12 +240,11 @@ def fmag2(i, z):
|
||||
if z < 0:
|
||||
return -1
|
||||
|
||||
N = 250
|
||||
term1 = (-2 * i * N + 0.2223394555e5)
|
||||
denominator = (0.2466906550e10 * z + 0.6886569338e8)
|
||||
term1 = (-2 * i * mc['N'] + mc['const1'])
|
||||
denominator = (mc['const2'] * z + mc['const3'])
|
||||
|
||||
Fm = (0.6167266375e9 * term1**2 / denominator**2 -
|
||||
0.3042813963e19 * z * term1**2 / denominator**3)
|
||||
Fm = (mc['const4'] * term1**2 / denominator**2 -
|
||||
mc['const5'] * z * term1**2 / denominator**3)
|
||||
|
||||
return Fm
|
||||
else:
|
||||
@@ -180,24 +259,22 @@ def fmag2(i, z):
|
||||
valid_mask = z >= 0
|
||||
|
||||
if np.any(valid_mask):
|
||||
N = 250
|
||||
z_valid = z[valid_mask]
|
||||
i_valid = i if i_scalar else i[valid_mask]
|
||||
|
||||
term1 = (-2 * i_valid * N + 0.2223394555e5)
|
||||
denominator = (0.2466906550e10 * z_valid + 0.6886569338e8)
|
||||
term1 = (-2 * i_valid * mc['N'] + mc['const1'])
|
||||
denominator = (mc['const2'] * z_valid + mc['const3'])
|
||||
|
||||
Fm[valid_mask] = (0.6167266375e9 * term1**2 / denominator**2 -
|
||||
0.3042813963e19 * z_valid * term1**2 / denominator**3)
|
||||
Fm[valid_mask] = (mc['const4'] * term1**2 / denominator**2 -
|
||||
mc['const5'] * z_valid * term1**2 / denominator**3)
|
||||
|
||||
Fm[~valid_mask] = -1
|
||||
return Fm
|
||||
else:
|
||||
N = 250
|
||||
term1 = (-2 * i * N + 0.2223394555e5)
|
||||
denominator = (0.2466906550e10 * z + 0.6886569338e8)
|
||||
term1 = (-2 * i * mc['N'] + mc['const1'])
|
||||
denominator = (mc['const2'] * z + mc['const3'])
|
||||
|
||||
Fm = (0.6167266375e9 * term1**2 / denominator**2 -
|
||||
0.3042813963e19 * z * term1**2 / denominator**3)
|
||||
Fm = (mc['const4'] * term1**2 / denominator**2 -
|
||||
mc['const5'] * z * term1**2 / denominator**3)
|
||||
|
||||
return Fm
|
||||
|
||||
@@ -199,8 +199,9 @@ def visualize_quad(S):
|
||||
writer = PillowWriter(fps=S['plotFrequency'])
|
||||
anim.save(S['gifFileName'], writer=writer)
|
||||
print(f"Animation saved to {S['gifFileName']}")
|
||||
|
||||
plt.show()
|
||||
plt.close(fig) # Close without displaying
|
||||
else:
|
||||
plt.show()
|
||||
|
||||
P = {
|
||||
'tPlot': tPlot,
|
||||
|
||||
Reference in New Issue
Block a user