adipu/guadaloop_lev_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

added Python DigitalTwin code

Browse Source
This commit is contained in:
pulipakaa24
2025-11-22 21:38:33 -06:00
parent f4a4e8b750
commit a174db6ef3
8 changed files with 1117 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
MAGLEV_DIGITALTWIN_PYTHON/controller.py Normal file
View File

@@ -0,0 +1,124 @@
"""
Decentralized PID controller for maglev system
Ported from decentralizedPIDcontroller.m
"""
import numpy as np
class DecentralizedPIDController:
"""
Decentralized PID controller for quadrotor/maglev control.
Controls altitude, roll, and pitch using gap sensor feedback.
"""
def __init__(self):
# Persistent variables (maintain state between calls)
self.preverror = np.zeros(4)
self.cumerror = np.zeros(4)
def reset(self):
"""Reset persistent variables"""
self.preverror = np.zeros(4)
self.cumerror = np.zeros(4)
def control(self, R, S, P):
"""
Compute control voltages for each yoke.
Parameters
----------
R : dict
Reference structure with elements:
- rIstark : 3-element array of desired CM position at time tk in I frame (meters)
- vIstark : 3-element array of desired CM velocity (meters/sec)
- aIstark : 3-element array of desired CM acceleration (meters/sec^2)
S : dict
State structure with element:
- statek : dict containing:
- rI : 3-element position in I frame (meters)
- RBI : 3x3 or 9-element direction cosine matrix
- vI : 3-element velocity (meters/sec)
- omegaB : 3-element angular rate vector in body frame (rad/sec)
P : dict
Parameters structure with elements:
- quadParams : QuadParams object
- constants : Constants object
Returns
-------
ea : ndarray, shape (4,)
4-element vector with voltages applied to each yoke
"""
# Extract current state
zcg = S['statek']['rI'][2] # z-component of CG position
rl = P['quadParams'].sensor_loc
# Reshape RBI if needed
RBI = S['statek']['RBI']
if RBI.shape == (9,):
RBI = RBI.reshape(3, 3)
# Calculate gaps at sensor locations
gaps = np.abs(zcg) - np.array([0, 0, 1]) @ RBI.T @ rl
gaps = gaps.flatten()
# Controller gains
kp = 10000
ki = 0
kd = 50000
# Reference z position
refz = R['rIstark'][2]
meangap = np.mean(gaps)
# Transform gaps using corner-based sensing
# gaps indices: 0=front, 1=right, 2=back, 3=left
gaps_transformed = np.array([
gaps[0] + gaps[3] - meangap, # gaps(1)+gaps(4)-meangap in MATLAB
gaps[0] + gaps[1] - meangap, # gaps(1)+gaps(2)-meangap
gaps[2] + gaps[1] - meangap, # gaps(3)+gaps(2)-meangap
gaps[2] + gaps[3] - meangap # gaps(3)+gaps(4)-meangap
])
# Calculate error for each yoke
err = -refz - gaps_transformed
derr = err - self.preverror
self.preverror = err
self.cumerror = self.cumerror + err
# Calculate desired voltages
eadesired = kp * err + derr * kd + ki * self.cumerror
# Apply saturation
s = np.sign(eadesired)
maxea = P['quadParams'].maxVoltage * np.ones(4)
ea = s * np.minimum(np.abs(eadesired), maxea)
return ea
def decentralized_pid_controller(R, S, P, controller=None):
"""
Wrapper function to maintain compatibility with MATLAB-style function calls.
Parameters
----------
R, S, P : dict
See DecentralizedPIDController.control() for details
controller : DecentralizedPIDController, optional
Controller instance to use. If None, creates a new one (loses state)
Returns
-------
ea : ndarray, shape (4,)
4-element vector with voltages applied to each yoke
"""
if controller is None:
controller = DecentralizedPIDController()
return controller.control(R, S, P)

113
MAGLEV_DIGITALTWIN_PYTHON/dynamics.py Normal file
View File

