How to prevent sudden changes from -180 to 180 degree when converting from quaternions to euler?
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation as R
from scipy.fft import fft, fftfreq
import math

import math
# Set tight layout for all plots
plt.rcParams['figure.autolayout'] = True
def wrist_rototranslation(q1, q2, q3, q4, q5, q6):
    """
    Calculate the rototranslation matrix based on joint parameters q1..q6 .
    
    Args:
        q1, q2, q3, q4, q5, q6: Joint parameters (in radians).
    
    Returns:
        A 4x4 rototranslation matrix as a list of lists.
    """
    a1 = 410
    a2 = 1075
    a3 = 165
    d1 = 780
    d4 = 1556
    d6 = 250
    # Compute sines and cosines of the joint angles
    s1 = math.sin(q1)
    c1 = math.cos(q1)
    s2 = math.sin(q2)
    c2 = math.cos(q2)
    s3 = math.sin(q3)
    c3 = math.cos(q3)
    s4 = math.sin(q4)
    c4 = math.cos(q4)
    
    # Compute sine and cosine of (q2 + q3)
    s23 = math.sin(q2 + q3)
    c23 = math.cos(q2 + q3)
    
    # First row
    m00 = c1 * c4 * s23 + s1 * s4
    m01 = c1 * c23
    m02 = -s1 * c4 + c1 * s23 * s4
    m03 = c1 * (a1 + d4 * c23 + a2 * s2 + a3 * s23)
    
    # Second row
    m10 = s1 * c4 * s23 - c1 * s4
    m11 = s1 * c23
    m12 = c1 * c4 + s1 * s23 * s4
    m13 = s1 * (a1 + d4 * c23 + a2 * s2 + a3 * s23)
    
    # Third row
    m20 = c23 * c4
    m21 = -s23
    m22 = c23 * s4
    m23 = d1 + a2 * c2 + a3 * c23 - d4 * s23
    
    # Fourth row
    m30 = 0
    m31 = 0
    m32 = 0
    m33 = 1
    
    # Construct the rototranslation matrix
    matrix = [
        [m00, m01, m02, m03],
        [m10, m11, m12, m13],
        [m20, m21, m22, m23],
        [m30, m31, m32, m33]
    ]
    
    return matrix

def wrap_angle(angle):
    """
    Wrap an angle in radians to the range [-pi, pi].

    Args:
        angle: Angle in radians.

    Returns:
        Wrapped angle in radians.
    """
    return (angle + np.pi) % (2 * np.pi) - np.pi
def extract_wrist_position(df):
    wrist_positions =[]

    for index, row in df.iterrows():
        q1, q2, q3, q4, q5, q6 = row['J1'], row['J2'], row['J3'], row['J4'], row['J5'], row['J6']
        matrix = wrist_rototranslation(q1*np.pi/180, q2*np.pi/180, q3*np.pi/180, q4*np.pi/180, q5*np.pi/180, q6*np.pi/180)
        # q1rad, q2rad, q3rad, q4rad, q5rad, q6rad = math.radians(q1), math.radians(q2-90), math.radians(q3), math.radians(q4), math.radians(q5), math.radians(q6)

        # matrix = wrist_rototranslation(q1rad, q2rad, q3rad, q4rad, q5rad, q6rad)
        # matrix = wrist_rototranslation(np.wrap(math.radians(q1),),math.radians(q2-90),math.radians(q3),math.radians(q4),math.radians(q5),math.radians(q6)+np.pi)
        wrist_positions.append([matrix[0][3], matrix[1][3], matrix[2][3]])

    return np.array(wrist_positions)

# Function to read the data from the txt file
def read_data(filename):
    with open(filename, 'r') as f:
        lines = f.readlines()

    # Find the index of the line that starts with 'Time'
    for idx, line in enumerate(lines):
        if line.strip().startswith('Time'):
            header_idx = idx
            break

    # Read the data from that line onwards
    df = pd.read_csv(filename, delimiter='\t', skiprows=header_idx + 1, header=None)

    # Set column names (adjusted to include the new column)
    columns = ['Time', 'X', 'Y', 'Z', 'Q1', 'Q2', 'Q3', 'Q4', 'Speed', 'Linear_Acceleration', 'cf1','cf4','cf6','cfx', 'J1', 'J2','J3','J4','J5','J6']
    df.columns = columns

    return df

