UkladyNieholonomiczne.ipynb

{
 "metadata": {
  "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.8.3-final"
  },
  "orig_nbformat": 2,
  "kernelspec": {
   "name": "python3",
   "display_name": "Python 3",
   "language": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2,
 "cells": [
  {
   "source": [
    "# Układy nieholonomiczne\n",
    "- Zerowe kolumny z macierzy D można usunąć"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "from IPython.display import display\n",
    "x, y, z, q = symbols(\"x y z q\")\n",
    "th, th1, th2 = symbols(\"\\\\theta \\\\theta_1 \\\\theta_2\")\n",
    "ph, ph1, ph2 = symbols(\"\\\\phi \\\\phi_1 \\\\phi_2\")\n",
    "x1, x2, x3, x4 = symbols(\"x_1 x_2 x_3 x_4\")\n",
    "y1, y2, y3, y4 = symbols(\"y_1 y_2 y_3 y_4\")\n",
    "q1, q2, q3, q4 = symbols(\"q_1 q_2 q_3 q_4\")\n",
    "\n",
    "# Stale\n",
    "L, l = symbols(\"L l\")\n",
    "r, R = symbols(\"r R\")\n",
    "a, b, c = symbols(\"a b c\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def SklNawiasuLiego(g: list, g2: list, name=\"\") -> Matrix:\n",
    "    global q\n",
    "    \n",
    "    dg = []\n",
    "    for q_i in q:\n",
    "        col = []\n",
    "        [col.append( Derivative(g_i, q_i).doit() ) for g_i in g]\n",
    "        dg.append(col)\n",
    "    out = Matrix(dg).transpose()*Matrix(g2)\n",
    "    print(f\"(składowe) SKładnik nawiasu Liego {name}:\")\n",
    "    display(out)\n",
    "    return out\n",
    "\n",
    "def NawiasLiego(ga:list, gb:list, name=\"\") -> list:\n",
    "    out = list(SklNawiasuLiego(gb, ga, name) - SklNawiasuLiego(ga, gb, name))\n",
    "    print(f\"Nawias Liego {name}:\")\n",
    "    display(Matrix(out))\n",
    "    return out"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "def KolejneD(D_skl:list) -> Matrix:\n",
    "    global stopienNieholonomicznosci\n",
    "    global DIndeks\n",
    "    global wektorWzrostu\n",
    "\n",
    "    D = Matrix(D_skl).transpose()\n",
    "    newR = D.rank()\n",
    "\n",
    "    print(f\"Dystrybucja D_{DIndeks}\")\n",
    "    display(D)\n",
    "    print(f\"rank{{D}} = {newR} \", end=\"\")\n",
    "\n",
    "    if newR == wymiarPrzestrzeniKonfig:\n",
    "        print(\"= r_i = n\")\n",
    "        if stopienNieholonomicznosci == None:\n",
    "            stopienNieholonomicznosci = DIndeks\n",
    "    else:\n",
    "        print(\"< n, kontynuuj obliczenia...\")\n",
    "\n",
    "    try:\n",
    "        print(f\"det{{D}} = \")\n",
    "        display(det(D).simplify())\n",
    "    except NonSquareMatrixError:\n",
    "        pass\n",
    "\n",
    "    DIndeks += 1\n",
    "    wektorWzrostu.append(newR)\n",
    "    return D"
   ]
  },
  {
   "source": [
    "____________________\n",
    "## Wprowadź dane\n",
    "### Wektor $q$"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# q\n",
    "# q = (x1, x2, x3, x4) #(wyklad)\n",
    "# q = (x, y, th, ph1, ph2) #a\n",
    "q = (x, y, th, ph) #(c)\n",
    "# q = (q1, q2, q3) #(d, e, f)"
   ]
  },
  {
   "source": [
    "### Wektory $g_1$, $g_2$"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# g1\n",
    "# g1 = (1, 0, x2, x3) #(wyklad)\n",
    "# g1 = (cos(th), sin(th), 1/L, 2/r, 0) #a\n",
    "g1 = (L*cos(th)*cos(ph), L*sin(th)*cos(ph), sin(ph), 0) #(c)\n",
    "# g1 = (1, 0, q2) #(d)\n",
    "# # g1 = (1, 0, -1) #(e)\n",
    "# g1 = (1, 0, -q2) #(f)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "# g2\n",
    "# g2 = (0, 1, 0, 0) #(wyklad)\n",
    "# g2 = (cos(th), sin(th), -1/L, 0, 2/r) # a\n",
    "g2 = (0, 0, 0, 1) #(c)\n",
    "# g2 = (0, 1, 0) #(d)\n",
    "# g2 = (0, 1, 0) #(e)\n",
    "# g2 = (0, 1, q1) #(f)"
   ]
  },
  {
   "source": [
    "__________________\n",
    "## Wprowadzone dane:"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[     x],\n",
       "[     y],\n",
       "[\\theta],\n",
       "[  \\phi]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\\\\\theta\\\\\\phi\\end{matrix}\\right]$"
     },
     "metadata": {},
     "execution_count": 7
    }
   ],
   "source": [
    "Matrix(q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[L*cos(\\phi)*cos(\\theta)],\n",
       "[L*sin(\\theta)*cos(\\phi)],\n",
       "[              sin(\\phi)],\n",
       "[                      0]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)}\\\\\\sin{\\left(\\phi \\right)}\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {},
     "execution_count": 8
    }
   ],
   "source": [
    "Matrix(g1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "Matrix([\n",
       "[0],\n",
       "[0],\n",
       "[0],\n",
       "[1]])"
      ],
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}0\\\\0\\\\0\\\\1\\end{matrix}\\right]$"
     },
     "metadata": {},
     "execution_count": 9
    }
   ],
   "source": [
    "Matrix(g2)"
   ]
  },
  {
   "source": [
    "____________________\n",
    "## Obliczenia"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "wymiarPrzestrzeniKonfig = len(q)\n",
    "stopienNieholonomicznosci = None\n",
    "wektorWzrostu = [] \n",
    "DIndeks = 0"
   ]
  },
  {
   "source": [
    "### $D_0$"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Dystrybucja D_0\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*cos(\\phi)*cos(\\theta), 0],\n[L*sin(\\theta)*cos(\\phi), 0],\n[              sin(\\phi), 0],\n[                      0, 1]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & 0\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)} & 0\\\\\\sin{\\left(\\phi \\right)} & 0\\\\0 & 1\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "rank{D} = 2 < n, kontynuuj obliczenia...\ndet{D} = \n"
     ]
    }
   ],
   "source": [
    "D0 = KolejneD([g1, g2])"
   ]
  },
  {
   "source": [
    "### $D_1$"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g12:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[0],\n[0],\n[0],\n[0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}0\\\\0\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g12:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[-L*sin(\\phi)*cos(\\theta)],\n[-L*sin(\\phi)*sin(\\theta)],\n[               cos(\\phi)],\n[                       0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}- L \\sin{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\- L \\sin{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)}\\\\\\cos{\\left(\\phi \\right)}\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Nawias Liego g12:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*sin(\\phi)*cos(\\theta)],\n[L*sin(\\phi)*sin(\\theta)],\n[             -cos(\\phi)],\n[                      0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\sin{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\L \\sin{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)}\\\\- \\cos{\\left(\\phi \\right)}\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Dystrybucja D_1\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*cos(\\phi)*cos(\\theta), 0, L*sin(\\phi)*cos(\\theta)],\n[L*sin(\\theta)*cos(\\phi), 0, L*sin(\\phi)*sin(\\theta)],\n[              sin(\\phi), 0,              -cos(\\phi)],\n[                      0, 1,                       0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & 0 & L \\sin{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)} & 0 & L \\sin{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)}\\\\\\sin{\\left(\\phi \\right)} & 0 & - \\cos{\\left(\\phi \\right)}\\\\0 & 1 & 0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "rank{D} = 3 < n, kontynuuj obliczenia...\ndet{D} = \n"
     ]
    }
   ],
   "source": [
    "g12 = NawiasLiego(g1, g2, \"g12\")\n",
    "D1 = KolejneD([g1, g2, g12])\n"
   ]
  },
  {
   "source": [
    "### $D_2$"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g112:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[-L*sin(\\phi)**2*sin(\\theta)],\n[ L*sin(\\phi)**2*cos(\\theta)],\n[                          0],\n[                          0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}- L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)}\\\\L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g112:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[ L*sin(\\theta)*cos(\\phi)**2],\n[-L*cos(\\phi)**2*cos(\\theta)],\n[                          0],\n[                          0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)}\\\\- L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Nawias Liego g112:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[-L*sin(\\phi)**2*sin(\\theta) - L*sin(\\theta)*cos(\\phi)**2],\n[ L*sin(\\phi)**2*cos(\\theta) + L*cos(\\phi)**2*cos(\\theta)],\n[                                                       0],\n[                                                       0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}- L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} - L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)}\\\\L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} + L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g212:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*cos(\\phi)*cos(\\theta)],\n[L*sin(\\theta)*cos(\\phi)],\n[              sin(\\phi)],\n[                      0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)}\\\\\\sin{\\left(\\phi \\right)}\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g212:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[0],\n[0],\n[0],\n[0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}0\\\\0\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Nawias Liego g212:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*cos(\\phi)*cos(\\theta)],\n[L*sin(\\theta)*cos(\\phi)],\n[              sin(\\phi)],\n[                      0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)}\\\\\\sin{\\left(\\phi \\right)}\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Dystrybucja D_2\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*cos(\\phi)*cos(\\theta), 0, L*sin(\\phi)*cos(\\theta), -L*sin(\\phi)**2*sin(\\theta) - L*sin(\\theta)*cos(\\phi)**2, L*cos(\\phi)*cos(\\theta)],\n[L*sin(\\theta)*cos(\\phi), 0, L*sin(\\phi)*sin(\\theta),  L*sin(\\phi)**2*cos(\\theta) + L*cos(\\phi)**2*cos(\\theta), L*sin(\\theta)*cos(\\phi)],\n[              sin(\\phi), 0,              -cos(\\phi),                                                        0,               sin(\\phi)],\n[                      0, 1,                       0,                                                        0,                       0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & 0 & L \\sin{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & - L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} - L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)} & L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)} & 0 & L \\sin{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} & L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} + L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)}\\\\\\sin{\\left(\\phi \\right)} & 0 & - \\cos{\\left(\\phi \\right)} & 0 & \\sin{\\left(\\phi \\right)}\\\\0 & 1 & 0 & 0 & 0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "rank{D} = 4 = r_i = n\ndet{D} = \n"
     ]
    }
   ],
   "source": [
    "g112 = NawiasLiego(g1, g12, \"g112\")\n",
    "g212 = NawiasLiego(g2, g12, \"g212\")\n",
    "D2 = KolejneD([g1, g2, g12, g112, g212])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g1212:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[(-L*sin(\\phi)**2*cos(\\theta) - L*cos(\\phi)**2*cos(\\theta))*sin(\\phi)],\n[(-L*sin(\\phi)**2*sin(\\theta) - L*sin(\\theta)*cos(\\phi)**2)*sin(\\phi)],\n[                                                                   0],\n[                                                                   0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\left(- L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} - L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\\\left(- L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} - L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "(składowe) SKładnik nawiasu Liego g1212:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[0],\n[0],\n[0],\n[0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}0\\\\0\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Nawias Liego g1212:\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[(-L*sin(\\phi)**2*cos(\\theta) - L*cos(\\phi)**2*cos(\\theta))*sin(\\phi)],\n[(-L*sin(\\phi)**2*sin(\\theta) - L*sin(\\theta)*cos(\\phi)**2)*sin(\\phi)],\n[                                                                   0],\n[                                                                   0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}\\left(- L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} - L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\\\left(- L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} - L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\0\\\\0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "Dystrybucja D_3\n"
     ]
    },
    {
     "output_type": "display_data",
     "data": {
      "text/plain": "Matrix([\n[L*cos(\\phi)*cos(\\theta), 0, L*sin(\\phi)*cos(\\theta), -L*sin(\\phi)**2*sin(\\theta) - L*sin(\\theta)*cos(\\phi)**2, L*cos(\\phi)*cos(\\theta), (-L*sin(\\phi)**2*cos(\\theta) - L*cos(\\phi)**2*cos(\\theta))*sin(\\phi)],\n[L*sin(\\theta)*cos(\\phi), 0, L*sin(\\phi)*sin(\\theta),  L*sin(\\phi)**2*cos(\\theta) + L*cos(\\phi)**2*cos(\\theta), L*sin(\\theta)*cos(\\phi), (-L*sin(\\phi)**2*sin(\\theta) - L*sin(\\theta)*cos(\\phi)**2)*sin(\\phi)],\n[              sin(\\phi), 0,              -cos(\\phi),                                                        0,               sin(\\phi),                                                                    0],\n[                      0, 1,                       0,                                                        0,                       0,                                                                    0]])",
      "text/latex": "$\\displaystyle \\left[\\begin{matrix}L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & 0 & L \\sin{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & - L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} - L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)} & L \\cos{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & \\left(- L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} - L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)} & 0 & L \\sin{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} & L \\sin^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} + L \\cos^{2}{\\left(\\phi \\right)} \\cos{\\left(\\theta \\right)} & L \\sin{\\left(\\theta \\right)} \\cos{\\left(\\phi \\right)} & \\left(- L \\sin^{2}{\\left(\\phi \\right)} \\sin{\\left(\\theta \\right)} - L \\sin{\\left(\\theta \\right)} \\cos^{2}{\\left(\\phi \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\\\sin{\\left(\\phi \\right)} & 0 & - \\cos{\\left(\\phi \\right)} & 0 & \\sin{\\left(\\phi \\right)} & 0\\\\0 & 1 & 0 & 0 & 0 & 0\\end{matrix}\\right]$"
     },
     "metadata": {}
    },
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "rank{D} = 4 = r_i = n\ndet{D} = \n"
     ]
    }
   ],
   "source": [
    "g1212 = NawiasLiego(g1, g112, \"g1212\")\n",
    "D3 = KolejneD([g1, g2, g12, g112, g212, g1212])"
   ]
  },
  {
   "source": [
    "________________"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "source": [
    "### Wektor wzorostu i $r_i(q)$"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "output_type": "stream",
     "name": "stdout",
     "text": [
      "r_0(q) = 2\nr_1(q) = 3\nr_2(q) = 4\nr_3(q) = 4\n"
     ]
    }
   ],
   "source": [
    "for i in range(len(wektorWzrostu)):\n",
    "    print( f\"r_{i}(q) = {wektorWzrostu[i]}\")"
   ]
  },
  {
   "source": [
    "### Stopień nieholonomiczności"
   ],
   "cell_type": "markdown",
   "metadata": {}
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "2"
      ]
     },
     "metadata": {},
     "execution_count": 16
    }
   ],
   "source": [
    "stopienNieholonomicznosci"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "output_type": "execute_result",
     "data": {
      "text/plain": [
       "4"
      ]
     },
     "metadata": {},
     "execution_count": 17
    }
   ],
   "source": [
    "wymiarPrzestrzeniKonfig"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "#TODO liczba ograniczeń nieholonomicznych - l\n",
    "#TODO liczba generatoró układubezdryfowego - m"
   ]
  }
 ]
}