@@ -0,0 +1,113 @@
"""
ODE function for maglev quadrotor dynamics
Ported from quadOdeFunctionHF.m
"""
import numpy as np
from utils import cross_product_equivalent, fmag2
def quad_ode_function_hf(t, X, eaVec, distVec, P):
"""
Ordinary differential equation function that models quadrotor dynamics
-- high-fidelity version. For use with scipy's ODE solvers (e.g., solve_ivp).
Parameters
----------
t : float
Scalar time input, as required by ODE solver format
X : ndarray, shape (22,)
Quad state, arranged as:
X = [rI, vI, RBI(flattened), omegaB, currents]
rI = position vector in I in meters (3 elements)
vI = velocity vector wrt I and in I, in meters/sec (3 elements)
RBI = attitude matrix from I to B frame, flattened (9 elements)
omegaB = angular rate vector of body wrt I, expressed in B, rad/sec (3 elements)
currents = vector of yoke currents, in amperes (4 elements)
eaVec : ndarray, shape (4,)
4-element vector of voltages applied to yokes, in volts
distVec : ndarray, shape (3,)
3-element vector of constant disturbance forces acting on quad's
center of mass, expressed in Newtons in I
P : dict
Structure with the following elements:
- quadParams : QuadParams object
- constants : Constants object
Returns
-------
Xdot : ndarray, shape (22,)
Time derivative of the input vector X
"""
yokeR = P['quadParams'].yokeR # in ohms
yokeL = P['quadParams'].yokeL # in henries
# Extract state variables
currents = X[18:22] # indices 19:22 in MATLAB (1-indexed) = 18:22 in Python
currentsdot = np.zeros(4)
R = X[6:15].reshape(3, 3) # indices 7:15 in MATLAB = 6:15 in Python
w = X[15:18] # indices 16:18 in MATLAB = 15:18 in Python
wx = cross_product_equivalent(w)
rdot = X[3:6] # indices 4:6 in MATLAB = 3:6 in Python
z = X[2] # index 3 in MATLAB = 2 in Python (zI of cg)
rl = P['quadParams'].sensor_loc
gaps = np.abs(z) - np.array([0, 0, 1]) @ R.T @ rl
gaps = gaps.flatten()
# Calculate magnetic forces
Fm = fmag2(currents, gaps)
# Handle collision with track
if np.any(Fm == -1):
rdot = rdot.copy()
rdot[2] = 0 # rdot(end) in MATLAB
Fm = np.zeros(4)
sumF = np.sum(Fm)
# Calculate linear acceleration
vdot = (np.array([0, 0, -P['quadParams'].m * P['constants'].g]) +
np.array([0, 0, sumF]) +
distVec) / P['quadParams'].m
# Calculate rotation rate
Rdot = -wx @ R
# 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
NiB = np.zeros(3) # since yokes can't cause moment by themselves
FiB = np.array([0, 0, Fm[i]])
# Accumulate torque (rotate FiB to inertial frame since fmag is always towards track)
Nb = Nb + (NiB + cross_product_equivalent(P['quadParams'].rotor_loc[:, i]) @ R.T @ FiB)
# Calculate angular acceleration
wdot = P['quadParams'].invJq @ (Nb - wx @ P['quadParams'].Jq @ w)
# Enforce constraint if pod is against track
if np.any(Fm == -1):
vdot[2] = 0
# Assemble state derivative
Xdot = np.concatenate([
rdot, # 3 elements
vdot, # 3 elements
Rdot.flatten(), # 9 elements
wdot, # 3 elements
currentsdot # 4 elements
])
return Xdot

57
MAGLEV_DIGITALTWIN_PYTHON/parameters.py Normal file
View File

@@ -0,0 +1,57 @@
"""
Quad parameters and constants for maglev simulation
Ported from quadParamsScript.m and constantsScript.m
"""
import numpy as np
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
yh = self.yh
# Yoke/rotor locations (at corners)
self.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]
])
# Sensor locations (independent from yoke/rotor locations, at edge centers)
self.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.gap_sigma = 0.5e-3 # usually on micron scale
# Mass of the quad, in kg
self.m = 6
# 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])
self.invJq = np.linalg.inv(self.Jq)
# Quad electrical characteristics
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
class Constants:
"""Physical constants"""
def __init__(self):
# Acceleration due to gravity, in m/s^2
self.g = 9.81
# Mass density of moist air, in kg/m^3
self.rho = 1.225

178
MAGLEV_DIGITALTWIN_PYTHON/simulate.py Normal file
View File