# Adjust the time column by removing 3 ms and rescaling
def adjust_time(df, msInit, msLast):
    max_time = df['Time'].max()
    # Filter out the first 3 ms
    df = df[df['Time'] > msInit].reset_index(drop=True)
    df = df[df['Time'] < (max_time - msLast)].reset_index(drop=True)
    # Rescale the time to start from 0
    df['Time'] = df['Time'] - df['Time'].min()
    return df

# Calculate the fixed point's position and orientation in the world frame
def calculate_fixed_point_positions(df):
    # Fixed point in end-effector frame
    P_fixed_ee = np.array([0, 0, 253])  # mm # For HYD
    # P_fixed_ee = np.array([0, 0, -900])  # mm # For O2 LAnce


    # Initialize lists to collect positions and orientations
    P_fixed_world_list = []
    euler_fixed_list = []

    P_ee_world_list = []
    euler_ee_list = []

    for index, row in df.iterrows():
        # Extract end-effector position
        P_ee_world = np.array([row['X'], row['Y'], row['Z']])

        # Extract quaternion
        q = np.array([row['Q2'], row['Q3'], row['Q4'], row['Q1']])
        q = q / np.linalg.norm(q)  # Normalize quaternion

        # Create rotation object from quaternion
        # Note: Quaternion order is assumed to be [x, y, z, w]
        r = R.from_quat([row['Q2'], row['Q3'], row['Q4'],row['Q1']])

        # Compute rotation matrix
        R_matrix = r.as_matrix()

        # Compute fixed point position in world frame
        P_fixed_world = P_ee_world + R_matrix.dot(P_fixed_ee)
        P_fixed_world_list.append(P_fixed_world)

        # Calculate the fixed point velocity in world frame

        # Get Euler angles for end-effector
        euler_ee = r.as_euler('xyz', degrees=True)  # In degrees
        euler_ee_list.append(euler_ee)

        # For the fixed point orientation, assuming no additional rotation
        euler_fixed = euler_ee  # Same as end-effector
        euler_fixed_list.append(euler_fixed)

        # Collect end-effector positions
        P_ee_world_list.append(P_ee_world)

    # Convert to numpy arrays
    P_fixed_world_array = np.array(P_fixed_world_list)  # Shape: (N, 3)
    euler_fixed_array = np.array(euler_fixed_list)

    P_ee_world_array = np.array(P_ee_world_list)
    euler_ee_array = np.array(euler_ee_list)

    return P_fixed_world_array, euler_fixed_array, P_ee_world_array, euler_ee_array


