From a174db6ef3fc8201492c5f10f7b3f747068d3083 Mon Sep 17 00:00:00 2001 From: pulipakaa24 Date: Sat, 22 Nov 2025 21:38:33 -0600 Subject: [PATCH] added Python DigitalTwin code --- .gitignore | 1 + MAGLEV_DIGITALTWIN_PYTHON/controller.py | 124 ++++++++++++ MAGLEV_DIGITALTWIN_PYTHON/dynamics.py | 113 +++++++++++ MAGLEV_DIGITALTWIN_PYTHON/parameters.py | 57 ++++++ MAGLEV_DIGITALTWIN_PYTHON/simulate.py | 178 +++++++++++++++++ MAGLEV_DIGITALTWIN_PYTHON/topSimulate.py | 206 ++++++++++++++++++++ MAGLEV_DIGITALTWIN_PYTHON/utils.py | 203 ++++++++++++++++++++ MAGLEV_DIGITALTWIN_PYTHON/visualize.py | 235 +++++++++++++++++++++++ 8 files changed, 1117 insertions(+) create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/controller.py create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/dynamics.py create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/parameters.py create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/simulate.py create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/topSimulate.py create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/utils.py create mode 100644 MAGLEV_DIGITALTWIN_PYTHON/visualize.py diff --git a/.gitignore b/.gitignore index a2ac9cd..bd2d311 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ .vscode/ .DS_Store +__pycache__/ \ No newline at end of file diff --git a/MAGLEV_DIGITALTWIN_PYTHON/controller.py b/MAGLEV_DIGITALTWIN_PYTHON/controller.py new file mode 100644 index 0000000..927dd10 --- /dev/null +++ b/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) diff --git a/MAGLEV_DIGITALTWIN_PYTHON/dynamics.py b/MAGLEV_DIGITALTWIN_PYTHON/dynamics.py new file mode 100644 index 0000000..7b06229 --- /dev/null +++ b/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 diff --git a/MAGLEV_DIGITALTWIN_PYTHON/parameters.py b/MAGLEV_DIGITALTWIN_PYTHON/parameters.py new file mode 100644 index 0000000..d706fe3 --- /dev/null +++ b/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 diff --git a/MAGLEV_DIGITALTWIN_PYTHON/simulate.py b/MAGLEV_DIGITALTWIN_PYTHON/simulate.py new file mode 100644 index 0000000..d809dbf --- /dev/null +++ b/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 diff --git a/MAGLEV_DIGITALTWIN_PYTHON/topSimulate.py b/MAGLEV_DIGITALTWIN_PYTHON/topSimulate.py new file mode 100644 index 0000000..a6894bf --- /dev/null +++ b/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'])}") diff --git a/MAGLEV_DIGITALTWIN_PYTHON/utils.py b/MAGLEV_DIGITALTWIN_PYTHON/utils.py new file mode 100644 index 0000000..baed130 --- /dev/null +++ b/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 diff --git a/MAGLEV_DIGITALTWIN_PYTHON/visualize.py b/MAGLEV_DIGITALTWIN_PYTHON/visualize.py new file mode 100644 index 0000000..f4554c8 --- /dev/null +++ b/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]