@@ -0,0 +1,178 @@
"""
Simulate maglev control
Ported from simulateMaglevControl.m
"""
import numpy as np
from scipy.integrate import solve_ivp
from utils import euler2dcm, dcm2euler
from dynamics import quad_ode_function_hf
from controller import DecentralizedPIDController
def simulate_maglev_control(R, S, P):
"""
Simulates closed-loop control of a maglev quadrotor.
Parameters
----------
R : dict
Reference structure with the following elements:
- tVec : (N,) array of uniformly-sampled time offsets from initial time (seconds)
- rIstar : (N, 3) array of desired CM positions in I frame (meters)
- vIstar : (N, 3) array of desired CM velocities (meters/sec)
- aIstar : (N, 3) array of desired CM accelerations (meters/sec^2)
- xIstar : (N, 3) array of desired body x-axis direction (unit vectors)
S : dict
Simulation structure with the following elements:
- oversampFact : Oversampling factor (must be >= 1)
- state0 : Initial state dict with:
- r : (3,) position in world frame (meters)
- e : (3,) Euler angles (radians)
- v : (3,) velocity (meters/sec)
- omegaB : (3,) angular rate vector (rad/sec)
- distMat : (N-1, 3) array of disturbance forces (Newtons)
P : dict
Parameters structure with:
- quadParams : QuadParams object
- constants : Constants object
Returns
-------
Q : dict
Output structure with:
- tVec : (M,) array of output sample time points
- state : dict with:
- rMat : (M, 3) position matrix
- eMat : (M, 3) Euler angles matrix
- vMat : (M, 3) velocity matrix
- omegaBMat : (M, 3) angular rate matrix
- gaps : (M, 4) gap measurements
- currents : (M, 4) yoke currents
"""
# Extract initial state
s0 = S['state0']
r0 = s0['r']
e0 = s0['e']
v0 = s0['v']
w0 = s0['omegaB']
currents0 = np.zeros(4)
R0 = euler2dcm(e0).flatten()
x0 = np.concatenate([r0, v0, R0, w0, currents0])
# Setup simulation parameters
N = len(R['tVec'])
dtIn = R['tVec'][1] - R['tVec'][0]
dt = dtIn / S['oversampFact']
p = {
'quadParams': P['quadParams'],
'constants': P['constants']
}
rl = P['quadParams'].rotor_loc
# Initialize outputs
tOut = []
xOut = []
gapsOut = []
xk = x0
# Create controller instance to maintain state
controller = DecentralizedPIDController()
for k in range(N - 1): # loop through each time step
tspan = np.arange(S['tVec'][k], S['tVec'][k+1] + dt/2, dt)
# Setup reference for this time step
Rk = {
'rIstark': R['rIstar'][k, :],
'vIstark': R['vIstar'][k, :],
'aIstark': R['aIstar'][k, :],
'xIstark': R['xIstar'][k, :]
}
# Setup state for this time step
Sk = {
'statek': {
'rI': xk[0:3],
'vI': xk[3:6],
'RBI': xk[6:15],
'omegaB': xk[15:18],
'currents': xk[18:22]
}
}
# Compute control voltage with noise
eak = controller.control(Rk, Sk, P) + np.random.normal(0, 0.01, 4)
# Disturbance for this time step
dk = S['distMat'][k, :]
# Integrate ODE
sol = solve_ivp(
lambda t, X: quad_ode_function_hf(t, X, eak, dk, p),
[tspan[0], tspan[-1]],
xk,
t_eval=tspan,
method='RK45',
max_step=dt
)
tk = sol.t
xk_traj = sol.y.T # Transpose to get (time, state) shape
# Calculate gaps for all time points in this segment
NTK = len(tk)
gapsk = np.zeros((NTK, 4))
for q in range(NTK):
R_q = xk_traj[q, 6:15].reshape(3, 3)
gapskq = np.abs(xk_traj[q, 2]) - np.array([0, 0, 1]) @ R_q.T @ rl
gapsk[q, :] = gapskq.flatten()
# Store results (excluding last point to avoid duplication)
tOut.append(tk[:-1])
xOut.append(xk_traj[:-1, :])
gapsOut.append(gapsk[:-1, :])
# Prepare for next iteration
xk = xk_traj[-1, :]
# Concatenate all segments
tOut = np.concatenate(tOut)
xOut = np.vstack(xOut)
gapsOut = np.vstack(gapsOut)
# Add final point
tOut = np.append(tOut, tk[-1])
xOut = np.vstack([xOut, xk_traj[-1, :]])
gapsOut = np.vstack([gapsOut, gapsk[-1, :]])
M = len(tOut)
# Form Euler angles from rotation matrices
eMat = np.zeros((M, 3))
for k in range(M):
Rk = xOut[k, 6:15].reshape(3, 3)
ek = dcm2euler(Rk)
eMat[k, :] = np.real(ek)
# Assemble output structure
Q = {
'tVec': tOut,
'state': {
'rMat': xOut[:, 0:3],
'vMat': xOut[:, 3:6],
'eMat': eMat,
'omegaBMat': xOut[:, 15:18],
'gaps': gapsOut,
'currents': xOut[:, 18:22]
}
}
return Q

