{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "511e474d-9226-4261-ab68-442400e424f8",
   "metadata": {},
   "source": [
    "# 6 DOF Vibrational Analysis Tool\n",
    "#### This tool calculates the modal frequencies for a 6-DOF system. Based on the modal frequencies you can tune mount locations and mount directional stiffness values"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "72544b62-1598-437b-9200-9b5f213fd5ce",
   "metadata": {},
   "source": [
    "## IMPORTS & INPUT PARAMETERS"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "587d2dae-6fcb-4bb9-949e-02b14f0f8958",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.linalg import eigh\n",
    "\n",
    "# Total Mass of the system (kg)\n",
    "m = 250 \n",
    "\n",
    "# Inertia Tensor of the system at CG (kg-m^2) [Ixx, Ixy, Ixz; Iyx, Iyy, Iyz; Izx, Izy, Izz]\n",
    "I = np.array([\n",
    "    [ 22.95,   2.223, -0.1393], \n",
    "    [  2.223, 14.95,  -2.286 ], \n",
    "    [-0.1393, -2.286,  24.47 ]\n",
    "])\n",
    "\n",
    "# Mount Positions relative to CG of the system (meters) [x, y, z]\n",
    "r1 = np.array([ 0.15, -0.39, -0.26891]) \n",
    "r2 = np.array([ 0.15,  0.56, -0.26891]) \n",
    "r3 = np.array([-0.29,  0.0,  -0.26891])\n",
    "\n",
    "# Mount directional Stiffnesses in Global X, Y, Z (N/m)\n",
    "k1 = np.diag([2.0e5, 1.5e5, 1.6e5])\n",
    "k2 = np.diag([2.0e5, 1.5e5, 1.6e5])\n",
    "k3 = np.diag([2.5e5, 2.0e5, 1.8e5])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "2775a1cd-5cdd-4b3a-9dbc-fca9bc17cdae",
   "metadata": {},
   "source": [
    "### ASSEMBLE MASS MATRIX"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "5f92621b-8d21-47f1-a6c9-e1cb5d81ac13",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Mass Matrix (6x6) assembled successfully.\n"
     ]
    }
   ],
   "source": [
    "M = np.zeros((6, 6))\n",
    "M[0:3, 0:3] = m * np.eye(3)\n",
    "M[3:6, 3:6] = I\n",
    "\n",
    "print(\"Mass Matrix (6x6) assembled successfully.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1a68c475-1852-4eeb-a7df-1a3c163f5dfa",
   "metadata": {},
   "source": [
    "### ASSEMBLE STIFFNESS MATRIX"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "f337699f-09fe-485f-9b8f-796791d18972",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Stiffness Matrix (6x6) assembled successfully.\n"
     ]
    }
   ],
   "source": [
    "# Helper function for skew-symmetric matrix (cross product operator)\n",
    "def skew(r):\n",
    "    return np.array([\n",
    "        [    0, -r[2],  r[1]],\n",
    "        [ r[2],     0, -r[0]],\n",
    "        [-r[1],  r[0],     0]\n",
    "    ])\n",
    "\n",
    "# Calculate sub-matrices for each mount using matrix multiplication (@)\n",
    "K11 = k1 + k2 + k3\n",
    "K12 = -(k1 @ skew(r1) + k2 @ skew(r2) + k3 @ skew(r3))\n",
    "K21 = (skew(r1) @ k1 + skew(r2) @ k2 + skew(r3) @ k3)\n",
    "K22 = -(skew(r1) @ k1 @ skew(r1) + skew(r2) @ k2 @ skew(r2) + skew(r3) @ k3 @ skew(r3))\n",
    "\n",
    "# Global Stiffness Matrix (6x6)\n",
    "K = np.block([\n",
    "    [K11, K12],\n",
    "    [K21, K22]\n",
    "])\n",
    "\n",
    "print(\"Stiffness Matrix (6x6) assembled successfully.\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f351287a-4246-4c08-9a7f-28062b2380f4",
   "metadata": {},
   "source": [
    "## SOLVE EIGENVALUE PROBLEM"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "614dc304-9144-4cd9-8356-3d2defeb48a1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# eigh is highly recommended here over eig because K and M are symmetric real matrices\n",
    "eigenvalues, eigenvectors = eigh(K, M)\n",
    "\n",
    "# Extract natural frequencies in Hz\n",
    "# Note: eigenvalues = omega_n^2\n",
    "omega_n = np.sqrt(np.maximum(eigenvalues, 0)) # maximum(x,0) guards against tiny negative floats\n",
    "freq_Hz = omega_n / (2 * np.pi)\n",
    "\n",
    "# Sort frequencies ascending (eigh usually sorts them, but this guarantees it)\n",
    "sort_idx = np.argsort(freq_Hz)\n",
    "freq_Hz_sorted = freq_Hz[sort_idx]\n",
    "Modes_sorted = eigenvectors[:, sort_idx]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8c86eb96-22cf-4892-b95f-618a6d63d91b",
   "metadata": {},
   "source": [
    "## DISPLAY NATURAL FREQUENCIES"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "59d194df-8f18-4fac-b6bf-ebcbd51fa299",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "--- Powertrain Natural Frequencies (Hz) ---\n",
      "Mode 1: 3.79 Hz\n",
      "Mode 2: 5.34 Hz\n",
      "Mode 3: 7.12 Hz\n",
      "Mode 4: 10.45 Hz\n",
      "Mode 5: 11.80 Hz\n",
      "Mode 6: 14.03 Hz\n"
     ]
    }
   ],
   "source": [
    "print('\\n--- Powertrain Natural Frequencies (Hz) ---')\n",
    "for i in range(6):\n",
    "    print(f'Mode {i+1}: {freq_Hz_sorted[i]:.2f} Hz')"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.13.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}