obsidian_backup/补课/多体动力学/05_vectors.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "sm.init_printing()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3, q4 = sm.symbols('q1:5')\n",
    "l1, l2, l3, l4 = sm.symbols('l1:5')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "N, A, B, C = sm.symbols(\"N, A, B, C\", cls=me.ReferenceFrame)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{1} \\right)} & \\sin{\\left(q_{1} \\right)} & 0\\\\- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} & \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} & \\sin{\\left(q_{2} \\right)}\\\\\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} & - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} & \\cos{\\left(q_{2} \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡    cos(q₁)           sin(q₁)          0   ⎤\n",
       "⎢                                           ⎥\n",
       "⎢-sin(q₁)⋅cos(q₂)  cos(q₁)⋅cos(q₂)   sin(q₂)⎥\n",
       "⎢                                           ⎥\n",
       "⎣sin(q₁)⋅sin(q₂)   -sin(q₂)⋅cos(q₁)  cos(q₂)⎦"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.orient_body_fixed(N, (q1, q2, 0),'ZXZ')\n",
    "A.dcm(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "B.orient_axis(A, q3, A.x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.orient_body_fixed(B, (q3, q4, 0), 'XZX')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "r_P1_P2 = l1 * A.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "r_P2_P3 = l2 * B.z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "r_P3_P4 = l3 *C.z - l4 *C.y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "r_P1_P4 = r_P1_P2 + r_P2_P3 + r_P3_P4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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
      "text/latex": [
       "$\\displaystyle l_{4} \\sin{\\left(q_{4} \\right)}\\mathbf{\\hat{b}_x} + (l_{1} \\sin{\\left(q_{3} \\right)} - l_{3} \\sin{\\left(q_{3} \\right)} - l_{4} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)})\\mathbf{\\hat{b}_y} + (l_{1} \\cos{\\left(q_{3} \\right)} + l_{2} + l_{3} \\cos{\\left(q_{3} \\right)} - l_{4} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)})\\mathbf{\\hat{b}_z}$"
      ],
      "text/plain": [
       "l₄⋅sin(q₄) b_x + (l₁⋅sin(q₃) - l₃⋅sin(q₃) - l₄⋅cos(q₃)⋅cos(q₄)) b_y + (l₁⋅cos( ↪\n",
       "\n",
       "↪ q₃) + l₂ + l₃⋅cos(q₃) - l₄⋅sin(q₃)⋅cos(q₄)) b_z"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "r_P1_P4.express(B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left\\{l_{1}, l_{2}, l_{3}, l_{4}, q_{3}, q_{4}\\right\\}$"
      ],
      "text/plain": [
       "{l₁, l₂, l₃, l₄, q₃, q₄}"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "r_P1_P4.free_symbols(B)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Turtle",
   "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.11.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}