206
MAGLEV_DIGITALTWIN_PYTHON/topSimulate.py Normal file
View File

@@ -0,0 +1,206 @@
"""
Top-level script for calling simulate_maglev_control
Ported from topSimulate.m to Python
"""
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation, PillowWriter
from parameters import QuadParams, Constants
from utils import euler2dcm, fmag2
from simulate import simulate_maglev_control
from visualize import visualize_quad
def main():
"""Main simulation script"""
# SET REFERENCE HERE
# Total simulation time, in seconds
Tsim = 2
# Update interval, in seconds. This value should be small relative to the
# shortest time constant of your system.
delt = 0.005 # sampling interval
# Maglev parameters and constants
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, 10, (N-1, 3))
# Oversampling factor
oversampFact = 10
# Check nominal gap
print(f"Force check: {4*fmag2(0, 11.239e-3) - m*g}")
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
}
# Run simulation
print("Running simulation...")
P0 = simulate_maglev_control(R, S, P)
print("Simulation complete!")
# Extract results
tVec_out = P0['tVec']
state = P0['state']
rMat = state['rMat']
eMat = state['eMat']
vMat = state['vMat']
gaps = state['gaps']
currents = state['currents']
omegaBMat = state['omegaBMat']
# Calculate forces
Fm = fmag2(currents[:, 0], gaps[:, 0])
# Calculate ex and ey for visualization
N_out = len(eMat)
ex = np.zeros(N_out)
ey = np.zeros(N_out)
for k in range(N_out):
Rk = euler2dcm(eMat[k, :])
x = Rk @ np.array([1, 0, 0])
ex[k] = x[0]
ey[k] = x[1]
# Visualize the quad motion
print("Generating 3D visualization...")
S2 = {
'tVec': tVec_out,
'rMat': rMat,
'eMat': eMat,
'plotFrequency': 20,
'makeGifFlag': True,
'gifFileName': 'testGif.gif',
'bounds': [-1, 1, -1, 1, -300e-3, 0.000]
}
visualize_quad(S2)
# Create plots
fig = plt.figure(figsize=(12, 8))
# 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)
plt.ylabel('Vertical (mm)')
plt.title('Gaps')
plt.grid(True)
plt.xticks([])
# Plot 2: Currents
ax2 = plt.subplot(3, 1, 2)
plt.plot(tVec_out, currents)
plt.ylabel('Current (Amps)')
plt.title('Power')
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('Fm (N)')
plt.title('Forcing')
plt.grid(True)
plt.tight_layout()
plt.savefig('simulation_results.png', dpi=150)
print("Saved plot to simulation_results.png")
# Commented out horizontal position plot (as in MATLAB)
# fig_horizontal = plt.figure()
# plt.plot(rMat[:, 0], rMat[:, 1])
# spacing = 300
# plt.quiver(rMat[::spacing, 0], rMat[::spacing, 1],
# ex[::spacing], -ey[::spacing])
# plt.axis('equal')
# plt.grid(True)
# plt.xlabel('X (m)')
# plt.ylabel('Y (m)')
# plt.title('Horizontal position of CM')
# Frequency analysis (mech freq)
Fs = 1/delt * oversampFact # Sampling frequency
T = 1/Fs # Sampling period
L = len(tVec_out) # Length of signal
Y = np.fft.fft(Fm)
frequencies = Fs / L * np.arange(L)
fig2 = plt.figure(figsize=(10, 6))
plt.semilogx(frequencies, np.abs(Y), linewidth=3)
plt.title("Complex Magnitude of fft Spectrum")
plt.xlabel("f (Hz)")
plt.ylabel("|fft(X)|")
plt.grid(True)
plt.savefig('fft_spectrum.png', dpi=150)
print("Saved FFT plot to fft_spectrum.png")
# Show all plots
plt.show()
return P0
if __name__ == '__main__':
results = main()
print("\nSimulation completed successfully!")
print(f"Final time: {results['tVec'][-1]:.3f} seconds")
print(f"Number of time points: {len(results['tVec'])}")

