UR5Transformation.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Diner Robot Inverse Kinematics calculations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sp\n",
    "import numpy as np\n",
    "import math\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "UR5 angles and link lengths:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3, q4, q5, q6, q7, = sp.symbols('q1 q2 q3 q4 q5 q6 q7')\n",
    "d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823]\n",
    "a = [0.0, -0.425, -0.39225, 0.0, 0.0, 0]\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Transformation Matrix:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "T0 = sp.Matrix([[1, 0, 0, 0],\n",
    "               [0, 1, 0, 0],\n",
    "               [0, 0, 1, 0],\n",
    "               [0, 0, 0, 1]])\n",
    "\n",
    "T1 = sp.Matrix([[sp.cos(q1), 0, -sp.sin(q1), 0],\n",
    "                [sp.sin(q1),     0, sp.cos(q1), 0],\n",
    "                [0,             -1,   0,       d[0]],\n",
    "                [0, 0, 0, 1]])\n",
    "\n",
    "#T1 = T1.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})\n",
    "\n",
    "T2 = sp.Matrix([[sp.cos(q2), 0, sp.sin(q2), 0],\n",
    "                [sp.sin(q2), 0, -sp.cos(q2), 0],\n",
    "                [0, 1, 0, d[1]],\n",
    "                [0, 0, 0, 1]])\n",
    "\n",
    "#T2 = T2.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})\n",
    "q3 = 0\n",
    "T3 = sp.Matrix([[sp.cos(q3), 0, sp.sin(q3), 0],\n",
    "                [sp.sin(q3), 0, -sp.cos(q3), 0],\n",
    "                [0, 1, 0, d[2]],\n",
    "                [0, 0, 0, 1]])\n",
    "\n",
    "#T3 = T3.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})\n",
    "\n",
    "T4 = sp.Matrix([[sp.cos(q4), 0, -sp.sin(q4), 0],\n",
    "                [sp.sin(q4), 0, sp.cos(q4), 0],\n",
    "                [0, -1, 0, d[3]],\n",
    "                [0, 0, 0, 1]])\n",
    "#T4 = T4.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})\n",
    "\n",
    "T5 = sp.Matrix([[sp.cos(q5), 0, -sp.sin(q5), 0],\n",
    "                [sp.sin(q5), 0, sp.cos(q5), 0],\n",
    "                [0, -1, 0, d[4]],\n",
    "                [0, 0, 0, 1]])\n",
    "\n",
    "\n",
    "#T5 = T5.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})\n",
    "\n",
    "T6 = sp.Matrix([[sp.cos(q6), 0, sp.sin(q6), 0],\n",
    "                [sp.sin(q6), 0, -sp.cos(q6), 0],\n",
    "                [0, 1, 0, d[5]],\n",
    "                [0, 0, 0, 1]])\n",
    "#T6= T6.subs({q1: math.radians(90), q2: math.radians(0), q3: math.radians(0), q4: math.radians(-90), q5: math.radians(0), q6: math.radians(0), q7: math.radians(0)})\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Final Transformation Matrix:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\left(\\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{6} \\right)} & - \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{5} \\right)} - \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} & \\left(\\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} - \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{6} \\right)} & - 0.0823 \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{5} \\right)} - 0.0823 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} + 0.10915 \\sin{\\left(q_{1} \\right)} + 0.09465 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} - 0.09465 \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\\\\\left(\\left(\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} + \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{6} \\right)} & - \\left(\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} + \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} & \\left(\\left(\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} + \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{6} \\right)} - \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{6} \\right)} & - 0.0823 \\left(\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} + \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + 0.09465 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} - 0.09465 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} + 0.0823 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} - 0.10915 \\cos{\\left(q_{1} \\right)}\\\\\\left(- \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} & - \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)} & - \\left(- \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{5} \\right)} & - 0.0823 \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + 0.09465 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} + 0.09465 \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + 0.089159\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[((sin(q2)*sin(q4)*cos(q1) + cos(q1)*cos(q2)*cos(q4))*cos(q5) - sin(q1)*sin(q5))*cos(q6) + (-sin(q2)*cos(q1)*cos(q4) + sin(q4)*cos(q1)*cos(q2))*sin(q6), -(sin(q2)*sin(q4)*cos(q1) + cos(q1)*cos(q2)*cos(q4))*sin(q5) - sin(q1)*cos(q5), ((sin(q2)*sin(q4)*cos(q1) + cos(q1)*cos(q2)*cos(q4))*cos(q5) - sin(q1)*sin(q5))*sin(q6) - (-sin(q2)*cos(q1)*cos(q4) + sin(q4)*cos(q1)*cos(q2))*cos(q6), -0.0823*(sin(q2)*sin(q4)*cos(q1) + cos(q1)*cos(q2)*cos(q4))*sin(q5) - 0.0823*sin(q1)*cos(q5) + 0.10915*sin(q1) + 0.09465*sin(q2)*cos(q1)*cos(q4) - 0.09465*sin(q4)*cos(q1)*cos(q2)],\n",
       "[((sin(q1)*sin(q2)*sin(q4) + sin(q1)*cos(q2)*cos(q4))*cos(q5) + sin(q5)*cos(q1))*cos(q6) + (-sin(q1)*sin(q2)*cos(q4) + sin(q1)*sin(q4)*cos(q2))*sin(q6), -(sin(q1)*sin(q2)*sin(q4) + sin(q1)*cos(q2)*cos(q4))*sin(q5) + cos(q1)*cos(q5), ((sin(q1)*sin(q2)*sin(q4) + sin(q1)*cos(q2)*cos(q4))*cos(q5) + sin(q5)*cos(q1))*sin(q6) - (-sin(q1)*sin(q2)*cos(q4) + sin(q1)*sin(q4)*cos(q2))*cos(q6), -0.0823*(sin(q1)*sin(q2)*sin(q4) + sin(q1)*cos(q2)*cos(q4))*sin(q5) + 0.09465*sin(q1)*sin(q2)*cos(q4) - 0.09465*sin(q1)*sin(q4)*cos(q2) + 0.0823*cos(q1)*cos(q5) - 0.10915*cos(q1)],\n",
       "[                                                   (-sin(q2)*sin(q4) - cos(q2)*cos(q4))*sin(q6) + (-sin(q2)*cos(q4) + sin(q4)*cos(q2))*cos(q5)*cos(q6),                                  -(-sin(q2)*cos(q4) + sin(q4)*cos(q2))*sin(q5),                                                   -(-sin(q2)*sin(q4) - cos(q2)*cos(q4))*cos(q6) + (-sin(q2)*cos(q4) + sin(q4)*cos(q2))*sin(q6)*cos(q5),                                                                -0.0823*(-sin(q2)*cos(q4) + sin(q4)*cos(q2))*sin(q5) + 0.09465*sin(q2)*sin(q4) + 0.09465*cos(q2)*cos(q4) + 0.089159],\n",
       "[                                                                                                                                                     0,                                                                              0,                                                                                                                                                      0,                                                                                                                                                                                  1]])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = (T0*T1*T2*T3*T4*T5*T6)\n",
    "\n",
    "T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Transformation at start position:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-1.74523786614818 \\cdot 10^{-5} & -0.999999999695393 & -1.74535971126676 \\cdot 10^{-5} & 0.0268483480543087\\\\-5.23601821401489 \\cdot 10^{-5} & -1.74526832816749 \\cdot 10^{-5} & 0.999999998476908 & 0.0946504685989643\\\\-0.999999998476913 & 1.7453292508424 \\cdot 10^{-5} & -5.23598775333623 \\cdot 10^{-5} & 0.0891571324977001\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-1.74523786614818e-5,   -0.999999999695393, -1.74535971126676e-5, 0.0268483480543087],\n",
       "[-5.23601821401489e-5, -1.74526832816749e-5,    0.999999998476908, 0.0946504685989643],\n",
       "[  -0.999999998476913,   1.7453292508424e-5, -5.23598775333623e-5, 0.0891571324977001],\n",
       "[                   0,                    0,                    0,                  1]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q = sp.Matrix([ [math.radians(90.001)], [math.radians(0.001)], [math.radians(-90.001)], [math.radians(0.001)], [math.radians(0.001)],  [math.radians(0.001)]])\n",
    "Tstart = T.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })\n",
    "Tstart"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Jacobian calculation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0.81725 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} - 0.0823 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} + 0.09465 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} + 0.0823 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} + 0.10915 \\cos{\\left(q_{1} \\right)} & \\left(- 0.0823 \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} - 0.81725 \\cos{\\left(q_{2} \\right)} - 0.09465 \\cos{\\left(q_{2} + q_{4} \\right)}\\right) \\cos{\\left(q_{1} \\right)} & \\left(- 0.0823 \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} - 0.39225 \\cos{\\left(q_{2} \\right)} - 0.09465 \\cos{\\left(q_{2} + q_{4} \\right)}\\right) \\cos{\\left(q_{1} \\right)} & \\left(- 0.0823 \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} - 0.09465 \\cos{\\left(q_{2} + q_{4} \\right)}\\right) \\cos{\\left(q_{1} \\right)} & - 0.0823 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} + 0.0823 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} & 0\\\\0.0823 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} + 0.10915 \\sin{\\left(q_{1} \\right)} - 0.81725 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} + 0.0823 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} - 0.09465 \\sin{\\left(q_{2} + q_{4} \\right)} \\cos{\\left(q_{1} \\right)} & \\left(- 0.0823 \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} - 0.81725 \\cos{\\left(q_{2} \\right)} - 0.09465 \\cos{\\left(q_{2} + q_{4} \\right)}\\right) \\sin{\\left(q_{1} \\right)} & \\left(- 0.0823 \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} - 0.39225 \\cos{\\left(q_{2} \\right)} - 0.09465 \\cos{\\left(q_{2} + q_{4} \\right)}\\right) \\sin{\\left(q_{1} \\right)} & \\left(- 0.0823 \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} - 0.09465 \\cos{\\left(q_{2} + q_{4} \\right)}\\right) \\sin{\\left(q_{1} \\right)} & 0.0823 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} + 0.0823 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} & 0\\\\0 & - 0.81725 \\sin{\\left(q_{2} \\right)} + 0.0823 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} - 0.09465 \\sin{\\left(q_{2} + q_{4} \\right)} & - 0.39225 \\sin{\\left(q_{2} \\right)} + 0.0823 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} - 0.09465 \\sin{\\left(q_{2} + q_{4} \\right)} & 0.0823 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} - 0.09465 \\sin{\\left(q_{2} + q_{4} \\right)} & 0.0823 \\sin{\\left(q_{2} + q_{4} \\right)} \\cos{\\left(q_{5} \\right)} & 0\\\\0 & \\sin{\\left(q_{1} \\right)} & \\sin{\\left(q_{1} \\right)} & \\sin{\\left(q_{1} \\right)} & - \\sin{\\left(q_{2} + q_{4} \\right)} \\cos{\\left(q_{1} \\right)} & \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)}\\\\0 & - \\cos{\\left(q_{1} \\right)} & - \\cos{\\left(q_{1} \\right)} & - \\cos{\\left(q_{1} \\right)} & - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)} & \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} + q_{4} \\right)} - \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)}\\\\1 & 0 & 0 & 0 & \\cos{\\left(q_{2} + q_{4} \\right)} & \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{2} + q_{4} \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0.81725*sin(q1)*sin(q2) - 0.0823*sin(q1)*sin(q5)*cos(q2 + q4) + 0.09465*sin(q1)*sin(q2 + q4) + 0.0823*cos(q1)*cos(q5) + 0.10915*cos(q1), (-0.0823*sin(q5)*sin(q2 + q4) - 0.81725*cos(q2) - 0.09465*cos(q2 + q4))*cos(q1), (-0.0823*sin(q5)*sin(q2 + q4) - 0.39225*cos(q2) - 0.09465*cos(q2 + q4))*cos(q1), (-0.0823*sin(q5)*sin(q2 + q4) - 0.09465*cos(q2 + q4))*cos(q1), -0.0823*sin(q1)*sin(q5) + 0.0823*cos(q1)*cos(q5)*cos(q2 + q4),                                              0],\n",
       "[0.0823*sin(q1)*cos(q5) + 0.10915*sin(q1) - 0.81725*sin(q2)*cos(q1) + 0.0823*sin(q5)*cos(q1)*cos(q2 + q4) - 0.09465*sin(q2 + q4)*cos(q1), (-0.0823*sin(q5)*sin(q2 + q4) - 0.81725*cos(q2) - 0.09465*cos(q2 + q4))*sin(q1), (-0.0823*sin(q5)*sin(q2 + q4) - 0.39225*cos(q2) - 0.09465*cos(q2 + q4))*sin(q1), (-0.0823*sin(q5)*sin(q2 + q4) - 0.09465*cos(q2 + q4))*sin(q1),  0.0823*sin(q1)*cos(q5)*cos(q2 + q4) + 0.0823*sin(q5)*cos(q1),                                              0],\n",
       "[                                                                                                                                      0,           -0.81725*sin(q2) + 0.0823*sin(q5)*cos(q2 + q4) - 0.09465*sin(q2 + q4),           -0.39225*sin(q2) + 0.0823*sin(q5)*cos(q2 + q4) - 0.09465*sin(q2 + q4),            0.0823*sin(q5)*cos(q2 + q4) - 0.09465*sin(q2 + q4),                                   0.0823*sin(q2 + q4)*cos(q5),                                              0],\n",
       "[                                                                                                                                      0,                                                                         sin(q1),                                                                         sin(q1),                                                       sin(q1),                                         -sin(q2 + q4)*cos(q1), sin(q1)*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q4)],\n",
       "[                                                                                                                                      0,                                                                        -cos(q1),                                                                        -cos(q1),                                                      -cos(q1),                                         -sin(q1)*sin(q2 + q4), sin(q1)*sin(q5)*cos(q2 + q4) - cos(q1)*cos(q5)],\n",
       "[                                                                                                                                      1,                                                                               0,                                                                               0,                                                             0,                                                  cos(q2 + q4),                           sin(q5)*sin(q2 + q4)]])"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "J = sp.Matrix([[-a[1]*sp.sin(q1)*sp.sin(q2) - d[5]*sp.sin(q1)*sp.sin(q5)*sp.cos(q2 + q3 + q4) - a[2]*sp.sin(q1)*sp.sin(q2 + q3) + d[4]*sp.sin(q1)*sp.sin(q2 + q3 + q4) + d[5]*sp.cos(q1)*sp.cos(q5) + d[3]*sp.cos(q1),\n",
    "                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[1]*sp.cos(q2) -\n",
    "                  a[2]*sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),\n",
    "                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[2] *\n",
    "                  sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),\n",
    "                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) +\n",
    "                  d[4]*sp.cos(q2 + q3 + q4))*sp.cos(q1),\n",
    "                - d[5]*sp.sin(q1)*sp.sin(q5) + d[5]*sp.cos(q1) *\n",
    "                sp.cos(q5)*sp.cos(q2 + q3 + q4),\n",
    "                0],\n",
    "               [d[5]*sp.sin(q1)*sp.cos(q5) + d[3]*sp.sin(q1) + a[1]*sp.sin(q2)*sp.cos(q1) + d[5]*sp.sin(q5)*sp.cos(q1)*sp.cos(q2 + q3 + q4) + a[2]*sp.sin(q2 + q3)*sp.cos(q1) - d[4]*sp.sin(q2 + q3 + q4)*sp.cos(q1),\n",
    "                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[1]*sp.cos(q2) -\n",
    "                   a[2]*sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),\n",
    "                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) - a[2] *\n",
    "                   sp.cos(q2 + q3) + d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),\n",
    "                -(d[5]*sp.sin(q5)*sp.sin(q2 + q3 + q4) +\n",
    "                   d[4]*sp.cos(q2 + q3 + q4))*sp.sin(q1),\n",
    "                d[5]*sp.sin(q1)*sp.cos(q5)*sp.cos(q2 + q3 + q4) +\n",
    "                d[5]*sp.sin(q5)*sp.cos(q1),\n",
    "                0],\n",
    "               [0,\n",
    "                a[1]*sp.sin(q2) + d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) +\n",
    "                a[2]*sp.sin(q2 + q3) - d[4]*sp.sin(q2 + q3 + q4),\n",
    "                d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) + a[2] *\n",
    "                sp.sin(q2 + q3) - d[4]*sp.sin(q2 + q3 + q4),\n",
    "                d[5]*sp.sin(q5)*sp.cos(q2 + q3 + q4) -\n",
    "                d[4]*sp.sin(q2 + q3 + q4),\n",
    "                d[5]*sp.sin(q2 + q3 + q4)*sp.cos(q5),\n",
    "                0],\n",
    "               [0, sp.sin(q1), sp.sin(q1), sp.sin(q1), -sp.sin(q2 + q3 + q4)*sp.cos(q1),\n",
    "                sp.sin(q1)*sp.cos(q5) + sp.sin(q5)*sp.cos(q1)*sp.cos(q2 + q3 + q4)],\n",
    "               [0, -sp.cos(q1), -sp.cos(q1), -sp.cos(q1), -sp.sin(q1)*sp.sin(q2 + q3 + q4),\n",
    "                sp.sin(q1)*sp.sin(q5)*sp.cos(q2 + q3 + q4) - sp.cos(q1)*sp.cos(q5)],\n",
    "               [1, 0, 0, 0, sp.cos(q2 + q3 + q4), sp.sin(q5)*sp.sin(q2 + q3 + q4)]])\n",
    "J\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-0.0946390777151275 & 1.42636782391076 \\cdot 10^{-5} & 6.84602891958906 \\cdot 10^{-6} & -2.50700136463318 \\cdot 10^{-11} & -1.43640597409963 \\cdot 10^{-6} & 0\\\\0.191448348253117 & -0.817248563345077 & -0.39224856347454 & 1.43640597411485 \\cdot 10^{-6} & -2.50700268809156 \\cdot 10^{-11} & 0\\\\0 & 0.0946357362966888 & 0.0946431539460094 & 0.09465 & -0.082299999987465 & 0\\\\0 & 0.999999999847691 & 0.999999999847691 & 0.999999999847691 & -1.74532925191725 \\cdot 10^{-5} & 0.999999999695383\\\\0 & 1.74532925191725 \\cdot 10^{-5} & 1.74532925191725 \\cdot 10^{-5} & 1.74532925191725 \\cdot 10^{-5} & 0.999999999847691 & 1.74532925165142 \\cdot 10^{-5}\\\\1 & 0 & 0 & 0 & -1.60812264967664 \\cdot 10^{-16} & -1.74532925190572 \\cdot 10^{-5}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-0.0946390777151275, 1.42636782391076e-5, 6.84602891958906e-6, -2.50700136463318e-11,  -1.43640597409963e-6,                    0],\n",
       "[  0.191448348253117,  -0.817248563345077,   -0.39224856347454,   1.43640597411485e-6, -2.50700268809156e-11,                    0],\n",
       "[                  0,  0.0946357362966888,  0.0946431539460094,               0.09465,    -0.082299999987465,                    0],\n",
       "[                  0,   0.999999999847691,   0.999999999847691,     0.999999999847691,  -1.74532925191725e-5,    0.999999999695383],\n",
       "[                  0, 1.74532925191725e-5, 1.74532925191725e-5,   1.74532925191725e-5,     0.999999999847691,  1.74532925165142e-5],\n",
       "[                  1,                   0,                   0,                     0, -1.60812264967664e-16, -1.74532925190572e-5]])"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q = sp.Matrix([ [math.radians(90.001)], [math.radians(0.001)], [math.radians(-90.001)], [math.radians(0.001)], [math.radians(0.001)],  [math.radians(0.001)]])\n",
    "Jstart = J.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })\n",
    "Jstart"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Waypoint Generation:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x:  -0.103148396181722\n",
      "y:  0.0438657283681490\n",
      "z:  -0.0355430285281103\n",
      "x:  -0.147273159898797\n",
      "y:  -0.125202044444935\n",
      "z:  0.124921686787584\n",
      "x:  -0.00937470520408906\n",
      "y:  -0.176525091404733\n",
      "z:  0.173067649289900\n",
      "x:  0.0427741715230540\n",
      "y:  0.0457480001443346\n",
      "z:  0.174259377166235\n",
      "x:  0.200543121647522\n",
      "y:  -0.0465204867667080\n",
      "z:  0.0942175473649006\n",
      "x:  0.169376367062934\n",
      "y:  -0.0628805749009145\n",
      "z:  0.147405750141163\n",
      "x:  -0.134108848018656\n",
      "y:  0.0477309380107116\n",
      "z:  0.0328496368472096\n",
      "x:  -0.0350128115402672\n",
      "y:  0.0515125302498191\n",
      "z:  0.00857264081972968\n",
      "x:  -0.169606940063976\n",
      "y:  0.0988007497385818\n",
      "z:  0.151133368225458\n",
      "x:  -0.184433570610797\n",
      "y:  -0.0372919938820578\n",
      "z:  0.186481385745066\n",
      "x:  -0.00990999729662447\n",
      "y:  0.0445627947231336\n",
      "z:  0.177971844273570\n",
      "x:  -0.0576029004372159\n",
      "y:  0.0709841622164546\n",
      "z:  -0.0310210247623603\n",
      "Singularity occured\n",
      "x:  0.203005119143047\n",
      "y:  0.0201969079554846\n",
      "z:  0.152017052475800\n",
      "x:  0.160648802671862\n",
      "y:  -0.0530220562046413\n",
      "z:  0.0914412222132482\n",
      "x:  0.0709230323404108\n",
      "y:  0.0714487921069014\n",
      "z:  0.0752101830647717\n",
      "x:  0.113419298694141\n",
      "y:  -0.138450091962684\n",
      "z:  -0.0138227579807484\n",
      "x:  -0.0662967586403884\n",
      "y:  -0.0668554221950254\n",
      "z:  0.00915276648810031\n",
      "x:  -0.199541201193674\n",
      "y:  -0.0354925315615094\n",
      "z:  0.0787001938399681\n",
      "x:  -0.123104025540629\n",
      "y:  0.0115586624282927\n",
      "z:  0.173154659364518\n",
      "x:  -0.0814403328514375\n",
      "y:  0.162513388345019\n",
      "z:  0.185558652419942\n",
      "x:  0.0380996583327256\n",
      "y:  0.0679197985395505\n",
      "z:  0.149309033042676\n",
      "x:  -0.0174403172925617\n",
      "y:  0.0869443544770498\n",
      "z:  -0.0319437852086548\n",
      "x:  -0.100645009355833\n",
      "y:  0.0332332432424140\n",
      "z:  -0.0324276269241449\n",
      "x:  -0.0969571742203594\n",
      "y:  0.0103086517259070\n",
      "z:  0.200678604211980\n",
      "x:  0.0590625064303628\n",
      "y:  -0.0451298281083800\n",
      "z:  -0.00742153409214925\n",
      "x:  -0.0407356020896451\n",
      "y:  0.0883687577773600\n",
      "z:  0.126692415459500\n",
      "x:  0.0703878408935651\n",
      "y:  0.0663175685738039\n",
      "z:  -0.00434934606126991\n",
      "x:  0.0158311061762567\n",
      "y:  0.0459512835742991\n",
      "z:  0.192073463732889\n",
      "Singularity occured\n",
      "x:  -0.147150931015434\n",
      "y:  0.139781190636149\n",
      "z:  0.0372244168588145\n",
      "x:  -0.0466785404697651\n",
      "y:  0.200460967122675\n",
      "z:  0.145376416304960\n",
      "x:  -0.150464368091746\n",
      "y:  0.0192014133460974\n",
      "z:  0.0338198542996504\n",
      "x:  0.0143818348683777\n",
      "y:  -0.0770862493933570\n",
      "z:  -0.0300539329563644\n",
      "Singularity occured\n",
      "x:  0.174641032076904\n",
      "y:  0.0894126858472976\n",
      "z:  0.00479471558306991\n",
      "x:  -0.103326895483028\n",
      "y:  -0.160937358613615\n",
      "z:  0.0211206718056824\n",
      "x:  0.0885162493401619\n",
      "y:  0.180639467472392\n",
      "z:  0.134417659979678\n",
      "x:  -0.0165738006154281\n",
      "y:  0.0488425167188685\n",
      "z:  0.180814713126384\n",
      "x:  0.126874745384624\n",
      "y:  -0.0159131656002903\n",
      "z:  0.0325312652304365\n",
      "x:  -0.156766615937550\n",
      "y:  -0.0590423776609432\n",
      "z:  0.192240684272611\n",
      "x:  0.0513729551442988\n",
      "y:  -0.0448545243770389\n",
      "z:  -0.00971169175926069\n",
      "x:  0.0503098007065950\n",
      "y:  -0.101772739030657\n",
      "z:  -0.0299103754424827\n"
     ]
    }
   ],
   "source": [
    "qdash = sp.Matrix([ [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)], [math.radians(0)]])\n",
    "\n",
    "theta = np.pi/2 \n",
    "\n",
    "while(theta< ((2*np.pi) + np.pi/2)):\n",
    "    Js = J.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5]})\n",
    "    try:\n",
    "        Jp = Js.inv()\n",
    "    except:\n",
    "        print('Singularity occured')\n",
    "        q = (q + qdash).evalf()\n",
    "    radius = 100\n",
    "    Vx = -radius*np.sin(theta)*((2*np.pi) / 40)\n",
    "    Vz = radius*np.cos(theta)*((2*np.pi) / 40)\n",
    "    theta += ((2*np.pi) / 40)\n",
    "    V = sp.Matrix([ [Vx], [0], [Vz], [0], [0], [0]])\n",
    "    qdash=Jp*V\n",
    "    #q_current= q_pervious + 𝑞̇_current . ∆t\n",
    "    q = (q + qdash).evalf()\n",
    "     \n",
    "    Tdash = T.subs({q1: q[0], q2: q[1], q4: q[2], q5: q[3], q6: q[4], q7: q[5] })\n",
    "    x = Tdash[0,3]\n",
    "    y = Tdash[1,3]\n",
    "    z = Tdash[2,3]\n",
    "\n",
    "    print('x: ',x)\n",
    "    print('y: ',y)\n",
    "    print('z: ',z)\n",
    "\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1"
  },
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "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.10"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}