KonfiguracjeOsobliwe.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Konfiguracje osobliwe\n",
    "\n",
    "Pozwala wskazać konfiguracje osobliwe."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "from RobotykaLib import *\n",
    "q1, q2, q3, q4 = symbols('q1 q2 q3 q4', real=True)  # q\n",
    "a, a1, a2, a3 = symbols('a a1 a2 a3', real=True)    # a\n",
    "d, d1, d2, d3 = symbols('d d1 d2 d3', real=True)    # d\n",
    "l, l1, l2, l3 = symbols('l l1 l2 l3', real=True)\n",
    "alfa, beta,gamma = symbols('\\\\alpha \\\\beta \\\\gamma', real=True)\n",
    "x, y, z = symbols('x y z')                          # osie\n",
    "#\n",
    "# rot(z,q1)*trans(z,a1)*trans(x,a)*rot(z,pi/2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Dane: Wektor współrzędnych"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = [q1, q2, q3]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Dane: Macierz Kinematyki"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "K = [a2*q1**2 + d2, d1*(q2**2 + q3**2)]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_____________________________________"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}a_{2} q_{1}^{2} + d_{2}\\\\d_{1} \\left(q_{2}^{2} + q_{3}^{2}\\right)\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[     a2*q1**2 + d2],\n",
       "[d1*(q2**2 + q3**2)]])"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Matrix(K)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Obliczenia:\n",
    "### Jakobian analityczny"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}2 a_{2} q_{1} & 0 & 0\\\\0 & 2 d_{1} q_{2} & 2 d_{1} q_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[2*a2*q1,       0,       0],\n",
       "[      0, 2*d1*q2, 2*d1*q3]])"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "J = []\n",
    "for Krow in K:\n",
    "    row = []\n",
    "    for q_i in q:\n",
    "        row.append(Krow.diff(q_i).doit())\n",
    "    J.append(row)\n",
    "J = Matrix(J)\n",
    "J"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Wyznacznik jakobianu"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- Minor 0 -----------------------\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0\\\\2 d_{1} q_{2} & 2 d_{1} q_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[      0,       0],\n",
       "[2*d1*q2, 2*d1*q3]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle 0$"
      ],
      "text/plain": [
       "0"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- Minor 1 -----------------------\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}2 a_{2} q_{1} & 0\\\\0 & 2 d_{1} q_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[2*a2*q1,       0],\n",
       "[      0, 2*d1*q3]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle 4 a_{2} d_{1} q_{1} q_{3}$"
      ],
      "text/plain": [
       "4*a2*d1*q1*q3"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[(0, q2, q3), (q1, q2, 0)]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0\\\\q_{2}\\\\q_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0],\n",
       "[q2],\n",
       "[q3]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}q_{1}\\\\q_{2}\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[q1],\n",
       "[q2],\n",
       "[ 0]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "--- Minor 2 -----------------------\n"
     ]
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}2 a_{2} q_{1} & 0\\\\0 & 2 d_{1} q_{2}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[2*a2*q1,       0],\n",
       "[      0, 2*d1*q2]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle 4 a_{2} d_{1} q_{1} q_{2}$"
      ],
      "text/plain": [
       "4*a2*d1*q1*q2"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "[(0, q2, q3), (q1, 0, q3)]"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0\\\\q_{2}\\\\q_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ 0],\n",
       "[q2],\n",
       "[q3]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}q_{1}\\\\0\\\\q_{3}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[q1],\n",
       "[ 0],\n",
       "[q3]])"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "if J.shape[1] == J.shape[0]:\n",
    "    display(det(J))\n",
    "    display(solve(det(J), q))\n",
    "elif J.shape[1] == J.shape[0] + 1:\n",
    "    for j in range(0, J.shape[1]):\n",
    "        Jk = J.copy()\n",
    "        Jk.col_del(j)\n",
    "        print(f\"--- Minor {j} -----------------------\")\n",
    "        display(Jk)\n",
    "        display(det(Jk))\n",
    "        display(solve(det(Jk), q))\n",
    "        [display(Matrix(v)) for v in solve(det(Jk), q)]\n",
    "elif J.shape[1] < J.shape[0]:\n",
    "    print(\"Wszystkie konfiguracje są osobliwe\")\n",
    "else:\n",
    "    print(\"Policz na piechotę lub rozbuduj notatnik\")\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.8.3-final"
  },
  "orig_nbformat": 2
 },
 "nbformat": 4,
 "nbformat_minor": 2
}