203
MAGLEV_DIGITALTWIN_PYTHON/utils.py Normal file
View File

@@ -0,0 +1,203 @@
"""
Utility functions for maglev simulation
Ported from MATLAB to Python
"""
import numpy as np
def cross_product_equivalent(u):
"""
Outputs the cross-product-equivalent matrix uCross such that for arbitrary
3-by-1 vectors u and v, cross(u,v) = uCross @ v.
Parameters
----------
u : array_like, shape (3,)
3-element vector
Returns
-------
uCross : ndarray, shape (3, 3)
Skew-symmetric cross-product equivalent matrix
"""
u1, u2, u3 = u[0], u[1], u[2]
uCross = np.array([
[0, -u3, u2],
[u3, 0, -u1],
[-u2, u1, 0]
])
return uCross
def rotation_matrix(aHat, phi):
"""
Generates the rotation matrix R corresponding to a rotation through an
angle phi about the axis defined by the unit vector aHat. This is a
straightforward implementation of Euler's formula for a rotation matrix.
Parameters
----------
aHat : array_like, shape (3,)
3-by-1 unit vector constituting the axis of rotation
phi : float
Angle of rotation, in radians
Returns
-------
R : ndarray, shape (3, 3)
Rotation matrix
"""
aHat = np.array(aHat).reshape(3, 1)
R = (np.cos(phi) * np.eye(3) +
(1 - np.cos(phi)) * (aHat @ aHat.T) -
np.sin(phi) * cross_product_equivalent(aHat.flatten()))
return R
def euler2dcm(e):
"""
Converts Euler angles phi = e[0], theta = e[1], and psi = e[2]
(in radians) into a direction cosine matrix for a 3-1-2 rotation.
Let the world (W) and body (B) reference frames be initially aligned. In a
3-1-2 order, rotate B away from W by angles psi (yaw, about the body Z
axis), phi (roll, about the body X axis), and theta (pitch, about the body Y
axis). R_BW can then be used to cast a vector expressed in W coordinates as
a vector in B coordinates: vB = R_BW @ vW
Parameters
----------
e : array_like, shape (3,)
Vector containing the Euler angles in radians: phi = e[0],
theta = e[1], and psi = e[2]
Returns
-------
R_BW : ndarray, shape (3, 3)
Direction cosine matrix
"""
a1 = np.array([0, 0, 1])
a2 = np.array([1, 0, 0])
a3 = np.array([0, 1, 0])
phi = e[0]
theta = e[1]
psi = e[2]
R_BW = rotation_matrix(a3, theta) @ rotation_matrix(a2, phi) @ rotation_matrix(a1, psi)
return R_BW
def dcm2euler(R_BW):
"""
Converts a direction cosine matrix R_BW to Euler angles phi = e[0],
theta = e[1], and psi = e[2] (in radians) for a 3-1-2 rotation.
If the conversion to Euler angles is singular (not unique), then this
function raises an error.
Parameters
----------
R_BW : ndarray, shape (3, 3)
Direction cosine matrix
Returns
-------
e : ndarray, shape (3,)
Vector containing the Euler angles in radians: phi = e[0],
theta = e[1], and psi = e[2]
Raises
------
ValueError
If gimbal lock occurs (|phi| = pi/2)
"""
R_BW = np.real(R_BW)
if R_BW[1, 2] == 1:
raise ValueError("Error: Gimbal lock since |phi| = pi/2")
theta = np.arctan2(-R_BW[0, 2], R_BW[2, 2])
phi = np.arcsin(R_BW[1, 2])
psi = np.arctan2(-R_BW[1, 0], R_BW[1, 1])
e = np.array([phi, theta, psi])
return e
def fmag2(i, z):
"""
Converts a given gap distance, z, and current, i, to yield the attraction
force Fm to a steel plate.
i > 0 runs current in direction to weaken Fm
i < 0 runs current to strengthen Fm
z is positive for valid conditions
Parameters
----------
i : float or ndarray
Current in amperes
z : float or ndarray
Gap distance in meters (must be positive)
Returns
-------
Fm : float or ndarray
Magnetic force in Newtons (-1 if z < 0, indicating error)
"""
# Handle scalar and array inputs
z_scalar = np.isscalar(z)
i_scalar = np.isscalar(i)
if z_scalar and i_scalar:
if z < 0:
return -1
N = 250
term1 = (-2 * i * N + 0.2223394555e5)
denominator = (0.2466906550e10 * z + 0.6886569338e8)
Fm = (0.6167266375e9 * term1**2 / denominator**2 -
0.3042813963e19 * z * term1**2 / denominator**3)
return Fm
else:
# Handle array inputs
z = np.asarray(z)
i = np.asarray(i)
# Check for negative z values
if np.any(z < 0):
# Create output array
Fm = np.zeros_like(z, dtype=float)
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)
Fm[valid_mask] = (0.6167266375e9 * term1**2 / denominator**2 -
0.3042813963e19 * 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)
Fm = (0.6167266375e9 * term1**2 / denominator**2 -
0.3042813963e19 * z * term1**2 / denominator**3)
return Fm

