The physical simulation

import numpy as np
from scipy.spatial.transform import Rotation as R, Slerp
import matplotlib.pyplot as plt

#from scipy.linalg import block_diag
#from dataclasses import dataclass
#import copy

from ipynb.fs.defs.plot_data import plot_dashboard, plot_3d_interactive, plot_final_3d_path, plot_imu_vs_truth

Parametric quintic polynomial

$$p(t) = p_0 + (p_f - p_0) \left( 10\tau^3 - 15\tau^4 + 6\tau^5 \right)$$

Where $\tau = \frac{t}{T}$ is the normalized time, i.e. $\tau \in [0, 1[$

T=2.0
fs=5000

def stationary_translation(T_still, fs, start_pos=np.array([0.0, 0.0, 0.0])):
    n_samples = int(T_still * fs)
    t = np.linspace(0, T_still, n_samples, endpoint=False)
    
    # Generate smooth noise by integrating white noise twice (Random Walk)
    # This creates a "slow drift/tremble" rather than "jitter"
    acc_noise_std = 0.05  # Small vibration in m/s^2
    acc = np.random.normal(0, acc_noise_std, (n_samples, 3))
    
    # Integrate acceleration to get velocity and position (with zero initial conditions)
    dt = 1.0 / fs
    vel = np.cumsum(acc, axis=0) * dt
    pos_offset = np.cumsum(vel, axis=0) * dt
    
    pos = start_pos + pos_offset
    
    return t, pos, vel, acc

def stationary_orientation(T_still, fs, start_euler=np.array([0, 0, 0])):
    n_samples = int(T_still * fs)
    dt = 1.0 / fs
    
    # Generate smooth angular velocity (Rad/s)
    # A small standard deviation like 0.01 rad/s creates a realistic wobble
    omega_std = 0.01 
    omega = np.zeros((n_samples, 3))
    for i in range(1, n_samples):
        # Random walk for omega ensures it doesn't jump instantly
        omega[i] = omega[i-1] + np.random.normal(0, omega_std * dt, 3)

    # Integrate omega to get quaternions
    quat = np.zeros((n_samples, 4))
    current_rot = R.from_euler('xyz', start_euler, degrees=True)
    
    for i in range(n_samples):
        # Update rotation: q_new = q_old * exp(omega * dt)
        step_rot = R.from_rotvec(omega[i] * dt)
        current_rot = current_rot * step_rot
        quat[i] = current_rot.as_quat()
        
    alpha = np.gradient(omega, dt, axis=0)
    
    return quat, omega, alpha

def straight_xyz_kinematics(T, fs, x_dist=1.2, y_peak=0.8, z_dist=0.5, k=8.0):
    t = np.linspace(0, T, int(T*fs))
    tau = t / T
    
    # 1. Master Minimum Jerk Polynomial and its derivatives
    s = 10*tau**3 - 15*tau**4 + 6*tau**5
    sd = (30*tau**2 - 60*tau**3 + 30*tau**4) / T
    sdd = (60*tau - 180*tau**2 + 120*tau**3) / (T**2)

    # 2. The Y-Arc: Parameterized "There and Back" logic
    # Base term: b = s * (1 - s)
    # y_shape = (4^k) * b^k
    b = s * (1 - s)
    bd = sd * (1 - 2 * s)
    bdd = sdd * (1 - 2 * s) - 2 * sd**2
    
    # Normalization constant to keep peak at 1.0
    norm = 4**k
    
    # Calculus chain rule for y = norm * b^k:
    # y' = norm * k * b^(k-1) * b'
    # y'' = norm * k * [(k-1) * b^(k-2) * (b')^2 + b^(k-1) * b'']
    
    y_shape = norm * (b**k)
    
    # Handling k=1 to avoid division by zero or numerical instability
    if k == 1.0:
        y_shape_d = norm * bd
        y_shape_dd = norm * bdd
    else:
        y_shape_d = norm * k * (b**(k-1)) * bd
        y_shape_dd = norm * k * ((k-1) * (b**(k-2)) * (bd**2) + (b**(k-1)) * bdd)

    # 3. Combine into Global Arrays
    pos = np.stack([x_dist * s, y_peak * y_shape, z_dist * s], axis=1)
    vel = np.stack([x_dist * sd, y_peak * y_shape_d, z_dist * sd], axis=1)
    acc = np.stack([x_dist * sdd, y_peak * y_shape_dd, z_dist * sdd], axis=1)

    return t, pos, vel, acc

t, pos, vel, acc_global = straight_xyz_kinematics(T, fs)

Rotation

• active transformations of vectors
  • rot.apply(vec_body), it takes a vector defined in the racket's coordinate system and rotates it into the global room coordinates.
  • rot.inv().apply(...), from global to local

total_rot = R.from_euler('xyz', [0, 180, 360], degrees=True)
total_rot_vec = total_rot.as_rotvec()
total_rot_vec

array([ 3.84734139e-16,  3.14159265e+00, -2.35581716e-32])

def swing_rotation_kinematics(t, T):
    tau = t / T
    # Smooth step function (S-curve)
    s = 10*tau**3 - 15*tau**4 + 6*tau**5
    
    # Define start and end Euler angles (degrees)
    start_euler = np.array([0, 0, 0])
    end_euler = np.array([45, 120, 80]) 
    
    quat = np.zeros((len(t), 4))
    omega_body = np.zeros((len(t), 3))
    
    # --- Step 1: Generate Orientations ---
    for i in range(len(t)):
        current_euler = start_euler + (end_euler - start_euler) * s[i]
        rot = R.from_euler('xyz', current_euler, degrees=True)
        quat[i] = rot.as_quat() # Result is [x, y, z, w]
        
    # --- Step 2: Generate "Perfect" Omega from relative rotations ---
    for i in range(len(t) - 1):
        dt = t[i+1] - t[i]
        q_now = R.from_quat(quat[i])
        q_next = R.from_quat(quat[i+1])
        
        # Calculate the delta rotation that happened in the Body Frame
        # dq_body = q_now_inv * q_next
        res_rot = q_now.inv() * q_next
        
        # as_rotvec returns the vector (angle * axis), exactly what we need
        omega_body[i] = res_rot.as_rotvec() / dt

    # Fill the last value to keep array size consistent
    omega_body[-1] = omega_body[-2]

    # --- Step 3: Angular Acceleration ---
    # Gradient uses actual time spacing for better accuracy
    alpha_body = np.gradient(omega_body, t, axis=0)
        
    return quat, omega_body, alpha_body

def generate_swing_path(T_swing, fs, T_still=None):
    if T_still is None:
        # Random duration between 0.4s and 1.2s
        T_still = np.random.uniform(0.4, 1.2)
    
    # 1. Generate Noisy Stationary Phase
    t_s, p_s, v_s, a_s = stationary_translation(T_still, fs)
    q_s, o_s, al_s = stationary_orientation(T_still, fs)
    
    # 2. Generate Swing Phase
    t_w, p_w, v_w, a_w = straight_xyz_kinematics(T_swing, fs)
    q_w, o_w, al_w = swing_rotation_kinematics(t_w, T_swing)
    
    # 3. Seamless Concatenation
    # We ensure the swing starts exactly where the noisy phase ended 
    # to avoid a "teleportation" jump
    last_pos_s = p_s[-1]
    last_q_s = R.from_quat(q_s[-1])
    
    # Adjust swing to start at the end of stillness
    p_w_adjusted = p_w + last_pos_s
    
    # Re-calculate swing quaternions relative to the last still orientation
    q_w_adjusted = np.zeros_like(q_w)
    for i in range(len(q_w)):
        q_w_adjusted[i] = (last_q_s * R.from_quat(q_w[i])).as_quat()

    t_combined = np.concatenate([t_s, t_w + T_still])
    pos_combined = np.concatenate([p_s, p_w_adjusted])
    vel_combined = np.concatenate([v_s, v_w])
    acc_combined = np.concatenate([a_s, a_w])
    
    quat_combined = np.concatenate([q_s, q_w_adjusted])
    omega_combined = np.concatenate([o_s, o_w])
    alpha_combined = np.concatenate([al_s, al_w])
    
    return {
        't': t_combined,
        'pos': pos_combined,
        'vel': vel_combined,
        'acc_global': acc_combined,
        'quat': quat_combined,
        'omega': omega_combined,
        'alpha': alpha_combined,
        'T_still': T_still
    }

3D Visualization

# Run it:
swing_data = generate_swing_path(T, fs)
plot_dashboard(swing_data)

swing_data['pos'][4999:5005], print(swing_data['vel'][4999:5005])

[[0.63222175 0.01027659 0.26342573]
 [0.63255934 0.01034445 0.26356639]
 [0.63289688 0.01041272 0.26370703]
 [0.63323438 0.01048139 0.26384766]
 [0.63357184 0.01055046 0.26398827]
 [0.63390925 0.01061994 0.26412885]]

(array([[0.12418597, 0.00036893, 0.05160857],
        [0.12431246, 0.000371  , 0.05166127],
        [0.12443902, 0.00037307, 0.051714  ],
        [0.12456565, 0.00037516, 0.05176676],
        [0.12469234, 0.00037727, 0.05181955],
        [0.1248191 , 0.00037938, 0.05187237]]),
 None)

import plotly.io as pio
#pio.renderers.default = "notebook_connected" # or "iframe" if "notebook_connected" fails
pio.renderers.default = "iframe"
plot_3d_interactive(swing_data)

plot_final_3d_path(swing_data)

Generate IMU Data

G_GLOBAL = np.array([0, 0, 9.81])

def generate_imu_measurements(data, target_fs=100, 
                             acc_noise=0.01, gyro_noise=0.001, 
                             acc_bias_std=0.0001, gyro_bias_std=0.00001,
                             g_global_pseudo_acc = - G_GLOBAL): # Gravity Vector (Global Z is up)
    """
    Transforms ground truth into noisy 200Hz IMU data.
    """
    # 1. Downsampling indices (from 5000Hz to 200Hz)
    original_fs = 1 / (data['t'][1] - data['t'][0])
    step = int(original_fs / target_fs)
    idx = np.arange(0, len(data['t']), step)
    
    t_imu = data['t'][idx]
    N = len(t_imu)
    dt = 1.0 / target_fs
    
    imu_acc = np.zeros((N, 3))
    imu_gyro = np.zeros((N, 3))
    
    # Initial Biases
    b_acc = np.zeros(3)
    b_gyro = np.zeros(3)
    
    for i, g_idx in enumerate(idx):
        # Current Orientation (Matrix form for rotation)
        
        rot = R.from_quat(data['quat'][g_idx])
        
        # Accelerometer = Rot_matrix^T * (Acc_global + Gravity_global) + Bias + Noise
        # Specific Force: the sensor "pushes back" against gravity.
        acc_body = rot.inv().apply(data['acc_global'][g_idx] + g_global_pseudo_acc)
        
        # Apply random walk to bias
        b_acc += np.random.normal(0, acc_bias_std, 3)
        b_gyro += np.random.normal(0, gyro_bias_std, 3)
        
        # Combine everything
        imu_acc[i] = acc_body + b_acc + np.random.normal(0, acc_noise, 3)
        imu_gyro[i] = data['omega'][g_idx] + b_gyro + np.random.normal(0, gyro_noise, 3)
        
    return {
        't': t_imu,
        'acc': imu_acc,   # Noisy Accelerometer [m/s^2]
        'gyro': imu_gyro, # Noisy Gyroscope [rad/s]
        'dt': dt
    }

# 1. Generate the perfect ground truth (5000 Hz)
#truth_data = generate_swing_path(T, fs)

target_fs=100
# 2. Generate the noisy IMU data (200 Hz)
# Adjust noise levels here to see how they affect the "fuzziness" of the plot
imu_data = generate_imu_measurements(
    swing_data, 
    target_fs, 
    acc_noise=0.1,    # m/s^2
    gyro_noise=0.01   # rad/s
)

# 3. Call the plotting function
plot_imu_vs_truth(imu_data, swing_data, g_global = -G_GLOBAL)