obsidian_backup/多体+耦合求解器/WindTurbine_Kane推导_rigid.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 72,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "me.init_vprinting(use_latex='mathjax')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "metadata": {},
   "outputs": [],
   "source": [
    "E = me.ReferenceFrame('E')\n",
    "A = me.ReferenceFrame('A')\n",
    "# B = me.ReferenceFrame('B')\n",
    "D = me.ReferenceFrame('D')\n",
    "C = me.ReferenceFrame('C')\n",
    "G = me.ReferenceFrame('G')\n",
    "G_prime1 = me.ReferenceFrame('G_prime1')\n",
    "G_prime2 = me.ReferenceFrame('G_prime2')\n",
    "G_prime3 = me.ReferenceFrame('G_prime3')\n",
    "# N1 = me.ReferenceFrame('N1')\n",
    "# N2 = me.ReferenceFrame('N2')\n",
    "# N3 = me.ReferenceFrame('N3')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 74,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 广义坐标\n",
    "q_yaw, q_drtr, q_geaz= me.dynamicsymbols('q_yaw, q_drtr, q_geaz')\n",
    "u_yaw, u_drtr, u_geaz = me.dynamicsymbols('u_yaw, u_drtr, u_geaz')\n",
    "D.orient_axis(A, q_yaw, A.y)\n",
    "D.set_ang_vel(A, u_yaw * A.y)\n",
    "\n",
    "# shaft C \n",
    "theta_tilt = sm.symbols(\"theta_tilt\")\n",
    "C.orient_axis(D, theta_tilt, D.z)\n",
    "\n",
    "# Hub G = Azimuth E\n",
    "Azimuth = q_drtr + q_geaz\n",
    "G.orient_axis(C, Azimuth, C.x)\n",
    "G.set_ang_vel(C, (u_drtr + u_geaz) * C.x)\n",
    "\n",
    "# 到每只叶片\n",
    "angle = 2*sm.pi/3\n",
    "G_prime1.orient_axis(G, angle * 0, G.x)\n",
    "G_prime2.orient_axis(G, angle * 1, G.x)\n",
    "G_prime3.orient_axis(G, angle * 2, G.x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle 0$"
      ],
      "text/plain": [
       "0"
      ]
     },
     "execution_count": 75,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 设置点\n",
    "Z = me.Point('Z') # 塔基\n",
    "Z.set_vel(A, 0)\n",
    "\n",
    "O = me.Point('O') # 塔顶\n",
    "Tower_length = sm.symbols(\"L_T\")\n",
    "O.set_pos(Z, Tower_length * A.y)\n",
    "\n",
    "O.v2pt_theory(Z, A, D)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 76,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\cos{\\left(q_{yaw} \\right)}\\mathbf{\\hat{a}_x} -  \\sin{\\left(q_{yaw} \\right)}\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "cos(q_yaw) a_x + -sin(q_yaw) a_z"
      ]
     },
     "execution_count": 76,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 机舱\n",
    "# 机舱质心位置\n",
    "N_x = sm.symbols(\"Nx\")\n",
    "N_y = sm.symbols(\"Ny\")\n",
    "N_z = sm.symbols(\"Nz\")\n",
    "U = me.Point('U')\n",
    "U.set_pos(O, N_x * D.x + N_y * D.y + N_z * D.z)\n",
    "\n",
    "# 机舱IMU\n",
    "IMU_x = sm.symbols(\"I_x\")\n",
    "IMU_y = sm.symbols(\"I_y\")\n",
    "IMU_z = sm.symbols(\"I_z\")\n",
    "IMU = me.Point('I')\n",
    "IMU.set_pos(O, IMU_x * D.x + IMU_y * D.y + IMU_z * D.z)\n",
    "D.x.express(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{yaw} \\right)}\\mathbf{\\hat{a}_x} + (L_{overhang} \\sin{\\left(\\theta_{tilt} \\right)} + L_{t2s})\\mathbf{\\hat{a}_y} -  L_{overhang} \\sin{\\left(q_{yaw} \\right)} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{a}_z}$"
      ],
      "text/plain": [
       "L_overhang⋅cos(θₜᵢₗₜ)⋅cos(q_yaw) a_x + (L_overhang⋅sin(θₜᵢₗₜ) + Lₜ₂ₛ) a_y + -L ↪\n",
       "\n",
       "↪ _overhang⋅sin(q_yaw)⋅cos(θₜᵢₗₜ) a_z"
      ]
     },
     "execution_count": 77,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# point P\n",
    "P = me.Point('P')\n",
    "L_t2s, L_overhang = sm.symbols(\"L_t2s, L_overhang\")\n",
    "P.set_pos(O, L_t2s * D.y + L_overhang * C.x)\n",
    "r_o_p = P.pos_from(O)\n",
    "r_o_p.express(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 78,
   "metadata": {},
   "outputs": [],
   "source": [
    "# blade tip\n",
    "S_1 = me.Point('S_1')\n",
    "S_2 = me.Point('S_2')\n",
    "S_3 = me.Point('S_3')\n",
    "Blade_length = sm.symbols(\"L_B\")\n",
    "S_1.set_pos(P, Blade_length * G_prime1.y)\n",
    "S_2.set_pos(P, Blade_length * G_prime2.y)\n",
    "S_3.set_pos(P, Blade_length * G_prime3.y)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 获得点的速度、坐标系/刚体的角速度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( Nz u_{yaw}\\mathbf{\\hat{d}_x} -  Nx u_{yaw}\\mathbf{\\hat{d}_z}, \\  I_{z} u_{yaw}\\mathbf{\\hat{d}_x} -  I_{x} u_{yaw}\\mathbf{\\hat{d}_z}, \\  -  L_{overhang} u_{yaw} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_z}, \\  u_{yaw}\\mathbf{\\hat{a}_y}, \\  u_{yaw}\\mathbf{\\hat{a}_y}, \\  (u_{drtr} + u_{geaz})\\mathbf{\\hat{c}_x} + u_{yaw}\\mathbf{\\hat{a}_y}\\right)$"
      ],
      "text/plain": [
       "(Nz⋅u_yaw d_x + -Nx⋅u_yaw d_z, I_z⋅u_yaw d_x + -Iₓ⋅u_yaw d_z, -L_overhang⋅u_ya ↪\n",
       "\n",
       "↪ w⋅cos(θₜᵢₗₜ) c_z, u_yaw a_y, u_yaw a_y, (u_drtr + u_geaz) c_x + u_yaw a_y)"
      ]
     },
     "execution_count": 79,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 速度\n",
    "U.v2pt_theory(O, A, D)\n",
    "IMU.v2pt_theory(O, A, D)\n",
    "\n",
    "P.v2pt_theory(O, A, D)\n",
    "P.vel(A).express(D)\n",
    "# r_P_O.express(D)\n",
    "\n",
    "E_v_P = D.ang_vel_in(A).cross(r_o_p)\n",
    "\n",
    "U.vel(A), IMU.vel(A), P.vel(A), D.ang_vel_in(A), C.ang_vel_in(A), G.ang_vel_in(A)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 80,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( (- Nx u_{yaw}^{2} + Nz \\dot{u}_{yaw})\\mathbf{\\hat{d}_x} + (- Nx \\dot{u}_{yaw} - Nz u_{yaw}^{2})\\mathbf{\\hat{d}_z}, \\  (- I_{x} u_{yaw}^{2} + I_{z} \\dot{u}_{yaw})\\mathbf{\\hat{d}_x} + (- I_{x} \\dot{u}_{yaw} - I_{z} u_{yaw}^{2})\\mathbf{\\hat{d}_z}, \\  -  L_{overhang} u_{yaw}^{2} \\cos^{2}{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_x} + L_{overhang} u_{yaw}^{2} \\sin{\\left(\\theta_{tilt} \\right)} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_y} -  L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)} \\dot{u}_{yaw}\\mathbf{\\hat{c}_z}, \\  \\dot{u}_{yaw}\\mathbf{\\hat{a}_y}, \\  \\dot{u}_{yaw}\\mathbf{\\hat{a}_y}, \\  (\\dot{u}_{drtr} + \\dot{u}_{geaz})\\mathbf{\\hat{c}_x} -  \\left(u_{drtr} + u_{geaz}\\right) u_{yaw} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_z} + \\dot{u}_{yaw}\\mathbf{\\hat{a}_y}\\right)$"
      ],
      "text/plain": [
       "⎛⎛          2           ⎞       ⎛                    2⎞      ⎛          2      ↪\n",
       "⎝⎝- Nx⋅u_yaw  + Nz⋅u_ẏaw⎠ d_x + ⎝-Nx⋅u_ẏaw - Nz⋅u_yaw ⎠ d_z, ⎝- Iₓ⋅u_yaw  + I_ ↪\n",
       "\n",
       "↪        ⎞       ⎛                     2⎞                       2    2         ↪\n",
       "↪ z⋅u_ẏaw⎠ d_x + ⎝-Iₓ⋅u_ẏaw - I_z⋅u_yaw ⎠ d_z, -L_overhang⋅u_yaw ⋅cos (θₜᵢₗₜ)  ↪\n",
       "\n",
       "↪                       2                                                      ↪\n",
       "↪ c_x + L_overhang⋅u_yaw ⋅sin(θₜᵢₗₜ)⋅cos(θₜᵢₗₜ) c_y + -L_overhang⋅cos(θₜᵢₗₜ)⋅u ↪\n",
       "\n",
       "↪                                                                              ↪\n",
       "↪ _ẏaw c_z, u_ẏaw a_y, u_ẏaw a_y, (u_dṙtr + u_gėaz) c_x + -(u_drtr + u_geaz)⋅u ↪\n",
       "\n",
       "↪                                ⎞\n",
       "↪ _yaw⋅cos(θₜᵢₗₜ) c_z + u_ẏaw a_y⎠"
      ]
     },
     "execution_count": 80,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "U.acc(A), IMU.acc(A), P.acc(A), D.ang_acc_in(A), C.ang_acc_in(A), G.ang_acc_in(A)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 偏线速度,偏角速度"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 81,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( \\mathbf{\\hat{d}_y}, \\  0, \\  0\\right)$"
      ],
      "text/plain": [
       "(d_y, 0, 0)"
      ]
     },
     "execution_count": 81,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 机舱 D = E^w_r^N\n",
    "w_D_1 = D.ang_vel_in(A).diff(u_yaw, A).express(D)\n",
    "w_D_2 = D.ang_vel_in(A).diff(u_drtr, A).express(D)\n",
    "w_D_3 = D.ang_vel_in(A).diff(u_geaz, A).express(D)\n",
    "w_D_1, w_D_2, w_D_3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 82,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( (u_{drtr} + u_{geaz})\\mathbf{\\hat{c}_x} + u_{yaw}\\mathbf{\\hat{a}_y}, \\  \\sin{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_x} + \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_y}, \\  \\mathbf{\\hat{c}_x}, \\  \\mathbf{\\hat{c}_x}\\right)$"
      ],
      "text/plain": [
       "((u_drtr + u_geaz) c_x + u_yaw a_y, sin(θₜᵢₗₜ) c_x + cos(θₜᵢₗₜ) c_y, c_x, c_x)"
      ]
     },
     "execution_count": 82,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 风轮 G = E^w_r^L\n",
    "w_C_1 = G.ang_vel_in(A).diff(u_yaw, A).express(C)\n",
    "w_C_2 = G.ang_vel_in(A).diff(u_drtr, A).express(C)\n",
    "w_C_3 = G.ang_vel_in(A).diff(u_geaz, A).express(C)\n",
    "G.ang_vel_in(A), w_C_1, w_C_2, w_C_3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 83,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( Nx\\mathbf{\\hat{d}_x} + Ny\\mathbf{\\hat{d}_y} + Nz\\mathbf{\\hat{d}_z}, \\  Nz\\mathbf{\\hat{d}_x} -  Nx\\mathbf{\\hat{d}_z}, \\  Nz\\mathbf{\\hat{d}_x} -  Nx\\mathbf{\\hat{d}_z}, \\  0, \\  0\\right)$"
      ],
      "text/plain": [
       "(Nx d_x + Ny d_y + Nz d_z, Nz d_x + -Nx d_z, Nz d_x + -Nx d_z, 0, 0)"
      ]
     },
     "execution_count": 83,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# U点\n",
    "v_U_1 = U.vel(A).diff(u_yaw, A)\n",
    "v_U_2 = U.vel(A).diff(u_drtr, A)\n",
    "v_U_3 = U.vel(A).diff(u_geaz, A)\n",
    "U.pos_from(O).express(D), w_D_1.cross(U.pos_from(O).express(D)), v_U_1, v_U_2, v_U_3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 84,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( I_{x}\\mathbf{\\hat{d}_x} + I_{y}\\mathbf{\\hat{d}_y} + I_{z}\\mathbf{\\hat{d}_z}, \\  I_{z}\\mathbf{\\hat{d}_x} -  I_{x}\\mathbf{\\hat{d}_z}, \\  I_{z}\\mathbf{\\hat{d}_x} -  I_{x}\\mathbf{\\hat{d}_z}, \\  0, \\  0\\right)$"
      ],
      "text/plain": [
       "(Iₓ d_x + I_y d_y + I_z d_z, I_z d_x + -Iₓ d_z, I_z d_x + -Iₓ d_z, 0, 0)"
      ]
     },
     "execution_count": 84,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# IMU点\n",
    "v_I_1 = IMU.vel(A).diff(u_yaw, A).express(D)\n",
    "v_I_2 = IMU.vel(A).diff(u_drtr, A).express(D)\n",
    "v_I_3 = IMU.vel(A).diff(u_geaz, A).express(D)\n",
    "IMU.pos_from(O).express(D), w_D_1.cross(IMU.pos_from(O).express(D)), v_I_1, v_I_2, v_I_3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 85,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left( L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{d}_x} + (L_{overhang} \\sin{\\left(\\theta_{tilt} \\right)} + L_{t2s})\\mathbf{\\hat{d}_y}, \\  -  L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{d}_z}, \\  -  L_{overhang} \\cos{\\left(\\theta_{tilt} \\right)}\\mathbf{\\hat{c}_z}, \\  0, \\  0\\right)$"
      ],
      "text/plain": [
       "(L_overhang⋅cos(θₜᵢₗₜ) d_x + (L_overhang⋅sin(θₜᵢₗₜ) + Lₜ₂ₛ) d_y, -L_overhang⋅c ↪\n",
       "\n",
       "↪ os(θₜᵢₗₜ) d_z, -L_overhang⋅cos(θₜᵢₗₜ) c_z, 0, 0)"
      ]
     },
     "execution_count": 85,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# P点\n",
    "v_P_1 = P.vel(A).diff(u_yaw, A)\n",
    "v_P_2 = P.vel(A).diff(u_drtr, A)\n",
    "v_P_3 = P.vel(A).diff(u_geaz, A)\n",
    "P.pos_from(O).express(D), w_D_1.cross(P.pos_from(O).express(D)), v_P_1, v_P_2, v_P_3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}L_{overhang} g m_{hub} \\cos{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{drtr} + q_{geaz} \\right)} + Nx g m_{nac}\\\\0\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡L_overhang⋅g⋅m_hub⋅cos(θₜᵢₗₜ)⋅cos(q_drtr + q_geaz) + Nx⋅g⋅m_nac⎤\n",
       "⎢                                                               ⎥\n",
       "⎢                               0                               ⎥\n",
       "⎢                                                               ⎥\n",
       "⎣                               0                               ⎦"
      ]
     },
     "execution_count": 86,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 主动力\n",
    "# yaw\n",
    "F_yaw = sm.Function('q_yaw, u_yaw')\n",
    "# 考虑 机舱重力，轮毂重力\n",
    "m_nac, g, m_hub = sm.symbols(\"m_nac, g, m_hub\")\n",
    "R_nac = -m_nac * g * D.z\n",
    "R_hub = -m_hub * g * G.z\n",
    "\n",
    "F1 = v_U_1.dot(R_nac) + v_P_1.dot(R_hub)\n",
    "F2 = v_U_2.dot(R_nac) + v_P_2.dot(R_hub)\n",
    "F3 = v_U_3.dot(R_nac) + v_P_3.dot(R_hub)\n",
    "\n",
    "Fr = sm.Matrix([F1, F2, F3])\n",
    "Fr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 93,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}- L_{overhang}^{2} m_{hub} \\cos^{2}{\\left(\\theta_{tilt} \\right)} \\dot{u}_{yaw} + Nx m_{nac} \\left(- Nx \\dot{u}_{yaw} - Nz u_{yaw}^{2}\\right) - Nz m_{nac} \\left(- Nx u_{yaw}^{2} + Nz \\dot{u}_{yaw}\\right) - \\left(Nac_{YIner} - m_{nac} \\left(Nx^{2} + Ny^{2}\\right)\\right) \\dot{u}_{yaw} + \\left(- \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) \\left(- \\left(u_{drtr} + u_{geaz}\\right) u_{yaw} \\sin{\\left(q_{drtr} + q_{geaz} \\right)} \\cos{\\left(\\theta_{tilt} \\right)} + \\cos{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{drtr} + q_{geaz} \\right)} \\dot{u}_{yaw}\\right) + \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) \\left(u_{drtr} + u_{geaz} + u_{yaw} \\sin{\\left(\\theta_{tilt} \\right)}\\right) u_{yaw} \\sin{\\left(q_{drtr} + q_{geaz} \\right)} \\cos{\\left(\\theta_{tilt} \\right)}\\right) \\cos{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{drtr} + q_{geaz} \\right)} + \\left(- \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) \\left(\\sin{\\left(\\theta_{tilt} \\right)} \\dot{u}_{yaw} + \\dot{u}_{drtr} + \\dot{u}_{geaz}\\right) - \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) u_{yaw}^{2} \\sin{\\left(q_{drtr} + q_{geaz} \\right)} \\cos^{2}{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{drtr} + q_{geaz} \\right)}\\right) \\sin{\\left(\\theta_{tilt} \\right)}\\\\- \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) \\left(\\sin{\\left(\\theta_{tilt} \\right)} \\dot{u}_{yaw} + \\dot{u}_{drtr} + \\dot{u}_{geaz}\\right) - \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) u_{yaw}^{2} \\sin{\\left(q_{drtr} + q_{geaz} \\right)} \\cos^{2}{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{drtr} + q_{geaz} \\right)}\\\\- \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) \\left(\\sin{\\left(\\theta_{tilt} \\right)} \\dot{u}_{yaw} + \\dot{u}_{drtr} + \\dot{u}_{geaz}\\right) - \\left(Hub_{Iner} - hub_{cm}^{2} m_{hub}\\right) u_{yaw}^{2} \\sin{\\left(q_{drtr} + q_{geaz} \\right)} \\cos^{2}{\\left(\\theta_{tilt} \\right)} \\cos{\\left(q_{drtr} + q_{geaz} \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡            2          2                         ⎛                    2⎞      ↪\n",
       "⎢- L_overhang ⋅m_hub⋅cos (θₜᵢₗₜ)⋅u_ẏaw + Nx⋅m_nac⋅⎝-Nx⋅u_ẏaw - Nz⋅u_yaw ⎠ - Nz ↪\n",
       "⎢                                                                              ↪\n",
       "⎢                                                                              ↪\n",
       "⎢                                                                              ↪\n",
       "⎢                                                                              ↪\n",
       "⎢                                                                              ↪\n",
       "⎣                                                                              ↪\n",
       "\n",
       "↪        ⎛          2           ⎞   ⎛                  ⎛  2     2⎞⎞         ⎛  ↪\n",
       "↪ ⋅m_nac⋅⎝- Nx⋅u_yaw  + Nz⋅u_ẏaw⎠ - ⎝Nac_YIner - m_nac⋅⎝Nx  + Ny ⎠⎠⋅u_ẏaw + ⎝- ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "\n",
       "↪  ⎛                 2      ⎞                                                  ↪\n",
       "↪  ⎝Hub_Iner - hub_cm ⋅m_hub⎠⋅(-(u_drtr + u_geaz)⋅u_yaw⋅sin(q_drtr + q_geaz)⋅c ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                  ⎛           ↪\n",
       "↪                                                                - ⎝Hub_Iner - ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                  ⎛           ↪\n",
       "↪                                                                - ⎝Hub_Iner - ↪\n",
       "\n",
       "↪                                                      ⎛                 2     ↪\n",
       "↪ os(θₜᵢₗₜ) + cos(θₜᵢₗₜ)⋅cos(q_drtr + q_geaz)⋅u_ẏaw) + ⎝Hub_Iner - hub_cm ⋅m_h ↪\n",
       "↪                                                                              ↪\n",
       "↪        2      ⎞                                        ⎛                 2   ↪\n",
       "↪  hub_cm ⋅m_hub⎠⋅(sin(θₜᵢₗₜ)⋅u_ẏaw + u_dṙtr + u_gėaz) - ⎝Hub_Iner - hub_cm ⋅m ↪\n",
       "↪                                                                              ↪\n",
       "↪        2      ⎞                                        ⎛                 2   ↪\n",
       "↪  hub_cm ⋅m_hub⎠⋅(sin(θₜᵢₗₜ)⋅u_ẏaw + u_dṙtr + u_gėaz) - ⎝Hub_Iner - hub_cm ⋅m ↪\n",
       "\n",
       "↪   ⎞                                                                          ↪\n",
       "↪ ub⎠⋅(u_drtr + u_geaz + u_yaw⋅sin(θₜᵢₗₜ))⋅u_yaw⋅sin(q_drtr + q_geaz)⋅cos(θₜᵢₗ ↪\n",
       "↪                                                                              ↪\n",
       "↪     ⎞      2                         2                                       ↪\n",
       "↪ _hub⎠⋅u_yaw ⋅sin(q_drtr + q_geaz)⋅cos (θₜᵢₗₜ)⋅cos(q_drtr + q_geaz)           ↪\n",
       "↪                                                                              ↪\n",
       "↪     ⎞      2                         2                                       ↪\n",
       "↪ _hub⎠⋅u_yaw ⋅sin(q_drtr + q_geaz)⋅cos (θₜᵢₗₜ)⋅cos(q_drtr + q_geaz)           ↪\n",
       "\n",
       "↪   ⎞                                   ⎛  ⎛                 2      ⎞          ↪\n",
       "↪ ₜ)⎠⋅cos(θₜᵢₗₜ)⋅cos(q_drtr + q_geaz) + ⎝- ⎝Hub_Iner - hub_cm ⋅m_hub⎠⋅(sin(θₜᵢ ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "\n",
       "↪                                ⎛                 2      ⎞      2             ↪\n",
       "↪ ₗₜ)⋅u_ẏaw + u_dṙtr + u_gėaz) - ⎝Hub_Iner - hub_cm ⋅m_hub⎠⋅u_yaw ⋅sin(q_drtr  ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "↪                                                                              ↪\n",
       "\n",
       "↪              2                            ⎞           ⎤\n",
       "↪ + q_geaz)⋅cos (θₜᵢₗₜ)⋅cos(q_drtr + q_geaz)⎠⋅sin(θₜᵢₗₜ)⎥\n",
       "↪                                                       ⎥\n",
       "↪                                                       ⎥\n",
       "↪                                                       ⎥\n",
       "↪                                                       ⎥\n",
       "↪                                                       ⎥\n",
       "↪                                                       ⎦"
      ]
     },
     "execution_count": 93,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# 惯性力\n",
    "\n",
    "# 机舱惯性\n",
    "Nac_YIner = sm.symbols(\"Nac_YIner\")\n",
    "Inertia_nac = (Nac_YIner - m_nac * (N_x**2 + N_y**2)) * me.outer(D.y, D.y)\n",
    "Inertia_nac\n",
    "\n",
    "# hub惯性\n",
    "Hub_Iner, hub_cm = sm.symbols(\"Hub_Iner, hub_cm\")\n",
    "Inertia_hub = (Hub_Iner -  m_hub * hub_cm**2) * me.outer(G.x, G.x) + (Hub_Iner -  m_hub * hub_cm**2) * me.outer(G.y, G.y)\n",
    "Inertia_hub\n",
    "\n",
    "T_nac = -(D.ang_acc_in(A).dot(Inertia_nac) + me.cross(D.ang_vel_in(A), Inertia_nac).dot(D.ang_vel_in(A)))\n",
    "T_hub = -(G.ang_acc_in(A).dot(Inertia_hub) + me.cross(G.ang_vel_in(A), Inertia_hub).dot(G.ang_vel_in(A)))\n",
    "\n",
    "F1s = v_U_1.dot(-m_nac * U.acc(A)) + v_P_1.dot(-m_hub * P.acc(A)) + w_D_1.dot(T_nac) + w_C_1.dot(T_hub)\n",
    "F2s = v_U_2.dot(-m_nac * U.acc(A)) + v_P_2.dot(-m_hub * P.acc(A)) + w_D_2.dot(T_nac) + w_C_2.dot(T_hub)\n",
    "F3s = v_U_3.dot(-m_nac * U.acc(A)) + v_P_3.dot(-m_hub * P.acc(A)) + w_D_3.dot(T_nac) + w_C_3.dot(T_hub)\n",
    "\n",
    "Frs = sm.Matrix([F1s, F2s, F3s])\n",
    "Frs"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 94,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left\\{q_{drtr}, q_{geaz}\\right\\}$"
      ],
      "text/plain": [
       "{q_drtr, q_geaz}"
      ]
     },
     "execution_count": 94,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "me.find_dynamicsymbols(Fr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 95,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left\\{q_{drtr}, q_{geaz}, u_{drtr}, u_{geaz}, u_{yaw}, \\dot{u}_{drtr}, \\dot{u}_{geaz}, \\dot{u}_{yaw}\\right\\}$"
      ],
      "text/plain": [
       "{q_drtr, q_geaz, u_drtr, u_geaz, u_yaw, u_dṙtr, u_gėaz, u_ẏaw}"
      ]
     },
     "execution_count": 95,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "me.find_dynamicsymbols(Frs)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "MinerU",
   "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.10.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}