235
MAGLEV_DIGITALTWIN_PYTHON/visualize.py Normal file
View File

@@ -0,0 +1,235 @@
"""
Visualization function for quadrotor/maglev pod
Ported from visualizeQuad.m
"""
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
from matplotlib.animation import FuncAnimation, PillowWriter
from scipy.interpolate import interp1d
from utils import euler2dcm
from parameters import QuadParams
def visualize_quad(S):
"""
Takes in an input structure S and visualizes the resulting 3D motion
in approximately real-time. Outputs the data used to form the plot.
Parameters
----------
S : dict
Structure with the following elements:
- rMat : (M, 3) matrix of quad positions, in meters
- eMat : (M, 3) matrix of quad attitudes, in radians
- tVec : (M,) vector of times corresponding to each measurement
- plotFrequency : scalar number of frames per second (Hz)
- bounds : 6-element list [xmin, xmax, ymin, ymax, zmin, zmax]
- makeGifFlag : boolean (if True, export plot to .gif)
- gifFileName : string with the file name of the .gif
Returns
-------
P : dict
Structure with the following elements:
- tPlot : (N,) vector of time points used in the plot
- rPlot : (N, 3) vector of positions used to generate the plot
- ePlot : (N, 3) vector of attitudes used to generate the plot
"""
# UT colors
burntOrangeUT = np.array([191, 87, 0]) / 255
darkGrayUT = np.array([51, 63, 72]) / 255
# Get quad parameters
quad_params = QuadParams()
frame_l = 1.2192
frame_w = 0.711 # in meters
yh = quad_params.yh
# Parameters for the rotors
rotorLocations = 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]
])
r_rotor = 0.13
# Determine the location of the corners of the body box in the body frame
bpts = np.array([
[frame_l/2, frame_l/2, -frame_l/2, -frame_l/2, frame_l/2, frame_l/2, -frame_l/2, -frame_l/2],
[frame_w/2, -frame_w/2, frame_w/2, -frame_w/2, frame_w/2, -frame_w/2, frame_w/2, -frame_w/2],
[0.03, 0.03, 0.03, 0.03, -0.030, -0.030, -0.030, -0.030]
])
# Rectangles representing each side of the body box
b1 = bpts[:, [0, 4, 5, 1]]
b2 = bpts[:, [0, 4, 6, 2]]
b3 = bpts[:, [2, 6, 7, 3]]
b4 = bpts[:, [0, 2, 3, 1]]
b5 = bpts[:, [4, 6, 7, 5]]
b6 = bpts[:, [1, 5, 7, 3]]
# Create a circle for each rotor
t_circ = np.linspace(0, 2*np.pi, 20)
circpts = np.zeros((3, 20))
for i in range(20):
circpts[:, i] = r_rotor * np.array([np.cos(t_circ[i]), np.sin(t_circ[i]), 0])
m = len(S['tVec'])
# Single epoch plot
if m == 1:
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
# Extract params
RIB = euler2dcm(S['eMat'][0, :]).T
r = S['rMat'][0, :]
# Plot rotors
for i in range(4):
rotor_circle = r.reshape(3, 1) + RIB @ (circpts + rotorLocations[:, i].reshape(3, 1))
ax.plot(rotor_circle[0, :], rotor_circle[1, :], rotor_circle[2, :],
color=darkGrayUT, linewidth=2)
# Plot body
plot_body(ax, r, RIB, [b1, b2, b3, b4, b5, b6])
# Plot body axes
plot_axes(ax, r, RIB)
ax.set_xlim([S['bounds'][0], S['bounds'][1]])
ax.set_ylim([S['bounds'][2], S['bounds'][3]])
ax.set_zlim([S['bounds'][4], S['bounds'][5]])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.grid(True)
plt.show()
P = {
'tPlot': S['tVec'],
'rPlot': S['rMat'],
'ePlot': S['eMat']
}
else: # Animation
# Create time vectors
tf = 1 / S['plotFrequency']
tmax = S['tVec'][-1]
tmin = S['tVec'][0]
tPlot = np.arange(tmin, tmax + tf/2, tf)
tPlotLen = len(tPlot)
# Interpolate to regularize times
t2unique, indUnique = np.unique(S['tVec'], return_index=True)
rPlot_interp = interp1d(t2unique, S['rMat'][indUnique, :], axis=0,
kind='linear', fill_value='extrapolate')
ePlot_interp = interp1d(t2unique, S['eMat'][indUnique, :], axis=0,
kind='linear', fill_value='extrapolate')
rPlot = rPlot_interp(tPlot)
ePlot = ePlot_interp(tPlot)
# Create figure and axis
fig = plt.figure(figsize=(10, 8))
ax = fig.add_subplot(111, projection='3d')
# Storage for plot elements
plot_elements = []
def init():
ax.set_xlim([S['bounds'][0], S['bounds'][1]])
ax.set_ylim([S['bounds'][2], S['bounds'][3]])
ax.set_zlim([S['bounds'][4], S['bounds'][5]])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.view_init(elev=0, azim=0)
ax.grid(True)
return []
def update(frame):
nonlocal plot_elements
# Clear previous frame
for elem in plot_elements:
elem.remove()
plot_elements = []
# Extract data
RIB = euler2dcm(ePlot[frame, :]).T
r = rPlot[frame, :]
# Plot rotors
for i in range(4):
rotor_circle = r.reshape(3, 1) + RIB @ (circpts + rotorLocations[:, i].reshape(3, 1))
line, = ax.plot(rotor_circle[0, :], rotor_circle[1, :], rotor_circle[2, :],
color=darkGrayUT, linewidth=2)
plot_elements.append(line)
# Plot body
body_elem = plot_body(ax, r, RIB, [b1, b2, b3, b4, b5, b6])
plot_elements.append(body_elem)
# Plot axes
axis_elems = plot_axes(ax, r, RIB)
plot_elements.extend(axis_elems)
ax.set_title(f'Time: {tPlot[frame]:.3f} s')
return plot_elements
anim = FuncAnimation(fig, update, init_func=init, frames=tPlotLen,
interval=tf*1000, blit=False, repeat=True)
# Save as GIF if requested
if S.get('makeGifFlag', False):
writer = PillowWriter(fps=S['plotFrequency'])
anim.save(S['gifFileName'], writer=writer)
print(f"Animation saved to {S['gifFileName']}")
plt.show()
P = {
'tPlot': tPlot,
'rPlot': rPlot,
'ePlot': ePlot
}
return P
def plot_body(ax, r, RIB, body_faces):
"""Plot the body box"""
vertices = []
for face in body_faces:
face_r = r.reshape(3, 1) + RIB @ face
vertices.append(face_r.T)
# Create 3D polygon collection
poly = Poly3DCollection(vertices, alpha=0.5, facecolor=[0.5, 0.5, 0.5],
edgecolor='black', linewidths=1)
ax.add_collection3d(poly)
return poly
def plot_axes(ax, r, RIB):
"""Plot body axes"""
bodyX = 0.5 * RIB @ np.array([1, 0, 0])
bodyY = 0.5 * RIB @ np.array([0, 1, 0])
bodyZ = 0.5 * RIB @ np.array([0, 0, 1])
q1 = ax.quiver(r[0], r[1], r[2], bodyX[0], bodyX[1], bodyX[2],
color='red', arrow_length_ratio=0.3)
q2 = ax.quiver(r[0], r[1], r[2], bodyY[0], bodyY[1], bodyY[2],
color='blue', arrow_length_ratio=0.3)
q3 = ax.quiver(r[0], r[1], r[2], bodyZ[0], bodyZ[1], bodyZ[2],
color='green', arrow_length_ratio=0.3)
return [q1, q2, q3]