# Perform vibration analysis using provided acceleration data
def perform_vibration_analysis_with_provided_acceleration(time, acceleration):
    # Acceleration is given directly
    acc_magnitude = np.array(acceleration)

    # Compute FFT
    N = len(time)
    dt_mean = np.mean(np.diff(time))  # Average sampling interval
    T = dt_mean  # Sampling period
    yf = fft(acc_magnitude)
    xf = fftfreq(N, T)[:N // 2]

    # Only take the positive frequencies and magnitude
    yf_magnitude = 2.0 / N * np.abs(yf[0:N // 2])

    return acc_magnitude, xf, yf_magnitude


# Plotting the results including vibration analysis
def plot_results(df, P_fixed_world_array, euler_fixed_array, P_ee_world_array, euler_ee_array, acceleration):

    time = df["Time"].values

    ee_vel = df["Speed"].values

    # Positions of fixed point
    X_fixed = P_fixed_world_array[:, 0]
    Y_fixed = P_fixed_world_array[:, 1]
    Z_fixed = P_fixed_world_array[:, 2]

    # Positions of end-effector
    X_ee = P_ee_world_array[:, 0]
    Y_ee = P_ee_world_array[:, 1]
    Z_ee = P_ee_world_array[:, 2]

    # # Plot fixed point positions over time
    # plt.figure(figsize=(8,5))
    # plt.plot(time, X_fixed, label='Fixed Point X Position')
    # plt.plot(time, Y_fixed, label='Fixed Point Y Position')
    # plt.plot(time, Z_fixed, label='Fixed Point Z Position')
    # plt.xlabel('Time (s)')
    # plt.ylabel('Position (mm)')
    # plt.title('Fixed Point Position Over Time')
    # plt.legend()
    # plt.grid(True)
    # plt.show()

    # Plot end effector pos over time
    plt.figure(figsize=(8,5))
    plt.plot(time, X_ee,color='r', label='TCP X Position')
    plt.axhline(y=np.max(X_ee), color='r', linestyle='--', label='Max X-Position')
    plt.axhline(y=np.min(X_ee), color='r', linestyle='--', label='Min X-Position')
    plt.text(time[-1], np.max(X_ee), f'{np.max(X_ee):.2f}', color='r', ha='right')
    plt.text(time[-1], np.min(X_ee), f'{np.min(X_ee):.2f}', color='r', ha='right')
    plt.plot(time, Y_ee, color='g', label='TCP Y Position')
    plt.axhline(y=np.max(Y_ee), color='g', linestyle='--', label='Max Y-Position')
    plt.axhline(y=np.min(Y_ee), color='g', linestyle='--', label='Min Y-Position')
    plt.text(time[-1], np.max(Y_ee), f'{np.max(Y_ee):.2f}', color='g', ha='right')
    plt.text(time[-1], np.min(Y_ee), f'{np.min(Y_ee):.2f}', color='g', ha='right')
    plt.plot(time, Z_ee, color='b',label='TCP Z Position')
    plt.axhline(y=np.max(Z_ee), color='b', linestyle='--', label='Max Z-Position')
    plt.axhline(y=np.min(Z_ee), color='b', linestyle='--', label='Min Z-Position')
    plt.text(time[-1], np.max(Z_ee), f'{np.max(Z_ee):.2f}', color='b', ha='left')
    plt.text(time[-1], np.min(Z_ee), f'{np.min(Z_ee):.2f}', color='b', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Z-Position (mm)')
    plt.title('TCP Position Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # Plot x-end-effector positions over time
    plt.figure(figsize=(8,5))
    plt.plot(time, X_ee,color='r', label='TCP X Position')
    plt.axhline(y=np.max(X_ee), color='r', linestyle='--', label='Max X-Position')
    plt.axhline(y=np.min(X_ee), color='r', linestyle='--', label='Min X-Position')
    plt.text(time[-1], np.max(X_ee), f'{np.max(X_ee):.2f}', color='r', ha='right')
    plt.text(time[-1], np.min(X_ee), f'{np.min(X_ee):.2f}', color='r', ha='right')
    plt.xlabel('Time (s)')
    plt.ylabel('X-Position (mm)')
    plt.title('TCP X Position Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()
    plt.figure(figsize=(8,5))
    # Plot y-end-effector positions over time
    plt.plot(time, Y_ee, color='g', label='TCP Y Position')
    plt.axhline(y=np.max(Y_ee), color='g', linestyle='--', label='Max Y-Position')
    plt.axhline(y=np.min(Y_ee), color='g', linestyle='--', label='Min Y-Position')
    plt.text(time[-1], np.max(Y_ee), f'{np.max(Y_ee):.2f}', color='g', ha='right')
    plt.text(time[-1], np.min(Y_ee), f'{np.min(Y_ee):.2f}', color='g', ha='right')
    plt.xlabel('Time (s)')
    plt.ylabel('Y-Position (mm)')
    plt.title('TCP Y Position Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()
    # Plot z-end-effector positions over time
    plt.figure(figsize=(8,5))
    plt.plot(time, Z_ee, color='b',label='TCP Z Position')
    plt.axhline(y=np.max(Z_ee), color='b', linestyle='--', label='Max Z-Position')
    plt.axhline(y=np.min(Z_ee), color='b', linestyle='--', label='Min Z-Position')
    plt.text(time[-1], np.max(Z_ee), f'{np.max(Z_ee):.2f}', color='b', ha='left')
    plt.text(time[-1], np.min(Z_ee), f'{np.min(Z_ee):.2f}', color='b', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Z-Position (mm)')
    plt.title('TCP Z Position Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # Plot ee speed
    plt.figure(figsize=(8,5))
    plt.plot(time, ee_vel, color='g', label='TCP Velocity')
    # Plot max and min values as dashed lines
    plt.axhline(y=np.max(ee_vel), color='g', linestyle='--', label='Max Velocity')
    plt.axhline(y=np.min(ee_vel), color='g', linestyle='--', label='Min Velocity')
    # Annotate the max and min values
    plt.text(time[-1], np.max(ee_vel), f'{np.max(ee_vel):.2f}', color='g', ha='right')
    plt.text(time[-1], np.min(ee_vel), f'{np.min(ee_vel):.2f}', color='g', ha='right')
    plt.xlabel('Time (s)')
    plt.ylabel('Velocity (mm/s)')
    plt.title('TCP Velocity Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # Plot fixed point orientation over time (Euler angles)
    # plt.figure(figsize=(8,5))
    # plt.plot(time, euler_fixed_array[:, 0], label='Fixed Point Roll')
    # plt.plot(time, euler_fixed_array[:, 1], label='Fixed Point Pitch')
    # plt.plot(time, euler_fixed_array[:, 2], label='Fixed Point Yaw')
    # plt.xlabel('Time (s)')
    # plt.ylabel('Euler Angles (degrees)')
    # plt.title('Fixed Point Orientation Over Time')
    # plt.legend()
    # plt.grid(True)
    # plt.show()

    # Plot end-effector orientation over time (Euler angles)
    plt.figure(figsize=(8,5))
    plt.plot(time, euler_ee_array[:, 0],color='r', label='End-Effector Roll')
    plt.axhline(y=np.max(euler_ee_array[:, 0]), color='r', linestyle='--', label='Max Roll')
    plt.axhline(y=np.min(euler_ee_array[:, 0]), color='r', linestyle='--', label='Min Roll')
    plt.text(time[-1], np.max(euler_ee_array[:, 0]), f'{np.max(euler_ee_array[:, 0]):.2f}', color='r', va='bottom', ha='right')
    plt.text(time[0], np.min(euler_ee_array[:, 0]), f'{np.min(euler_ee_array[:, 0]):.2f}', color='r', va='top', ha='left')
    plt.plot(time, euler_ee_array[:, 1],color='g', label='End-Effector Pitch')
    plt.axhline(y=np.max(euler_ee_array[:, 1]), color='g', linestyle='--', label='Max Pitch')
    plt.axhline(y=np.min(euler_ee_array[:, 1]), color='g', linestyle='--', label='Min Pitch')
    plt.text(time[-1], np.max(euler_ee_array[:, 1]), f'{np.max(euler_ee_array[:, 1]):.2f}', color='g', va='bottom', ha='left')
    plt.text(time[-1], np.min(euler_ee_array[:, 1]), f'{np.min(euler_ee_array[:, 1]):.2f}', color='g', va='bottom', ha='right')
    plt.plot(time, euler_ee_array[:, 2],color='b', label='End-Effector Yaw')
    plt.axhline(y=np.max(euler_ee_array[:, 2]), color='b', linestyle='--', label='Max Yaw')
    plt.axhline(y=np.min(euler_ee_array[:, 2]), color='b', linestyle='--', label='Min Yaw')
    plt.text(time[-1], np.max(euler_ee_array[:, 2]), f'{np.max(euler_ee_array[:, 2]):.2f}', color='b', va='bottom', ha='right')
    plt.text(time[-1], np.min(euler_ee_array[:, 2]), f'{np.min(euler_ee_array[:, 2]):.2f}', color='b', va='top', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Euler Angles (degrees)')
    plt.title('TCP Orientation Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    plt.figure(figsize=(8,5))
    plt.plot(time, euler_ee_array[:, 0],color='r', label='End-Effector Roll')
    plt.axhline(y=np.max(euler_ee_array[:, 0]), color='r', linestyle='--', label='Max Roll')
    plt.axhline(y=np.min(euler_ee_array[:, 0]), color='r', linestyle='--', label='Min Roll')
    plt.text(time[-1], np.max(euler_ee_array[:, 0]), f'{np.max(euler_ee_array[:, 0]):.2f}', color='r', va='bottom', ha='right')
    plt.text(time[0], np.min(euler_ee_array[:, 0]), f'{np.min(euler_ee_array[:, 0]):.2f}', color='r', va='top', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Euler Angles (degrees)')
    plt.title('TCP Roll Orientation Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()
    plt.figure(figsize=(8,5))
    plt.plot(time, euler_ee_array[:, 1],color='g', label='End-Effector Pitch')
    plt.axhline(y=np.max(euler_ee_array[:, 1]), color='g', linestyle='--', label='Max Pitch')
    plt.axhline(y=np.min(euler_ee_array[:, 1]), color='g', linestyle='--', label='Min Pitch')
    plt.text(time[-1], np.max(euler_ee_array[:, 1]), f'{np.max(euler_ee_array[:, 1]):.2f}', color='g', va='bottom', ha='left')
    plt.text(time[-1], np.min(euler_ee_array[:, 1]), f'{np.min(euler_ee_array[:, 1]):.2f}', color='g', va='bottom', ha='right')
    plt.xlabel('Time (s)')
    plt.ylabel('Euler Angles (degrees)')
    plt.title('TCP Pitch Orientation Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()
    plt.figure(figsize=(8, 5))
    plt.plot(time, euler_ee_array[:, 2],color='b', label='End-Effector Yaw')
    plt.axhline(y=np.max(euler_ee_array[:, 2]), color='b', linestyle='--', label='Max Yaw')
    plt.axhline(y=np.min(euler_ee_array[:, 2]), color='b', linestyle='--', label='Min Yaw')
    plt.text(time[-1], np.max(euler_ee_array[:, 2]), f'{np.max(euler_ee_array[:, 2]):.2f}', color='b', va='bottom', ha='right')
    plt.text(time[-1], np.min(euler_ee_array[:, 2]), f'{np.min(euler_ee_array[:, 2]):.2f}', color='b', va='top', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Euler Angles (degrees)')
    plt.title('TCP Yaw Orientation Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # Plot orientation in quaternion
    plt.figure(figsize=(8,5))
    plt.plot(time, df['Q1'].values, label='Q1 values')
    plt.plot(time, df['Q2'].values, label='Q2 values')
    plt.plot(time, df['Q3'].values, label='Q3 values')
    plt.plot(time, df['Q4'].values, label='Q4 values')
    plt.xlabel('Time (s)')
    plt.ylabel('Quaternions')
    plt.title('TCP Quaternion Orientation Over Time')
    plt.legend()
    plt.grid()
    plt.show()

    # Plot fixed point 3D trajectory
    # fig = plt.figure(figsize=(10, 8))
    # ax = fig.add_subplot(111, projection='3d')
    # ax.plot(X_fixed, Y_fixed, Z_fixed, label='Fixed Point Trajectory')
    # ax.set_xlabel('X Position (mm)')
    # ax.set_ylabel('Y Position (mm)')
    # ax.set_zlabel('Z Position (mm)')
    # ax.set_title('Fixed Point Trajectory')
    # ax.legend()
    # plt.show()

    # Plot end-effector 3D trajectory
    # fig = plt.figure(figsize=(10, 8))
    # ax = fig.add_subplot(111, projection='3d')
    # ax.plot(X_ee, Y_ee, Z_ee, label='End-Effector Trajectory')
    # ax.set_xlabel('X Position (mm)')
    # ax.set_ylabel('Y Position (mm)')
    # ax.set_zlabel('Z Position (mm)')
    # ax.set_title('End-Effector Trajectory')
    # ax.legend()
    # plt.show()

    # Displacement from initial position (Fixed Point)
    P_fixed0 = P_fixed_world_array[0]
    displacement_fixed = np.linalg.norm(P_fixed_world_array - P_fixed0, axis=1)

    # Plot displacement over time (Fixed Point)
    # plt.figure(figsize=(8,5))
    # plt.plot(time, displacement_fixed)
    # plt.xlabel('Time (s)')
    # plt.ylabel('Displacement from Initial Position (mm)')
    # plt.title('Fixed Point Displacement Over Time')
    # plt.grid(True)
    # plt.show()

    # Displacement from initial position (End-Effector)
    P_ee0 = P_ee_world_array[0]
    displacement_ee = np.linalg.norm(P_ee_world_array - P_ee0, axis=1)

    # Plot displacement over time (End-Effector)
    # plt.figure(figsize=(8,5))
    # plt.plot(time, displacement_ee)
    # plt.xlabel('Time (s)')
    # plt.ylabel('Displacement from Initial Position (mm)')
    # plt.title('End-Effector Displacement Over Time')
    # plt.grid(True)
    # plt.show()

    # Fixed Point Error calculation
    error_ee = np.array(P_ee_world_array - P_ee0)
    # Calculate maximum and minimum values of the error
    max_error_ee = np.max(error_ee, axis=0)
    min_error_ee = np.min(error_ee, axis=0)

    # Plot error over time with highlighted max and min values
    plt.figure(figsize=(8,5))
    plt.plot(time, error_ee[:, 0], color='r', label='X-Error')
    # plt.plot(time, error_ee[:, 1], color='g', label='Y-Error')
    plt.plot(time, error_ee[:, 2], color='b', label='Z-Error')
    # Add horizontal dashed lines for max and min values
    plt.axhline(y=max_error_ee[0], color='r', linestyle='--', label='Max X-Error')
    plt.axhline(y=min_error_ee[0], color='r', linestyle='--', label='Min X-Error')
    # plt.axhline(y=max_error_ee[1], color='g', linestyle='--', label='Max Y-Error')
    # plt.axhline(y=min_error_ee[1], color='g', linestyle='--', label='Min Y-Error')
    plt.axhline(y=max_error_ee[2], color='b', linestyle='--', label='Max Z-Error')
    plt.axhline(y=min_error_ee[2], color='b', linestyle='--', label='Min Z-Error')
    # Annotate the max and min values
    plt.text(time[-1], max_error_ee[0], f'{max_error_ee[0]:.2f}', color='r', ha='right', )
    plt.text(time[-1], min_error_ee[0], f'{min_error_ee[0]:.2f}', color='r', ha='right')
    # plt.text(time[-1], max_error_ee[1], f'{max_error_ee[1]:.2f}', color='g', ha='right')
    # plt.text(time[-1], min_error_ee[1], f'{min_error_ee[1]:.2f}', color='g', ha='right')
    plt.text(time[-1], max_error_ee[2], f'{max_error_ee[2]:.2f}', color='b', ha='left')
    plt.text(time[-1], min_error_ee[2], f'{min_error_ee[2]:.2f}', color='b', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Error (mm)')
    plt.title('TCP Error Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()


    # Fixed Point Error calculation
    error = np.array(P_fixed_world_array - P_fixed0)
    # Calculate maximum and minimum values of the error
    max_error = np.max(error, axis=0)
    min_error = np.min(error, axis=0)

    # Plot error over time with highlighted max and min values
    plt.figure(figsize=(8,5))
    plt.plot(time, error[:, 0],color='r', label='X-Error')
    # plt.plot(time, error[:, 1], color='g', label='Y-Error')
    plt.plot(time, error[:, 2], color='b', label='Z-Error')
    # Add horizontal dashed lines for max and min values
    plt.axhline(y=max_error[0], color='r', linestyle='--', label='Max X-Error')
    plt.axhline(y=min_error[0], color='r', linestyle='--', label='Min X-Error')
    # plt.axhline(y=max_error[1], color='g', linestyle='--', label='Max Y-Error')
    # plt.axhline(y=min_error[1], color='g', linestyle='--', label='Min Y-Error')
    plt.axhline(y=max_error[2], color='b', linestyle='--', label='Max Z-Error')
    plt.axhline(y=min_error[2], color='b', linestyle='--', label='Min Z-Error')
    # Annotate the max and min values
    plt.text(time[-1], max_error[0], f'{max_error[0]:.2f}', color='r', ha='right')
    plt.text(time[-1], min_error[0], f'{min_error[0]:.2f}', color='r', ha='right')
    # plt.text(time[-1], max_error[1], f'{max_error[1]:.2f}', color='g', ha='right')
    # plt.text(time[-1], min_error[1], f'{min_error[1]:.2f}', color='g', ha='right')
    plt.text(time[-1], max_error[2], f'{max_error[2]:.2f}', color='b', ha='left')
    plt.text(time[-1], min_error[2], f'{min_error[2]:.2f}', color='b', ha='left')
    plt.xlabel('Time (s)')
    plt.ylabel('Error (mm)')
    plt.title('CART_END Position Error Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # # Plot error over time with highlighted max and min values
    # plt.figure(figsize=(8,5))
    # plt.plot(time, error[:, 1], color='g', label='Y-Error')
    # # Add horizontal dashed lines for max and min values
    # plt.axhline(y=max_error[1], color='g', linestyle='--', label='Max Y-Error')
    # plt.axhline(y=min_error[1], color='g', linestyle='--', label='Min Y-Error')
    # # Annotate the max and min values
    # plt.text(time[-1], max_error[1], f'{max_error[1]:.2f}', color='g', ha='left')
    # plt.text(time[-1], min_error[1], f'{min_error[1]:.2f}', color='g', ha='right')
    # plt.xlabel('Time (s)')
    # plt.ylabel('Error (mm)')
    # plt.title('Fixed Point Error Over Time')
    # plt.legend()
    # plt.grid(True)
    # plt.show()

    # Plot wrist positions over time
    wrist_positions = extract_wrist_position(df)
    X_min = np.min(wrist_positions[:, 0])
    X_max = np.max(wrist_positions[:, 0])
    Y_min = np.min(wrist_positions[:, 1])
    Y_max = np.max(wrist_positions[:, 1])
    Z_min = np.min(wrist_positions[:, 2])
    Z_max = np.max(wrist_positions[:, 2])
    plt.figure(figsize=(8, 5))
    plt.plot(time, wrist_positions[:, 0], color='r', label='Wrist X Position')
    plt.axhline(y=X_max, color='r', linestyle='--', label='Max X-Value')
    plt.axhline(y=X_min, color='r', linestyle='--', label='Min X-Value')
    plt.text(time[-1], X_max, f'{X_max:.2f}', color='r', ha='right')
    plt.text(time[-1], X_min, f'{X_min:.2f}', color='r', ha='right')
    plt.plot(time, wrist_positions[:, 1], color='g', label='Wrist Y Position')
    plt.axhline(y=Y_max, color='g', linestyle='--', label='Max Y-Value')
    plt.axhline(y=Y_min, color='g', linestyle='--', label='Min Y-Value')
    plt.text(time[-1], Y_max, f'{Y_max:.2f}', color='g', ha='right')
    plt.text(time[-1], Y_min, f'{Y_min:.2f}', color='g', ha='right')
    plt.plot(time, wrist_positions[:, 2], color='b', label='Wrist Z Position')
    plt.axhline(y=Z_max, color='b', linestyle='--', label='Max Z-Value')
    plt.axhline(y=Z_min, color='b', linestyle='--', label='Min Z-Value')
    plt.text(time[-1], Z_max, f'{Z_max:.2f}', color='b', ha='right')
    plt.text(time[-1], Z_min, f'{Z_min:.2f}', color='b', ha='right')
    plt.xlabel('Time (s)')
    plt.ylabel('Position (mm)')
    plt.title('Wrist Position Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # Vibration Analysis using provided acceleration data
    print("Performing vibration analysis using provided acceleration data...")
    acc_magnitude, xf, yf_magnitude = perform_vibration_analysis_with_provided_acceleration(time, acceleration)

    # Plot acceleration over time
    plt.figure(figsize=(8,5))
    plt.plot(time, acc_magnitude, color='r', label='Linear Acceleration')
    # Add horizontal dashed lines for max and min values
    plt.axhline(y=np.max(acc_magnitude), color='r', linestyle='--', label='Max Acceleration')
    plt.axhline(y=np.min(acc_magnitude), color='r', linestyle='--', label='Min Acceleration')
    # Annotate the max and min values
    plt.text(time[-1], np.max(acc_magnitude), f'{np.max(acc_magnitude):.2f}', color='r', ha='right')
    plt.text(time[-1], np.min(acc_magnitude), f'{np.min(acc_magnitude):.2f}', color='r', ha='right')
    plt.xlabel('Time (s)')
    plt.ylabel('Linear Acceleration (mm/s²)')
    plt.title('Linear Acceleration Over Time')
    plt.legend()
    plt.grid(True)
    plt.show()

    # Plot frequency spectrum
    plt.figure(figsize=(8,5))
    plt.plot(xf, yf_magnitude)
    plt.xlabel('Frequency (Hz)')
    plt.ylabel('Amplitude')
    plt.title('Vibration Frequency Spectrum (Using Provided Acceleration)')
    plt.grid(True)
    plt.show()


# Main execution
if __name__ == "__main__":
    # Specify the filename
    filename = ('AlgorithmSimulation_HYD_MoveJAbs_ConfLOff.txt')  # Replace with your actual file name
    #SingAreaSimulation_HYD_LANCE
    #AlgorithmSimulation_HYD_LANCE

    # Read data
    df = read_data(filename)

    # Adjust time
    df = adjust_time(df, 0.06, 0.0)  # Adjust time by removing 3 ms and rescaling

    # df = adjust_time(df,0, 0.036)  # Adjust time by removing 3 ms and rescaling
    # Time array
    time = df['Time'].values
    print(time)
    pause = input("Press Enter to continue...")

    # Acceleration data
    acceleration = df['Linear_Acceleration'].values

    # Calculate positions and orientations
    P_fixed_world_array, euler_fixed_array, P_ee_world_array, euler_ee_array = calculate_fixed_point_positions(df)

    # Plot results
    plot_results(df, P_fixed_world_array, euler_fixed_array, P_ee_world_array, euler_ee_array, acceleration)