From b1d1ad517a52aa4987762f9518ecc45842ec4f22 Mon Sep 17 00:00:00 2001 From: Angoosh Date: Mon, 4 Aug 2025 12:20:31 +0200 Subject: [PATCH] added visualization code, generated --- visualization.py | 97 +++++++++++++++++++++++++++++++++++++++++ visualization_no_usb.py | 91 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 188 insertions(+) create mode 100644 visualization.py create mode 100644 visualization_no_usb.py diff --git a/visualization.py b/visualization.py new file mode 100644 index 0000000..f25db2a --- /dev/null +++ b/visualization.py @@ -0,0 +1,97 @@ +import serial +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +from matplotlib.animation import FuncAnimation +import re + +# Serial settings (change COM port and baud rate as needed) +SERIAL_PORT = 'COM3' # e.g., 'COM3' on Windows or '/dev/ttyUSB0' on Linux/Mac +BAUD_RATE = 9600 + +# Open serial connection +ser = serial.Serial(SERIAL_PORT, BAUD_RATE, timeout=1) + +# Parse "roll: xx, pitch: xx, yaw: xx" +def parse_data(line): + match = re.search(r'roll:\s*(-?\d+\.?\d*),\s*pitch:\s*(-?\d+\.?\d*),\s*yaw:\s*(-?\d+\.?\d*)', line) + if match: + roll = float(match.group(1)) + pitch = float(match.group(2)) + yaw = float(match.group(3)) + return roll, pitch, yaw + return None + +def euler_to_rotation_matrix(roll, pitch, yaw): + roll = np.radians(roll) + pitch = np.radians(pitch) + yaw = np.radians(yaw) + + Rx = np.array([[1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)]]) + + Ry = np.array([[np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)]]) + + Rz = np.array([[np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1]]) + + return Rz @ Ry @ Rx + +def create_box(): + l, w, h = 1, 0.5, 0.2 + return np.array([ + [-l/2, -w/20, -h/2], + [ l/2, -w/2, -h/2], + [ l/2, w/2, -h/2], + [-l/2, w/20, -h/2], + [-l/2, -w/20, h/2], + [ l/2, -w/2, h/2], + [ l/2, w/2, h/2], + [-l/2, w/20, h/2], + ]) + +def get_faces(vertices): + return [ + [vertices[j] for j in [0,1,2,3]], + [vertices[j] for j in [4,5,6,7]], + [vertices[j] for j in [0,1,5,4]], + [vertices[j] for j in [2,3,7,6]], + [vertices[j] for j in [1,2,6,5]], + [vertices[j] for j in [3,0,4,7]], + ] + +# Create plot +fig = plt.figure() +ax = fig.add_subplot(111, projection='3d') +box = create_box() +collection = Poly3DCollection([], facecolors='skyblue', edgecolors='black', alpha=0.8) +ax.add_collection3d(collection) + +ax.set_xlim([-1, 1]) +ax.set_ylim([-1, 1]) +ax.set_zlim([-1, 1]) +ax.set_xlabel("X") +ax.set_ylabel("Y") +ax.set_zlabel("Z") +ax.view_init(elev=30, azim=45) + +# Animation update function +def update(frame): + line = ser.readline().decode('utf-8').strip() + result = parse_data(line) + if result: + roll, pitch, yaw = result + R = euler_to_rotation_matrix(roll, pitch, yaw) + rotated_box = box @ R.T + faces = get_faces(rotated_box) + collection.set_verts(faces) + ax.set_title(f"Roll={roll:.1f}°, Pitch={pitch:.1f}°, Yaw={yaw:.1f}°") + +ani = FuncAnimation(fig, update, interval=50) + +plt.tight_layout() +plt.show() diff --git a/visualization_no_usb.py b/visualization_no_usb.py new file mode 100644 index 0000000..a3a6b2e --- /dev/null +++ b/visualization_no_usb.py @@ -0,0 +1,91 @@ +import numpy as np +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d.art3d import Poly3DCollection + +def euler_to_rotation_matrix(roll, pitch, yaw): + # Convert to radians + roll = np.radians(roll) + pitch = np.radians(pitch) + yaw = np.radians(yaw) + + # Rotation matrices + Rx = np.array([ + [1, 0, 0], + [0, np.cos(roll), -np.sin(roll)], + [0, np.sin(roll), np.cos(roll)] + ]) + + Ry = np.array([ + [np.cos(pitch), 0, np.sin(pitch)], + [0, 1, 0], + [-np.sin(pitch), 0, np.cos(pitch)] + ]) + + Rz = np.array([ + [np.cos(yaw), -np.sin(yaw), 0], + [np.sin(yaw), np.cos(yaw), 0], + [0, 0, 1] + ]) + + return Rz @ Ry @ Rx # ZYX order + +def create_box(): + # Define 8 corners of a box centered at origin + l, w, h = 1, 0.5, 0.2 # length, width, height + corners = np.array([ + [-l/2, -w/20, -h/2], + [ l/2, -w/2, -h/2], + [ l/2, w/2, -h/2], + [-l/2, w/20, -h/2], + [-l/2, -w/20, h/2], + [ l/2, -w/2, h/2], + [ l/2, w/2, h/2], + [-l/2, w/20, h/2], + ]) + return corners + +def get_faces(vertices): + # Faces defined by vertices index + faces = [ + [vertices[j] for j in [0,1,2,3]], # Bottom + [vertices[j] for j in [4,5,6,7]], # Top + [vertices[j] for j in [0,1,5,4]], # Front + [vertices[j] for j in [2,3,7,6]], # Back + [vertices[j] for j in [1,2,6,5]], # Right + [vertices[j] for j in [3,0,4,7]], # Left + ] + return faces + +def plot_box(roll, pitch, yaw): + box = create_box() + R = euler_to_rotation_matrix(roll, pitch, yaw) + + # Rotate box + rotated_box = box @ R.T # Apply rotation + + faces = get_faces(rotated_box) + + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + + # Draw box + ax.add_collection3d(Poly3DCollection(faces, facecolors='skyblue', edgecolors='black', linewidths=1, alpha=0.9)) + + # Axis setup + ax.set_xlim([-1, 1]) + ax.set_ylim([-1, 1]) + ax.set_zlim([-1, 1]) + ax.set_xlabel('X') + ax.set_ylabel('Y') + ax.set_zlabel('Z') + ax.set_title(f"3D Box Orientation\nRoll={roll}°, Pitch={pitch}°, Yaw={yaw}°") + ax.view_init(elev=30, azim=45) + plt.tight_layout() + plt.show() + +# Example usage +if __name__ == "__main__": + roll = float(input("Enter roll (degrees): ")) + pitch = float(input("Enter pitch (degrees): ")) + yaw = float(input("Enter yaw (degrees): ")) + plot_box(roll, pitch, yaw)