From cfebd5077fa6ecb9ee0580a6f68aa2a879f3a7ed Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 13:00:39 +0530 Subject: [PATCH 1/9] Added markers for start and end points of the trajectory --- .gitignore | 1 + .../lib/plotTimeSeries.py | 21 +++++++++++++------ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/.gitignore b/.gitignore index dbf57ff..1101f82 100644 --- a/.gitignore +++ b/.gitignore @@ -68,6 +68,7 @@ __pypackages__/ env/ venv/ ENV/ +*Env* # If you are using PyCharm # .idea/**/workspace.xml diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index 1922994..cd598fd 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -65,8 +65,13 @@ def plotVehicleStates(simTime, simData, figNo): plt.grid() plt.subplot(3, 3, 1) - plt.plot(y, x) - plt.legend(["North-East positions (m)"], fontsize=legendSize) + plt.plot(y, x, label='Trajectory') + plt.plot(y[0], x[0], 'go', markersize=5, label='Start') # Start marker + plt.plot(y[-1], x[-1], 'rs', markersize=5, label='End') # End marker + plt.legend(fontsize=legendSize) + plt.xlabel('North (m)') + plt.ylabel('East (m)') + plt.title('North-East Trajectory') plt.grid() plt.subplot(3, 3, 2) @@ -181,7 +186,7 @@ def anim_function(num, dataSet, line): return line - dataSet = np.array([N, E, -D]) # Down is negative z + dataSet = np.array([E, N, -D]) # (East, North, Up) # Attaching 3D axis to the figure fig = plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), @@ -190,7 +195,12 @@ def anim_function(num, dataSet, line): fig.add_axes(ax) # Line/trajectory plot - line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='b')[0] + line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='b', label='Trajectory')[0] + + # Add Start and End markers + ax.plot(dataSet[0, 0], dataSet[1, 0], dataSet[2, 0], 'go', markerfacecolor='green', markersize=5, label='Start') + ax.plot(dataSet[0, -1], dataSet[1, -1], dataSet[2, -1], 'rs', markerfacecolor='red', markersize=5, label='End') + ax.legend() # Setting the axes properties ax.set_xlabel('X / East') @@ -224,5 +234,4 @@ def anim_function(num, dataSet, line): repeat=True) # Save the 3D animation as a gif file - ani.save(filename, writer=animation.PillowWriter(fps=FPS)) - + ani.save(filename, writer=animation.PillowWriter(fps=FPS)) \ No newline at end of file From 4688d02dfaf95ca57abdabd6193a887120a11400 Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 15:38:18 +0530 Subject: [PATCH 2/9] Reversed starting and ending points of the trajectory --- src/python_vehicle_simulator/main.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/python_vehicle_simulator/main.py b/src/python_vehicle_simulator/main.py index 32aff10..4346630 100644 --- a/src/python_vehicle_simulator/main.py +++ b/src/python_vehicle_simulator/main.py @@ -14,6 +14,7 @@ import sys import webbrowser import matplotlib.pyplot as plt +import numpy as np from python_vehicle_simulator.vehicles import ( DSRV, frigate, otter, ROVzefakkel, semisub, shipClarke83, supply, tanker, remus100, torpedo @@ -79,6 +80,9 @@ def main(): # Main simulation loop [simTime, simData] = simulate(N, sampleTime, vehicle) + # Reverse simData array to exchange start and end points for plotting + simData = np.flip(simData, axis=0) + # 3D plots and animation plotVehicleStates(simTime, simData, 1) plotControls(simTime, simData, vehicle, 2) @@ -92,5 +96,4 @@ def main(): plt.close() if __name__ == "__main__": - main() - + main() \ No newline at end of file From 2c10863b80604a237cb3aaf3f4e07b7e7ae34ff5 Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 15:42:13 +0530 Subject: [PATCH 3/9] Changed z-position (eta[2]) from 0.0 to 10.0 to start underwater --- src/python_vehicle_simulator/lib/mainLoop.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/python_vehicle_simulator/lib/mainLoop.py b/src/python_vehicle_simulator/lib/mainLoop.py index 320eec8..9911499 100644 --- a/src/python_vehicle_simulator/lib/mainLoop.py +++ b/src/python_vehicle_simulator/lib/mainLoop.py @@ -64,7 +64,8 @@ def simulate(N, sampleTime, vehicle): t = 0 # initial simulation time # Initial state vectors - eta = np.array([0, 0, 0, 0, 0, 0], float) # position/attitude, user editable + # Changed z-position (eta[2]) from 0.0 to 10.0 to start underwater + eta = np.array([0, 0, 10.0, 0, 0, 0], float) # position/attitude, user editable nu = vehicle.nu # velocity, defined by vehicle class u_actual = vehicle.u_actual # actual inputs, defined by vehicle class @@ -99,4 +100,4 @@ def simulate(N, sampleTime, vehicle): # Store simulation time vector simTime = np.arange(start=0, stop=t+sampleTime, step=sampleTime)[:, None] - return(simTime,simData) + return(simTime,simData) \ No newline at end of file From eb6e902ef05bbff0a13d2e5c8a4110729674cb27 Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 16:38:35 +0530 Subject: [PATCH 4/9] Added AUVGeometry class from JustA3DObject/JustAnother3DSimulator/blob/main/auv_model.py --- .../lib/plotTimeSeries.py | 183 ++++++++++++++++++ 1 file changed, 183 insertions(+) diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index cd598fd..bb06d97 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -14,6 +14,7 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo) import numpy as np from python_vehicle_simulator.lib.gnc import ssa import mpl_toolkits.mplot3d.axes3d as p3 +from mpl_toolkits.mplot3d.art3d import Poly3DCollection import matplotlib.animation as animation legendSize = 10 # legend size @@ -29,6 +30,188 @@ def R2D(value): # radians to degrees def cm2inch(value): # inch to cm return value / 2.54 +# AUV Geometry + +class AUVGeometry: + """ + Handles the generation and transformation of the 3D AUV model geometry. + """ + def __init__(self, geometry): + self.geometry = geometry + self.L = geometry['l'] # Store total length for COM shift + self.generate_base_geometry() + + def generate_base_geometry(self): + """Generate the AUV geometry""" + geo = self.geometry + + r_max = geo['d'] / 2 + num_x_points = 20 # Reduced for faster animation + num_theta_points = 18 # Reduced for faster animation + theta = np.linspace(0, 2 * np.pi, num_theta_points) + z_offset = 0.001 + + # NOSE SECTION (Elipsoid) + x_nose = np.linspace(0, geo['a'], num_x_points) + r_nose = r_max - (r_max - geo['a_offset']) * (1 - x_nose / geo['a'])**geo['n'] + + X_nose, THETA_nose = np.meshgrid(x_nose, theta) + R_nose, _ = np.meshgrid(r_nose, theta) + Y_nose = R_nose * np.cos(THETA_nose) + Z_nose = R_nose * np.sin(THETA_nose) + + # MID-SECTION (Cylinder) + mid_section_length = geo['lf'] - geo['a'] + num_x_mid_points = max(2, int(num_x_points * (mid_section_length / geo['a']))) + x_mid = np.linspace(geo['a'], geo['lf'], num_x_mid_points) + r_mid = np.full_like(x_mid, r_max) + + X_mid, THETA_mid = np.meshgrid(x_mid, theta) + R_mid, _ = np.meshgrid(r_mid, theta) + Y_mid = R_mid * np.cos(THETA_mid) + Z_mid = R_mid * np.sin(THETA_mid) + + # TAIL SECTION (Power Series Curve of Revolution) + c = geo['l'] - geo['lf'] + num_x_tail_points = max(2, int(num_x_points * (c / geo['a']))) + x_tail = np.linspace(geo['lf'], geo['l'], num_x_tail_points) + + x_norm_tail = (x_tail - geo['lf']) / c + r_tail = geo['c_offset'] + (r_max - geo['c_offset']) * (1 - x_norm_tail**geo['n']) + + X_tail, THETA_tail = np.meshgrid(x_tail, theta) + R_tail, _ = np.meshgrid(r_tail, theta) + Y_tail = R_tail * np.cos(THETA_tail) + Z_tail = R_tail * np.sin(THETA_tail) + + r_final = geo['c_offset'] + + # DVL (Doppler Velocity Log) Box + dvl_len = 0.1 + dvl_width = 0.08 + dvl_height = 0.03 + x_dvl_start = geo['lf'] - dvl_len - 0.05 + x_dvl_end = x_dvl_start + dvl_len + y_dvl_half = dvl_width / 2 + z_dvl_top = -r_max # Note: Z-down in body frame + z_dvl_bottom = z_dvl_top - dvl_height + v = np.array([ + [x_dvl_start, -y_dvl_half, z_dvl_top], [x_dvl_end, -y_dvl_half, z_dvl_top], + [x_dvl_end, y_dvl_half, z_dvl_top], [x_dvl_start, y_dvl_half, z_dvl_top], + [x_dvl_start, -y_dvl_half, z_dvl_bottom], [x_dvl_end, -y_dvl_half, z_dvl_bottom], + [x_dvl_end, y_dvl_half, z_dvl_bottom], [x_dvl_start, y_dvl_half, z_dvl_bottom] + ]) + dvl_faces = [ + [v[0], v[1], v[2], v[3]], [v[4], v[5], v[6], v[7]], [v[0], v[1], v[5], v[4]], + [v[2], v[3], v[7], v[6]], [v[0], v[3], v[7], v[4]], [v[1], v[2], v[6], v[5]] + ] + + # Fins + fin_length = 0.12 + fin_span = 0.1 + fin_taper_ratio = 0.8 + fin_x_end = geo['l'] - 0.025 + fin_x_start = fin_x_end - fin_length + + x_norm_fin_start = (fin_x_start - geo['lf']) / c + r_fin_start = geo['c_offset'] + (r_max - geo['c_offset']) * (1 - x_norm_fin_start**geo['n']) + x_norm_fin_end = (fin_x_end - geo['lf']) / c + r_fin_end = geo['c_offset'] + (r_max - geo['c_offset']) * (1 - x_norm_fin_end**geo['n']) + + fin_verts = [] + # Starboard (Y+) Fin + v1 = [fin_x_start, r_fin_start, 0]; v2 = [fin_x_start, r_fin_start + fin_span, 0] + v3 = [fin_x_end, r_fin_end + fin_span * fin_taper_ratio, 0]; v4 = [fin_x_end, r_fin_end, 0] + fin_verts.append([v1, v2, v3, v4]) + # Port (Y-) Fin + v1 = [fin_x_start, -r_fin_start, 0]; v2 = [fin_x_start, -(r_fin_start + fin_span), 0] + v3 = [fin_x_end, -(r_fin_end + fin_span * fin_taper_ratio), 0]; v4 = [fin_x_end, -r_fin_end, 0] + fin_verts.append([v1, v2, v3, v4]) + # Top (Z-) Fin (Z is down, so -r is "up" visually on plot) + v1 = [fin_x_start, 0, -r_fin_start]; v2 = [fin_x_start, 0, -(r_fin_start + fin_span)] + v3 = [fin_x_end, 0, -(r_fin_end + fin_span * fin_taper_ratio)]; v4 = [fin_x_end, 0, -r_fin_end] + fin_verts.append([v1, v2, v3, v4]) + # Bottom (Z+) Fin + v1 = [fin_x_start, 0, r_fin_start]; v2 = [fin_x_start, 0, r_fin_start + fin_span] + v3 = [fin_x_end, 0, r_fin_end + fin_span * fin_taper_ratio]; v4 = [fin_x_end, 0, r_fin_end] + fin_verts.append([v1, v2, v3, v4]) + + # Store all geometry + self.base_geometry = { + 'nose': (X_nose, Y_nose, Z_nose), + 'mid': (X_mid, Y_mid, Z_mid), + 'tail': (X_tail, Y_tail, Z_tail), + 'dvl_faces': dvl_faces, + } + self.fins = fin_verts + + def rotation_matrix(self, roll, pitch, yaw): + """Create rotation matrix from Euler angles (ZYX convention)""" + # Rotation around x-axis (roll) + Rx = np.array([ + [1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)] + ]) + # Rotation around y-axis (pitch) + Ry = np.array([ + [np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)] + ]) + # Rotation around z-axis (yaw) + Rz = np.array([ + [np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1] + ]) + # Combined rotation: Rz * Ry * Rx + return Rz @ Ry @ Rx + + def transform_geometry(self, X, Y, Z, position, orientation): + """Apply position and orientation to geometry points""" + # Apply COM shift: subtract L/2 from X before rotation + points = np.stack([X.flatten() - (self.L / 2.0), Y.flatten(), Z.flatten()]) + + R = self.rotation_matrix(*orientation) + rotated = R @ points + translated = rotated + position.reshape(3, 1) + + X_new = translated[0].reshape(X.shape) + Y_new = translated[1].reshape(Y.shape) + Z_new = translated[2].reshape(Z.shape) + + return X_new, Y_new, Z_new + + def transform_fins(self, position, orientation): + """Transform fin vertices""" + transformed_fins = [] + R = self.rotation_matrix(*orientation) + for fin in self.fins: + fin_array = np.array(fin) + # Apply COM shift: subtract L/2 from x-coordinates + fin_array[:, 0] -= (self.L / 2.0) + + rotated = (R @ fin_array.T).T + translated = rotated + position + transformed_fins.append(translated) + return transformed_fins + + def transform_dvl(self, position, orientation): + """Transform DVL box""" + R = self.rotation_matrix(*orientation) + transformed_faces = [] + for face in self.base_geometry['dvl_faces']: + face_array = np.array(face) + + # Apply COM shift: subtract L/2 from x-coordinates + face_array_shifted = face_array.copy() + face_array_shifted[:, 0] -= (self.L / 2.0) + + rotated = (R @ face_array_shifted.T).T + translated = rotated + position + transformed_faces.append(translated) + return transformed_faces # plotVehicleStates(simTime, simData, figNo) plots the 6-DOF vehicle # position/attitude and velocities versus time in figure no. figNo From fc71a0bde8fd2b21ca2bbe24f47bd0ca0ccb39fe Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 16:43:43 +0530 Subject: [PATCH 5/9] Setting up AUV geometry model --- .../lib/plotTimeSeries.py | 152 +++++++++++------- 1 file changed, 92 insertions(+), 60 deletions(-) diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index bb06d97..eebb850 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -346,75 +346,107 @@ def plotControls(simTime, simData, vehicle, figNo): plt.grid() -# plot3D(simData,numDataPoints,FPS,filename,figNo) plots the vehicles position (x, y, z) in 3D -# in figure no. figNo -def plot3D(simData,numDataPoints,FPS,filename,figNo): - - # State vectors - x = simData[:,0] - y = simData[:,1] - z = simData[:,2] +# plot3D(simData,numDataPoints,FPS,filename,figNo) plots the vehicles position +# (x, y, z) in 3D in figure no. figNo +def plot3D(simData, numDataPoints, FPS, filename, figNo): + """ + Animates the AUV's 3D trajectory using the full vehicle geometry, + with the COM on the trajectory and the nose pointing along the + physical vehicle heading (psi). + """ + + # AUV geometry model + # Parameters from remus100.py + L = 1.6 + D = 0.19 + + # Parameters from auv_model.py + old_L = 1.33 + old_a = 0.191 + old_a_offset = 0.0165 + old_c_offset = 0.0368 + old_lf = 0.828 + + scale_ratio = L / old_L + + auv_geo = { + 'a': old_a * scale_ratio, + 'a_offset': old_a_offset * scale_ratio, + 'c_offset': old_c_offset * scale_ratio, + 'n': 2, + 'd': D, + 'lf': old_lf * scale_ratio, + 'l': L, + } + + auv_model = AUVGeometry(auv_geo) + + + # # State vectors + # x = simData[:,0] + # y = simData[:,1] + # z = simData[:,2] - # down-sampling the xyz data points - N = y[::len(x) // numDataPoints]; - E = x[::len(x) // numDataPoints]; - D = z[::len(x) // numDataPoints]; + # # down-sampling the xyz data points + # N = y[::len(x) // numDataPoints]; + # E = x[::len(x) // numDataPoints]; + # D = z[::len(x) // numDataPoints]; - # Animation function - def anim_function(num, dataSet, line): + # # Animation function + # def anim_function(num, dataSet, line): - line.set_data(dataSet[0:2, :num]) - line.set_3d_properties(dataSet[2, :num]) - ax.view_init(elev=10.0, azim=-120.0) + # line.set_data(dataSet[0:2, :num]) + # line.set_3d_properties(dataSet[2, :num]) + # ax.view_init(elev=10.0, azim=-120.0) - return line + # return line - dataSet = np.array([E, N, -D]) # (East, North, Up) + # dataSet = np.array([E, N, -D]) # (East, North, Up) - # Attaching 3D axis to the figure - fig = plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), - dpi=dpiValue) - ax = p3.Axes3D(fig, auto_add_to_figure=False) - fig.add_axes(ax) + # # Attaching 3D axis to the figure + # fig = plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), + # dpi=dpiValue) + # ax = p3.Axes3D(fig, auto_add_to_figure=False) + # fig.add_axes(ax) - # Line/trajectory plot - line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='b', label='Trajectory')[0] - - # Add Start and End markers - ax.plot(dataSet[0, 0], dataSet[1, 0], dataSet[2, 0], 'go', markerfacecolor='green', markersize=5, label='Start') - ax.plot(dataSet[0, -1], dataSet[1, -1], dataSet[2, -1], 'rs', markerfacecolor='red', markersize=5, label='End') - ax.legend() - - # Setting the axes properties - ax.set_xlabel('X / East') - ax.set_ylabel('Y / North') - ax.set_zlim3d([-100, 20]) # default depth = -100 m + # # Line/trajectory plot + # line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='b', label='Trajectory')[0] + + # # Add Start and End markers + # ax.plot(dataSet[0, 0], dataSet[1, 0], dataSet[2, 0], 'go', markerfacecolor='green', markersize=5, label='Start') + # ax.plot(dataSet[0, -1], dataSet[1, -1], dataSet[2, -1], 'rs', markerfacecolor='red', markersize=5, label='End') + # ax.legend() + + # # Setting the axes properties + # ax.set_xlabel('X / East') + # ax.set_ylabel('Y / North') + # ax.set_zlim3d([-100, 20]) # default depth = -100 m - if np.amax(z) > 100.0: - ax.set_zlim3d([-np.amax(z), 20]) + # if np.amax(z) > 100.0: + # ax.set_zlim3d([-np.amax(z), 20]) - ax.set_zlabel('-Z / Down') - - # Plot 2D surface for z = 0 - [x_min, x_max] = ax.get_xlim() - [y_min, y_max] = ax.get_ylim() - x_grid = np.arange(x_min-20, x_max+20) - y_grid = np.arange(y_min-20, y_max+20) - [xx, yy] = np.meshgrid(x_grid, y_grid) - zz = 0 * xx - ax.plot_surface(xx, yy, zz, alpha=0.3) + # ax.set_zlabel('-Z / Down') + + # # Plot 2D surface for z = 0 + # [x_min, x_max] = ax.get_xlim() + # [y_min, y_max] = ax.get_ylim() + # x_grid = np.arange(x_min-20, x_max+20) + # y_grid = np.arange(y_min-20, y_max+20) + # [xx, yy] = np.meshgrid(x_grid, y_grid) + # zz = 0 * xx + # ax.plot_surface(xx, yy, zz, alpha=0.3) - # Title of plot - ax.set_title('North-East-Down') + # # Title of plot + # ax.set_title('North-East-Down') - # Create the animation object - ani = animation.FuncAnimation(fig, - anim_function, - frames=numDataPoints, - fargs=(dataSet,line), - interval=200, - blit=False, - repeat=True) + # # Create the animation object + # ani = animation.FuncAnimation(fig, + # anim_function, + # frames=numDataPoints, + # fargs=(dataSet,line), + # interval=200, + # blit=False, + # repeat=True) - # Save the 3D animation as a gif file - ani.save(filename, writer=animation.PillowWriter(fps=FPS)) \ No newline at end of file + # # Save the 3D animation as a gif file + # ani.save(filename, writer=animation.PillowWriter(fps=FPS)) \ No newline at end of file From ad6c07124117f1b0684dd6597b205f447ba54a95 Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 16:55:21 +0530 Subject: [PATCH 6/9] Extract and downsample simulation data --- .../lib/plotTimeSeries.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index eebb850..fafe0eb 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -381,6 +381,23 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): auv_model = AUVGeometry(auv_geo) + # Downsample simData to numDataPoints + step = max(1, len(simData // numDataPoints)) + data = simData[::step] + + # Extract states (N, E, D, phi, theta, psi) + n = data[:, 0] # North + e = data[:, 1] # East + d = data[:, 2] # Down + phi = data[:, 3] # Roll + theta = data[:, 4] # Pitch + psi = data[:, 5] # Yaw + + # Convert to plot coordinates (E, N, U) + x_plot = e + y_plot = n + z_plot = -d #Up is negative down + # # State vectors # x = simData[:,0] From bf3899449f79fdf12411924e6c7951e00de832e1 Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 17:00:39 +0530 Subject: [PATCH 7/9] Set up figure --- .../lib/plotTimeSeries.py | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index fafe0eb..748dc58 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -396,8 +396,31 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): # Convert to plot coordinates (E, N, U) x_plot = e y_plot = n - z_plot = -d #Up is negative down - + z_plot = -d # Up is negative down + + # Setup the figure + fig = plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), + dpi=dpiValue) + ax = p3.Axes3D(fig, auto_add_to_figure=False) + fig.add_axes(ax) + + # Store artist handles + auv_artists = { + 'nose': None, 'mid': None, 'tail': None, + 'dvl': None, 'fins': [], + } + + # Plot full trajectory line + ax.plot(x_plot, y_plot, z_plot, lw=2, c='b', label='Trajectory', alpha=0.5) + ax.plot(x_plot[0:1], y_plot[0:1], z_plot[0:1], 'go', markerfacecolor='green', markersize=8, label='Start') + ax.plot(x_plot[-1:], y_plot[-1:], z_plot[-1:], 'rs', markerfacecolor='red', markersize=8, label='End') + ax.legend(fontsize=legendSize) + + # Set axis lables + ax.set_xlabel('X / East (m)') + ax.set_ylabel('Y / North (m)') + ax.set_zlabel('Z / Up (m)') + ax.set_title('3D AUV Simulation') # # State vectors # x = simData[:,0] From 53a44f70b1f619887eb221f1a8c2c24796a71dfd Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 17:02:55 +0530 Subject: [PATCH 8/9] Define animation update function --- .../lib/plotTimeSeries.py | 61 +++++++++++++++++++ 1 file changed, 61 insertions(+) diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index 748dc58..dd31897 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -422,6 +422,67 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): ax.set_zlabel('Z / Up (m)') ax.set_title('3D AUV Simulation') + def update_frame(num): + + # Get Physical State + position = np.array([x_plot[num], y_plot[num], z_plot[num]]) + + # Create the orientation vector from the physical sim data + orientation = np.array([ + phi[num], # Physical Roll + theta[num], # Physical Pitch + psi[num] # Physical Yaw + ]) + + # Clear previous AUV artists + if auv_artists['nose']: auv_artists['nose'].remove() + if auv_artists['mid']: auv_artists['mid'].remove() + if auv_artists['tail']: auv_artists['tail'].remove() + if auv_artists['dvl']: auv_artists['dvl'].remove() + for art in auv_artists['fins']: art.remove() + auv_artists['fins'].clear() + + # Plot Nose + Xn, Yn, Zn = auv_model.transform_geometry( + *auv_model.base_geometry['nose'], position, orientation) + auv_artists['nose'] = ax.plot_surface(Xn, Yn, Zn, color='blue', alpha=0.7, rstride=3, cstride=3) + + # Plot Mid + Xm, Ym, Zm = auv_model.transform_geometry( + *auv_model.base_geometry['mid'], position, orientation) + auv_artists['mid'] = ax.plot_surface(Xm, Ym, Zm, color='green', alpha=0.7, rstride=3, cstride=3) + + # Plot Tail + Xt, Yt, Zt = auv_model.transform_geometry( + *auv_model.base_geometry['tail'], position, orientation) + auv_artists['tail'] = ax.plot_surface(Xt, Yt, Zt, color='red', alpha=0.7, rstride=3, cstride=3) + + # Plot DVL + dvl_faces = auv_model.transform_dvl(position, orientation) + auv_artists['dvl'] = Poly3DCollection(dvl_faces, facecolors='orange', alpha=0.9) + ax.add_collection3d(auv_artists['dvl']) + + # Plot Fins + fin_faces = auv_model.transform_fins(position, orientation) + for fin_verts in fin_faces: + fin_col = Poly3DCollection([fin_verts], facecolors='darkslategrey', alpha=0.9) + ax.add_collection3d(fin_col) + auv_artists['fins'].append(fin_col) + + # Set camera to follow AUV + offset = 4.0 # Camera distance + ax.set_xlim(position[0] - offset, position[0] + offset) # East + ax.set_ylim(position[1] - offset, position[1] + offset) # North + ax.set_zlim(position[2] - offset, position[2] + offset) # Up + + # Set view angle + ax.view_init(elev=20.0, azim=45.0) + + # Return a list of the artists to be drawn + all_artists = [auv_artists['nose'], auv_artists['mid'], auv_artists['tail'], auv_artists['dvl']] + all_artists.extend(auv_artists['fins']) + return all_artists + # # State vectors # x = simData[:,0] # y = simData[:,1] From 5fe40e5779367674f9b4ff09dfac2372b24d29b3 Mon Sep 17 00:00:00 2001 From: JustA3DObject Date: Mon, 10 Nov 2025 17:10:08 +0530 Subject: [PATCH 9/9] Create and save animation --- .../lib/plotTimeSeries.py | 103 +++++------------- 1 file changed, 26 insertions(+), 77 deletions(-) diff --git a/src/python_vehicle_simulator/lib/plotTimeSeries.py b/src/python_vehicle_simulator/lib/plotTimeSeries.py index dd31897..4b9db2b 100644 --- a/src/python_vehicle_simulator/lib/plotTimeSeries.py +++ b/src/python_vehicle_simulator/lib/plotTimeSeries.py @@ -34,7 +34,7 @@ def cm2inch(value): # inch to cm class AUVGeometry: """ - Handles the generation and transformation of the 3D AUV model geometry. + Handles the generation and transformation of AUV 3D model geometry. """ def __init__(self, geometry): self.geometry = geometry @@ -248,7 +248,7 @@ def plotVehicleStates(simTime, simData, figNo): plt.grid() plt.subplot(3, 3, 1) - plt.plot(y, x, label='Trajectory') + plt.plot(y, x, label='Trajectory') # y=North, x=East plt.plot(y[0], x[0], 'go', markersize=5, label='Start') # Start marker plt.plot(y[-1], x[-1], 'rs', markersize=5, label='End') # End marker plt.legend(fontsize=legendSize) @@ -357,7 +357,7 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): # AUV geometry model # Parameters from remus100.py - L = 1.6 + L = 1.6 D = 0.19 # Parameters from auv_model.py @@ -368,7 +368,7 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): old_lf = 0.828 scale_ratio = L / old_L - + auv_geo = { 'a': old_a * scale_ratio, 'a_offset': old_a_offset * scale_ratio, @@ -378,11 +378,11 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): 'lf': old_lf * scale_ratio, 'l': L, } - + auv_model = AUVGeometry(auv_geo) # Downsample simData to numDataPoints - step = max(1, len(simData // numDataPoints)) + step = max(1, len(simData) // numDataPoints) data = simData[::step] # Extract states (N, E, D, phi, theta, psi) @@ -398,7 +398,6 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): y_plot = n z_plot = -d # Up is negative down - # Setup the figure fig = plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), dpi=dpiValue) ax = p3.Axes3D(fig, auto_add_to_figure=False) @@ -415,13 +414,14 @@ def plot3D(simData, numDataPoints, FPS, filename, figNo): ax.plot(x_plot[0:1], y_plot[0:1], z_plot[0:1], 'go', markerfacecolor='green', markersize=8, label='Start') ax.plot(x_plot[-1:], y_plot[-1:], z_plot[-1:], 'rs', markerfacecolor='red', markersize=8, label='End') ax.legend(fontsize=legendSize) - - # Set axis lables + + # Set axis labels ax.set_xlabel('X / East (m)') ax.set_ylabel('Y / North (m)') ax.set_zlabel('Z / Up (m)') ax.set_title('3D AUV Simulation') - + + # Animation Update Function def update_frame(num): # Get Physical State @@ -456,7 +456,7 @@ def update_frame(num): Xt, Yt, Zt = auv_model.transform_geometry( *auv_model.base_geometry['tail'], position, orientation) auv_artists['tail'] = ax.plot_surface(Xt, Yt, Zt, color='red', alpha=0.7, rstride=3, cstride=3) - + # Plot DVL dvl_faces = auv_model.transform_dvl(position, orientation) auv_artists['dvl'] = Poly3DCollection(dvl_faces, facecolors='orange', alpha=0.9) @@ -483,71 +483,20 @@ def update_frame(num): all_artists.extend(auv_artists['fins']) return all_artists - # # State vectors - # x = simData[:,0] - # y = simData[:,1] - # z = simData[:,2] - - # # down-sampling the xyz data points - # N = y[::len(x) // numDataPoints]; - # E = x[::len(x) // numDataPoints]; - # D = z[::len(x) // numDataPoints]; - - # # Animation function - # def anim_function(num, dataSet, line): - - # line.set_data(dataSet[0:2, :num]) - # line.set_3d_properties(dataSet[2, :num]) - # ax.view_init(elev=10.0, azim=-120.0) - - # return line - - # dataSet = np.array([E, N, -D]) # (East, North, Up) - - # # Attaching 3D axis to the figure - # fig = plt.figure(figNo,figsize=(cm2inch(figSize1[0]),cm2inch(figSize1[1])), - # dpi=dpiValue) - # ax = p3.Axes3D(fig, auto_add_to_figure=False) - # fig.add_axes(ax) - - # # Line/trajectory plot - # line = plt.plot(dataSet[0], dataSet[1], dataSet[2], lw=2, c='b', label='Trajectory')[0] - - # # Add Start and End markers - # ax.plot(dataSet[0, 0], dataSet[1, 0], dataSet[2, 0], 'go', markerfacecolor='green', markersize=5, label='Start') - # ax.plot(dataSet[0, -1], dataSet[1, -1], dataSet[2, -1], 'rs', markerfacecolor='red', markersize=5, label='End') - # ax.legend() - - # # Setting the axes properties - # ax.set_xlabel('X / East') - # ax.set_ylabel('Y / North') - # ax.set_zlim3d([-100, 20]) # default depth = -100 m - - # if np.amax(z) > 100.0: - # ax.set_zlim3d([-np.amax(z), 20]) - - # ax.set_zlabel('-Z / Down') - - # # Plot 2D surface for z = 0 - # [x_min, x_max] = ax.get_xlim() - # [y_min, y_max] = ax.get_ylim() - # x_grid = np.arange(x_min-20, x_max+20) - # y_grid = np.arange(y_min-20, y_max+20) - # [xx, yy] = np.meshgrid(x_grid, y_grid) - # zz = 0 * xx - # ax.plot_surface(xx, yy, zz, alpha=0.3) - - # # Title of plot - # ax.set_title('North-East-Down') + # Create and Save Animation + # Set initial bounds to be reasonable + ax.set_xlim(np.min(x_plot)-2, np.max(x_plot)+2) + ax.set_ylim(np.min(y_plot)-2, np.max(y_plot)+2) + ax.set_zlim(np.min(z_plot)-2, np.max(z_plot)+2) - # # Create the animation object - # ani = animation.FuncAnimation(fig, - # anim_function, - # frames=numDataPoints, - # fargs=(dataSet,line), - # interval=200, - # blit=False, - # repeat=True) + print(f"\nGenerating 3D animation ({len(x_plot)} frames)...") - # # Save the 3D animation as a gif file - # ani.save(filename, writer=animation.PillowWriter(fps=FPS)) \ No newline at end of file + ani = animation.FuncAnimation(fig, + update_frame, + frames=len(x_plot), # Number of frames + interval=max(1, 1000/FPS), # ms + blit=False) # Blit=False is required for 3D + + # Save the 3D animation as a gif file + ani.save(filename, writer=animation.PillowWriter(fps=FPS)) + print(f"Animation saved to {filename}") \ No newline at end of file