added Python DigitalTwin code

.gitignore

@@ -1,2 +1,3 @@
 .vscode/
 .DS_Store
+__pycache__/

MAGLEV_DIGITALTWIN_PYTHON/controller.py

@@ -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)

MAGLEV_DIGITALTWIN_PYTHON/dynamics.py

@@ -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

MAGLEV_DIGITALTWIN_PYTHON/parameters.py

@@ -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

MAGLEV_DIGITALTWIN_PYTHON/simulate.py

@@ -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

MAGLEV_DIGITALTWIN_PYTHON/topSimulate.py

@@ -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'])}")

MAGLEV_DIGITALTWIN_PYTHON/utils.py

@@ -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

MAGLEV_DIGITALTWIN_PYTHON/visualize.py

@@ -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]