{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import sympy.physics.mechanics as me\n",
    "sm.init_printing()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "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": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "N, A, B, C = sm.symbols(\"N, A, B, C\", cls=me.ReferenceFrame)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "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": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "A.orient_body_fixed(N, (q1, q2, 0),'ZXZ')\n",
    "A.dcm(N)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "B.orient_axis(A, q3, A.x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "C.orient_body_fixed(B, (q3, q4, 0), 'XZX')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}