Control Brazo Robotico .ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<h2> Control Brazo Robótico (7DOF)</h2>\n",
    "\n",
    "<br>Requiere que los archivos 'sim.py', 'simConst.py', 'remoteapi.dll' estén alojados en la misma carpeta que este cuaderno de Jupyter."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 105,
   "metadata": {},
   "outputs": [],
   "source": [
    "# importamos las librerías necesarias\n",
    "import sim \n",
    "import sympy as sp  # librería para cálculo simbólico\n",
    "import numpy as np  # libreria para calculos matematicos"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "conectado a 19999\n"
     ]
    }
   ],
   "source": [
    "def connect(port):\n",
    "# Establece la conexión a VREP\n",
    "# port debe coincidir con el puerto de conexión en VREP\n",
    "# retorna el número de cliente o -1 si no puede establecer conexión\n",
    "    sim.simxFinish(-1) # just in case, close all opened connections\n",
    "    clientID=sim.simxStart('127.0.0.1',port,True,True,2000,5) # Conectarse\n",
    "    if clientID == 0: print(\"conectado a\", port)\n",
    "    else: print(\"no se pudo conectar\")\n",
    "    return clientID\n",
    "clientID = connect(19999)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "20 22 24 26 28 30 32\n"
     ]
    }
   ],
   "source": [
    "# Requerimos los manejadores para las articulaciones\n",
    "retCode,joint1=sim.simxGetObjectHandle(clientID,'redundantRob_joint1',sim.simx_opmode_blocking)\n",
    "retCode,joint2=sim.simxGetObjectHandle(clientID,'redundantRob_joint2',sim.simx_opmode_blocking)\n",
    "retCode,joint3=sim.simxGetObjectHandle(clientID,'redundantRob_joint3',sim.simx_opmode_blocking)\n",
    "retCode,joint4=sim.simxGetObjectHandle(clientID,'redundantRob_joint4',sim.simx_opmode_blocking)\n",
    "retCode,joint5=sim.simxGetObjectHandle(clientID,'redundantRob_joint5',sim.simx_opmode_blocking)\n",
    "retCode,joint6=sim.simxGetObjectHandle(clientID,'redundantRob_joint6',sim.simx_opmode_blocking)\n",
    "retCode,joint7=sim.simxGetObjectHandle(clientID,'redundantRob_joint7',sim.simx_opmode_blocking)\n",
    "print(joint1, joint2, joint3, joint4, joint5, joint6, joint7)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 108,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Enviamos las posiciones a las articulaciones\n",
    "\n",
    "j1 = 0 * np.pi/180\n",
    "j2 = 0 * np.pi/180\n",
    "j3 = 0 * np.pi/180\n",
    "j4 = 0 * np.pi/180\n",
    "j5 = 0 * np.pi/180\n",
    "j6 = 0 * np.pi/180\n",
    "j7 = 0 * np.pi/180\n",
    "\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 120,
   "metadata": {},
   "outputs": [],
   "source": [
    "pos0()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 119,
   "metadata": {},
   "outputs": [],
   "source": [
    "pos1()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 114,
   "metadata": {},
   "outputs": [],
   "source": [
    "pos2()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 115,
   "metadata": {},
   "outputs": [],
   "source": [
    "pos3()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 116,
   "metadata": {},
   "outputs": [],
   "source": [
    "pos4()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 118,
   "metadata": {},
   "outputs": [],
   "source": [
    "pos5()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 109,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos0 ():\n",
    "    j1 = 0 * np.pi/180\n",
    "    j2 = 0 * np.pi/180\n",
    "    j3 = 0 * np.pi/180\n",
    "    j4 = 0 * np.pi/180\n",
    "    j5 = 0 * np.pi/180\n",
    "    j6 = 0 * np.pi/180\n",
    "    j7 = 0 * np.pi/180\n",
    "\n",
    "\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos1 ():\n",
    "    j1 = 90 * np.pi/180\n",
    "    j2 = -30 * np.pi/180\n",
    "    j3 = 0 * np.pi/180\n",
    "    j4 = -30 * np.pi/180\n",
    "    j5 = 0 * np.pi/180\n",
    "    j6 = 0 * np.pi/180\n",
    "    j7 = -30 * np.pi/180\n",
    "\n",
    "\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 111,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos2 ():\n",
    "    j1 = 3 * np.pi/180\n",
    "    j2 = -30 * np.pi/180\n",
    "    j3 = 0 * np.pi/180\n",
    "    j4 = -50 * np.pi/180\n",
    "    j5 = 0 * np.pi/180\n",
    "    j6 = 0 * np.pi/180\n",
    "    j7 = -15 * np.pi/180\n",
    "\n",
    "\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 112,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos3 ():\n",
    "    j1 = 3 * np.pi/180\n",
    "    j2 = -50 * np.pi/180\n",
    "    j3 = 0 * np.pi/180\n",
    "    j4 = -38 * np.pi/180\n",
    "    j5 = 0 * np.pi/180\n",
    "    j6 = 0 * np.pi/180\n",
    "    j7 = -5 * np.pi/180\n",
    "\n",
    "\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 113,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos4 ():\n",
    "    j1 = 3 * np.pi/180\n",
    "    j2 = -50 * np.pi/180\n",
    "    j3 = 0 * np.pi/180\n",
    "    j4 = -32 * np.pi/180\n",
    "    j5 = 0 * np.pi/180\n",
    "    j6 = 0 * np.pi/180\n",
    "    j7 = -10 * np.pi/180\n",
    "\n",
    "\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 96,
   "metadata": {},
   "outputs": [],
   "source": [
    "def pos5 ():\n",
    "    j1 = 90 * np.pi/180\n",
    "    j2 = -30 * np.pi/180\n",
    "    j3 = 0 * np.pi/180\n",
    "    j4 = -30 * np.pi/180\n",
    "    j5 = 0 * np.pi/180\n",
    "    j6 = 0 * np.pi/180\n",
    "    j7 = -60 * np.pi/180\n",
    "\n",
    "\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint1, j1, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint2, j2, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint3, j3, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint4, j4, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint5, j5, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j6, sim.simx_opmode_oneshot)\n",
    "    retCode = sim.simxSetJointTargetPosition(clientID, joint6, j7, sim.simx_opmode_oneshot)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "###  1. Preparamos el modelo del robot\n",
    "Utilizaremos la librería sympy para obtener las acuaciones de cinemática directa y cinemática inversa.\n",
    "<br> Para más detalles refiérase a la documentación de la librería:\n",
    "<br> https://docs.sympy.org/latest/index.html"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Comenzaremos el trabajo desde la descripción de Denavit-Hartenberg para el robot\n",
    "#      theta     |      d      |      a      |    alpha\n",
    "# ---------------------------------------------------------\n",
    "#      q1        |    0.13     |    0     |     -90\n",
    "#      q2        |      0      |   0.11   |      90\n",
    "#      q3        |      0      |   0.16   |     -90\n",
    "#      q4        |      0      |   0.13   |      90\n",
    "#      q5        |      0      |   0.13   |     -90\n",
    "#      q6        |      0      |   0.18   |       0\n",
    "#      q7        |      0      |   0.22   |       0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Definimos una función para construir las matrices de transformación\n",
    "# en forma simbóĺica a partir de los parámetros D-H\n",
    "\n",
    "def symTfromDH(theta, d, a, alpha):\n",
    "    # theta y alpha en radianes\n",
    "    # d y a en metros\n",
    "    Rz = sp.Matrix([[sp.cos(theta), -sp.sin(theta), 0, 0],\n",
    "                   [sp.sin(theta), sp.cos(theta), 0, 0],\n",
    "                   [0, 0, 1, 0],\n",
    "                   [0, 0, 0, 1]])\n",
    "    tz = sp.Matrix([[1, 0, 0, 0],\n",
    "                   [0, 1, 0, 0],\n",
    "                   [0, 0, 1, d],\n",
    "                   [0, 0, 0, 1]])\n",
    "    ta = sp.Matrix([[1, 0, 0, a],\n",
    "                   [0, 1, 0, 0],\n",
    "                   [0, 0, 1, 0],\n",
    "                   [0, 0, 0, 1]])\n",
    "    Rx = sp.Matrix([[1, 0, 0, 0],\n",
    "                   [0, sp.cos(alpha), -sp.sin(alpha), 0],\n",
    "                   [0, sp.sin(alpha), sp.cos(alpha), 0],\n",
    "                   [0, 0, 0, 1]])\n",
    "    T = Rz*tz*ta*Rx\n",
    "    return T"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{1} \\right)} & 0 & - \\sin{\\left(q_{1} \\right)} & 0\\\\\\sin{\\left(q_{1} \\right)} & 0 & \\cos{\\left(q_{1} \\right)} & 0\\\\0 & -1 & 0 & 0.13\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q1),  0, -sin(q1),    0],\n",
       "[sin(q1),  0,  cos(q1),    0],\n",
       "[      0, -1,        0, 0.13],\n",
       "[      0,  0,        0,    1]])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q1 = sp.symbols('q1')\n",
    "T01 = symTfromDH(q1, 0.13, 0, -90 * sp.pi/180)\n",
    "T01"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{2} \\right)} & 0 & \\sin{\\left(q_{2} \\right)} & 0.11 \\cos{\\left(q_{2} \\right)}\\\\\\sin{\\left(q_{2} \\right)} & 0 & - \\cos{\\left(q_{2} \\right)} & 0.11 \\sin{\\left(q_{2} \\right)}\\\\0 & 1 & 0 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q2), 0,  sin(q2), 0.11*cos(q2)],\n",
       "[sin(q2), 0, -cos(q2), 0.11*sin(q2)],\n",
       "[      0, 1,        0,            0],\n",
       "[      0, 0,        0,            1]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q2 = sp.symbols('q2')\n",
    "T12 = symTfromDH(q2, 0, 0.11, 90 * sp.pi/180)\n",
    "T12"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{3} \\right)} & 0 & - \\sin{\\left(q_{3} \\right)} & 0.16 \\cos{\\left(q_{3} \\right)}\\\\\\sin{\\left(q_{3} \\right)} & 0 & \\cos{\\left(q_{3} \\right)} & 0.16 \\sin{\\left(q_{3} \\right)}\\\\0 & -1 & 0 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q3),  0, -sin(q3), 0.16*cos(q3)],\n",
       "[sin(q3),  0,  cos(q3), 0.16*sin(q3)],\n",
       "[      0, -1,        0,            0],\n",
       "[      0,  0,        0,            1]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q3 = sp.symbols('q3')\n",
    "T23 = symTfromDH(q3, 0, 0.16, -90 * sp.pi/180)\n",
    "T23"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{4} \\right)} & 0 & \\sin{\\left(q_{4} \\right)} & 0.13 \\cos{\\left(q_{4} \\right)}\\\\\\sin{\\left(q_{4} \\right)} & 0 & - \\cos{\\left(q_{4} \\right)} & 0.13 \\sin{\\left(q_{4} \\right)}\\\\0 & 1 & 0 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q4), 0,  sin(q4), 0.13*cos(q4)],\n",
       "[sin(q4), 0, -cos(q4), 0.13*sin(q4)],\n",
       "[      0, 1,        0,            0],\n",
       "[      0, 0,        0,            1]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q4 = sp.symbols('q4')\n",
    "T34 = symTfromDH(q4, 0, 0.13, 90 * sp.pi/180)\n",
    "T34"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{5} \\right)} & 0 & - \\sin{\\left(q_{5} \\right)} & 0.13 \\cos{\\left(q_{5} \\right)}\\\\\\sin{\\left(q_{5} \\right)} & 0 & \\cos{\\left(q_{5} \\right)} & 0.13 \\sin{\\left(q_{5} \\right)}\\\\0 & -1 & 0 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q5),  0, -sin(q5), 0.13*cos(q5)],\n",
       "[sin(q5),  0,  cos(q5), 0.13*sin(q5)],\n",
       "[      0, -1,        0,            0],\n",
       "[      0,  0,        0,            1]])"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q5 = sp.symbols('q5')\n",
    "T45 = symTfromDH(q5, 0, 0.13, -90 * sp.pi/180)\n",
    "T45"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{6} \\right)} & - \\sin{\\left(q_{6} \\right)} & 0 & 0.18 \\cos{\\left(q_{6} \\right)}\\\\\\sin{\\left(q_{6} \\right)} & \\cos{\\left(q_{6} \\right)} & 0 & 0.18 \\sin{\\left(q_{6} \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q6), -sin(q6), 0, 0.18*cos(q6)],\n",
       "[sin(q6),  cos(q6), 0, 0.18*sin(q6)],\n",
       "[      0,        0, 1,            0],\n",
       "[      0,        0, 0,            1]])"
      ]
     },
     "execution_count": 15,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q6 = sp.symbols('q6')\n",
    "T56 = symTfromDH(q6, 0, 0.18, 0)\n",
    "T56"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(q_{7} \\right)} & - \\sin{\\left(q_{7} \\right)} & 0 & 0.22 \\cos{\\left(q_{7} \\right)}\\\\\\sin{\\left(q_{7} \\right)} & \\cos{\\left(q_{7} \\right)} & 0 & 0.22 \\sin{\\left(q_{7} \\right)}\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(q7), -sin(q7), 0, 0.22*cos(q7)],\n",
       "[sin(q7),  cos(q7), 0, 0.22*sin(q7)],\n",
       "[      0,        0, 1,            0],\n",
       "[      0,        0, 0,            1]])"
      ]
     },
     "execution_count": 16,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "q7 = sp.symbols('q7')\n",
    "T67 = symTfromDH(q7, 0, 0.22, 0)\n",
    "T67"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\left(- \\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} + \\left(\\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} & \\left(- \\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} - \\left(\\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} & - \\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} & 0.22 \\left(- \\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} + 0.22 \\left(\\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} + 0.18 \\left(\\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + 0.18 \\left(- \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + 0.13 \\left(\\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + 0.13 \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} + 0.13 \\left(- \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)} - 0.16 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} - 0.13 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} + 0.16 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + 0.11 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\\\\\left(- \\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} + \\left(\\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} & \\left(- \\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} - \\left(\\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} & - \\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{5} \\right)} & 0.22 \\left(- \\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} + 0.22 \\left(\\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} + 0.18 \\left(\\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + 0.18 \\left(- \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + 0.13 \\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + 0.13 \\left(- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + 0.13 \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} + 0.16 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + 0.11 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} + 0.16 \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\\\\\left(- \\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} + \\left(\\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} & \\left(- \\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} - \\left(\\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} & - \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{5} \\right)} & 0.22 \\left(- \\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\cos{\\left(q_{6} \\right)}\\right) \\sin{\\left(q_{7} \\right)} + 0.22 \\left(\\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)}\\right) \\cos{\\left(q_{7} \\right)} + 0.18 \\left(\\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)}\\right) \\cos{\\left(q_{6} \\right)} + 0.18 \\left(\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{6} \\right)} + 0.13 \\left(- \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} + 0.13 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} - 0.13 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - 0.16 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} - 0.11 \\sin{\\left(q_{2} \\right)} - 0.13 \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} + 0.13\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[(-(((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*sin(q6) + (-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*cos(q6))*sin(q7) + ((((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*cos(q6) + (-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*sin(q6))*cos(q7), (-(((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*sin(q6) + (-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*cos(q6))*cos(q7) - ((((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*cos(q6) + (-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*sin(q6))*sin(q7), -((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*sin(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*cos(q5), 0.22*(-(((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*sin(q6) + (-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*cos(q6))*sin(q7) + 0.22*((((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*cos(q6) + (-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*sin(q6))*cos(q7) + 0.18*(((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + (-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5))*cos(q6) + 0.18*(-(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*sin(q4) - sin(q2)*cos(q1)*cos(q4))*sin(q6) + 0.13*((-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) - sin(q2)*sin(q4)*cos(q1))*cos(q5) + 0.13*(-sin(q1)*sin(q3) + cos(q1)*cos(q2)*cos(q3))*cos(q4) + 0.13*(-sin(q1)*cos(q3) - sin(q3)*cos(q1)*cos(q2))*sin(q5) - 0.16*sin(q1)*sin(q3) - 0.13*sin(q2)*sin(q4)*cos(q1) + 0.16*cos(q1)*cos(q2)*cos(q3) + 0.11*cos(q1)*cos(q2)],\n",
       "[    (-(((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*sin(q6) + (-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*cos(q6))*sin(q7) + ((((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*cos(q6) + (-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*sin(q6))*cos(q7),     (-(((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*sin(q6) + (-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*cos(q6))*cos(q7) - ((((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*cos(q6) + (-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*sin(q6))*sin(q7),  -((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*sin(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*cos(q5),         0.22*(-(((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*sin(q6) + (-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*cos(q6))*sin(q7) + 0.22*((((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*cos(q6) + (-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*sin(q6))*cos(q7) + 0.18*(((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + (-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5))*cos(q6) + 0.18*(-(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*sin(q4) - sin(q1)*sin(q2)*cos(q4))*sin(q6) + 0.13*((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*cos(q5) + 0.13*(-sin(q1)*sin(q3)*cos(q2) + cos(q1)*cos(q3))*sin(q5) + 0.13*(sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - 0.13*sin(q1)*sin(q2)*sin(q4) + 0.16*sin(q1)*cos(q2)*cos(q3) + 0.11*sin(q1)*cos(q2) + 0.16*sin(q3)*cos(q1)],\n",
       "[                                                                                                                                                                                                              (-((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*sin(q6) + (sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*cos(q6))*sin(q7) + (((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*cos(q6) + (sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*sin(q6))*cos(q7),                                                                                                                                                                                                               (-((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*sin(q6) + (sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*cos(q6))*cos(q7) - (((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*cos(q6) + (sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*sin(q6))*sin(q7),                                                                  -(-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*sin(q5) + sin(q2)*sin(q3)*cos(q5),                                                                                                                                                                                                                                                                                                                                                                                                                                                            0.22*(-((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*sin(q6) + (sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*cos(q6))*sin(q7) + 0.22*(((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*cos(q6) + (sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*sin(q6))*cos(q7) + 0.18*((-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + sin(q2)*sin(q3)*sin(q5))*cos(q6) + 0.18*(sin(q2)*sin(q4)*cos(q3) - cos(q2)*cos(q4))*sin(q6) + 0.13*(-sin(q2)*cos(q3)*cos(q4) - sin(q4)*cos(q2))*cos(q5) + 0.13*sin(q2)*sin(q3)*sin(q5) - 0.13*sin(q2)*cos(q3)*cos(q4) - 0.16*sin(q2)*cos(q3) - 0.11*sin(q2) - 0.13*sin(q4)*cos(q2) + 0.13],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  0,                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  1]])"
      ]
     },
     "execution_count": 17,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T0ee = T01 * T12 * T23 * T34 * T45 * T56 * T67 \n",
    "T0ee"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} & \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} & \\left(\\left(\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} - \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\sin{\\left(q_{5} \\right)} - \\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\cos{\\left(q_{5} \\right)} & 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} \\right)} + 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} - 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - 0.16 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} - 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{3} \\right)} - 0.18 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{5} \\right)} - 0.13 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} - 0.18 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} - 0.22 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} - 0.18 \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} - 0.18 \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} - 0.22 \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + 0.18 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} + 0.22 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + 0.13 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} + 0.13 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} + 0.16 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + 0.11 \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)}\\\\- \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} & \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{5} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} - \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} - \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} & - \\left(\\left(\\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)}\\right) \\cos{\\left(q_{4} \\right)} - \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)}\\right) \\sin{\\left(q_{5} \\right)} - \\left(\\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{2} \\right)} - \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\right) \\cos{\\left(q_{5} \\right)} & - 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} - 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{4} \\right)} - 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{4} \\right)} - 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{2} \\right)} - 0.18 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} - 0.22 \\sin{\\left(q_{1} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + 0.18 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} + 0.22 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + 0.13 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} + 0.13 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} + 0.16 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} + 0.11 \\sin{\\left(q_{1} \\right)} \\cos{\\left(q_{2} \\right)} - 0.18 \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{1} \\right)} - 0.22 \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{1} \\right)} + 0.18 \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} + 0.22 \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + 0.13 \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} + 0.13 \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{4} \\right)} + 0.16 \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{1} \\right)} + 0.18 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} \\right)} + 0.22 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + 0.13 \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{1} \\right)} \\cos{\\left(q_{3} \\right)}\\\\\\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{3} \\right)} - \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} & - \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} + \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{5} \\right)} - \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} & \\left(\\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} + \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)}\\right) \\sin{\\left(q_{5} \\right)} + \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\cos{\\left(q_{5} \\right)} & 0.18 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} + 0.22 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} + 0.13 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{3} \\right)} \\sin{\\left(q_{5} \\right)} + 0.18 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{3} \\right)} + 0.22 \\sin{\\left(q_{2} \\right)} \\sin{\\left(q_{4} \\right)} \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{3} \\right)} - 0.18 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} \\cos{\\left(q_{5} \\right)} - 0.13 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} \\cos{\\left(q_{4} \\right)} - 0.16 \\sin{\\left(q_{2} \\right)} \\cos{\\left(q_{3} \\right)} - 0.11 \\sin{\\left(q_{2} \\right)} - 0.18 \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} \\right)} - 0.22 \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{5} \\right)} \\cos{\\left(q_{6} + q_{7} \\right)} - 0.13 \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{5} \\right)} - 0.13 \\sin{\\left(q_{4} \\right)} \\cos{\\left(q_{2} \\right)} - 0.18 \\sin{\\left(q_{6} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} - 0.22 \\sin{\\left(q_{6} + q_{7} \\right)} \\cos{\\left(q_{2} \\right)} \\cos{\\left(q_{4} \\right)} + 0.13\\\\0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ sin(q1)*sin(q3)*sin(q4)*sin(q6 + q7) - sin(q1)*sin(q3)*cos(q4)*cos(q5)*cos(q6 + q7) - sin(q1)*sin(q5)*cos(q3)*cos(q6 + q7) - sin(q2)*sin(q4)*cos(q1)*cos(q5)*cos(q6 + q7) - sin(q2)*sin(q6 + q7)*cos(q1)*cos(q4) - sin(q3)*sin(q5)*cos(q1)*cos(q2)*cos(q6 + q7) - sin(q4)*sin(q6 + q7)*cos(q1)*cos(q2)*cos(q3) + cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6 + q7), sin(q1)*sin(q3)*sin(q4)*cos(q6 + q7) + sin(q1)*sin(q3)*sin(q6 + q7)*cos(q4)*cos(q5) + sin(q1)*sin(q5)*sin(q6 + q7)*cos(q3) + sin(q2)*sin(q4)*sin(q6 + q7)*cos(q1)*cos(q5) - sin(q2)*cos(q1)*cos(q4)*cos(q6 + q7) + sin(q3)*sin(q5)*sin(q6 + q7)*cos(q1)*cos(q2) - sin(q4)*cos(q1)*cos(q2)*cos(q3)*cos(q6 + q7) - sin(q6 + q7)*cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5),  ((sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3))*cos(q4) + sin(q2)*sin(q4)*cos(q1))*sin(q5) - (sin(q1)*cos(q3) + sin(q3)*cos(q1)*cos(q2))*cos(q5),  0.18*sin(q1)*sin(q3)*sin(q4)*sin(q6) + 0.22*sin(q1)*sin(q3)*sin(q4)*sin(q6 + q7) - 0.18*sin(q1)*sin(q3)*cos(q4)*cos(q5)*cos(q6) - 0.22*sin(q1)*sin(q3)*cos(q4)*cos(q5)*cos(q6 + q7) - 0.13*sin(q1)*sin(q3)*cos(q4)*cos(q5) - 0.13*sin(q1)*sin(q3)*cos(q4) - 0.16*sin(q1)*sin(q3) - 0.18*sin(q1)*sin(q5)*cos(q3)*cos(q6) - 0.22*sin(q1)*sin(q5)*cos(q3)*cos(q6 + q7) - 0.13*sin(q1)*sin(q5)*cos(q3) - 0.18*sin(q2)*sin(q4)*cos(q1)*cos(q5)*cos(q6) - 0.22*sin(q2)*sin(q4)*cos(q1)*cos(q5)*cos(q6 + q7) - 0.13*sin(q2)*sin(q4)*cos(q1)*cos(q5) - 0.13*sin(q2)*sin(q4)*cos(q1) - 0.18*sin(q2)*sin(q6)*cos(q1)*cos(q4) - 0.22*sin(q2)*sin(q6 + q7)*cos(q1)*cos(q4) - 0.18*sin(q3)*sin(q5)*cos(q1)*cos(q2)*cos(q6) - 0.22*sin(q3)*sin(q5)*cos(q1)*cos(q2)*cos(q6 + q7) - 0.13*sin(q3)*sin(q5)*cos(q1)*cos(q2) - 0.18*sin(q4)*sin(q6)*cos(q1)*cos(q2)*cos(q3) - 0.22*sin(q4)*sin(q6 + q7)*cos(q1)*cos(q2)*cos(q3) + 0.18*cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6) + 0.22*cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6 + q7) + 0.13*cos(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5) + 0.13*cos(q1)*cos(q2)*cos(q3)*cos(q4) + 0.16*cos(q1)*cos(q2)*cos(q3) + 0.11*cos(q1)*cos(q2)],\n",
       "[-sin(q1)*sin(q2)*sin(q4)*cos(q5)*cos(q6 + q7) - sin(q1)*sin(q2)*sin(q6 + q7)*cos(q4) - sin(q1)*sin(q3)*sin(q5)*cos(q2)*cos(q6 + q7) - sin(q1)*sin(q4)*sin(q6 + q7)*cos(q2)*cos(q3) + sin(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6 + q7) - sin(q3)*sin(q4)*sin(q6 + q7)*cos(q1) + sin(q3)*cos(q1)*cos(q4)*cos(q5)*cos(q6 + q7) + sin(q5)*cos(q1)*cos(q3)*cos(q6 + q7), sin(q1)*sin(q2)*sin(q4)*sin(q6 + q7)*cos(q5) - sin(q1)*sin(q2)*cos(q4)*cos(q6 + q7) + sin(q1)*sin(q3)*sin(q5)*sin(q6 + q7)*cos(q2) - sin(q1)*sin(q4)*cos(q2)*cos(q3)*cos(q6 + q7) - sin(q1)*sin(q6 + q7)*cos(q2)*cos(q3)*cos(q4)*cos(q5) - sin(q3)*sin(q4)*cos(q1)*cos(q6 + q7) - sin(q3)*sin(q6 + q7)*cos(q1)*cos(q4)*cos(q5) - sin(q5)*sin(q6 + q7)*cos(q1)*cos(q3), -((sin(q1)*cos(q2)*cos(q3) + sin(q3)*cos(q1))*cos(q4) - sin(q1)*sin(q2)*sin(q4))*sin(q5) - (sin(q1)*sin(q3)*cos(q2) - cos(q1)*cos(q3))*cos(q5), -0.18*sin(q1)*sin(q2)*sin(q4)*cos(q5)*cos(q6) - 0.22*sin(q1)*sin(q2)*sin(q4)*cos(q5)*cos(q6 + q7) - 0.13*sin(q1)*sin(q2)*sin(q4)*cos(q5) - 0.13*sin(q1)*sin(q2)*sin(q4) - 0.18*sin(q1)*sin(q2)*sin(q6)*cos(q4) - 0.22*sin(q1)*sin(q2)*sin(q6 + q7)*cos(q4) - 0.18*sin(q1)*sin(q3)*sin(q5)*cos(q2)*cos(q6) - 0.22*sin(q1)*sin(q3)*sin(q5)*cos(q2)*cos(q6 + q7) - 0.13*sin(q1)*sin(q3)*sin(q5)*cos(q2) - 0.18*sin(q1)*sin(q4)*sin(q6)*cos(q2)*cos(q3) - 0.22*sin(q1)*sin(q4)*sin(q6 + q7)*cos(q2)*cos(q3) + 0.18*sin(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6) + 0.22*sin(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6 + q7) + 0.13*sin(q1)*cos(q2)*cos(q3)*cos(q4)*cos(q5) + 0.13*sin(q1)*cos(q2)*cos(q3)*cos(q4) + 0.16*sin(q1)*cos(q2)*cos(q3) + 0.11*sin(q1)*cos(q2) - 0.18*sin(q3)*sin(q4)*sin(q6)*cos(q1) - 0.22*sin(q3)*sin(q4)*sin(q6 + q7)*cos(q1) + 0.18*sin(q3)*cos(q1)*cos(q4)*cos(q5)*cos(q6) + 0.22*sin(q3)*cos(q1)*cos(q4)*cos(q5)*cos(q6 + q7) + 0.13*sin(q3)*cos(q1)*cos(q4)*cos(q5) + 0.13*sin(q3)*cos(q1)*cos(q4) + 0.16*sin(q3)*cos(q1) + 0.18*sin(q5)*cos(q1)*cos(q3)*cos(q6) + 0.22*sin(q5)*cos(q1)*cos(q3)*cos(q6 + q7) + 0.13*sin(q5)*cos(q1)*cos(q3)],\n",
       "[                                                                                                                                                                      sin(q2)*sin(q3)*sin(q5)*cos(q6 + q7) + sin(q2)*sin(q4)*sin(q6 + q7)*cos(q3) - sin(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6 + q7) - sin(q4)*cos(q2)*cos(q5)*cos(q6 + q7) - sin(q6 + q7)*cos(q2)*cos(q4),                                                                                                                                                                     -sin(q2)*sin(q3)*sin(q5)*sin(q6 + q7) + sin(q2)*sin(q4)*cos(q3)*cos(q6 + q7) + sin(q2)*sin(q6 + q7)*cos(q3)*cos(q4)*cos(q5) + sin(q4)*sin(q6 + q7)*cos(q2)*cos(q5) - cos(q2)*cos(q4)*cos(q6 + q7),                                                                  (sin(q2)*cos(q3)*cos(q4) + sin(q4)*cos(q2))*sin(q5) + sin(q2)*sin(q3)*cos(q5),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0.18*sin(q2)*sin(q3)*sin(q5)*cos(q6) + 0.22*sin(q2)*sin(q3)*sin(q5)*cos(q6 + q7) + 0.13*sin(q2)*sin(q3)*sin(q5) + 0.18*sin(q2)*sin(q4)*sin(q6)*cos(q3) + 0.22*sin(q2)*sin(q4)*sin(q6 + q7)*cos(q3) - 0.18*sin(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6) - 0.22*sin(q2)*cos(q3)*cos(q4)*cos(q5)*cos(q6 + q7) - 0.13*sin(q2)*cos(q3)*cos(q4)*cos(q5) - 0.13*sin(q2)*cos(q3)*cos(q4) - 0.16*sin(q2)*cos(q3) - 0.11*sin(q2) - 0.18*sin(q4)*cos(q2)*cos(q5)*cos(q6) - 0.22*sin(q4)*cos(q2)*cos(q5)*cos(q6 + q7) - 0.13*sin(q4)*cos(q2)*cos(q5) - 0.13*sin(q4)*cos(q2) - 0.18*sin(q6)*cos(q2)*cos(q4) - 0.22*sin(q6 + q7)*cos(q2)*cos(q4) + 0.13],\n",
       "[                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                                                                                                                                                                                                                                                     0,                                                                                                                                              0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           1]])"
      ]
     },
     "execution_count": 18,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = sp.simplify (T0ee)\n",
    "T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Calculamos las expresiones de cinemática inversa ###\n",
    "\n",
    "La matriz T contiene toda la información necesaria para calcular la cinemática inversa. En esta ocasión nos preocuparemos únicamente de las coordenadas de posición."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Definimos un punto de destino\n",
    "x = 2\n",
    "y = 2\n",
    "z = 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'T' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-6-7158614e0d46>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m\u001b[0m\n\u001b[0;32m      1\u001b[0m \u001b[1;31m# preparamos las ecuaciones transformando las expresiones\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      2\u001b[0m \u001b[1;31m# de la forma <expresion = valor> a la forma <expresion - valor> = 0\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 3\u001b[1;33m \u001b[0meq1\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mT\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m3\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;33m-\u001b[0m \u001b[0mx\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m      4\u001b[0m \u001b[0meq2\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mT\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m7\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;33m-\u001b[0m \u001b[0my\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      5\u001b[0m \u001b[0meq3\u001b[0m \u001b[1;33m=\u001b[0m \u001b[0mT\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m11\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;33m-\u001b[0m \u001b[0mz\u001b[0m\u001b[1;33m\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mNameError\u001b[0m: name 'T' is not defined"
     ]
    }
   ],
   "source": [
    "# preparamos las ecuaciones transformando las expresiones\n",
    "# de la forma <expresion = valor> a la forma <expresion - valor> = 0\n",
    "eq1 = T[3] - x\n",
    "eq2 = T[7] - y\n",
    "eq3 = T[11] - z\n",
    "(eq1,eq2,eq3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "#n = sp.nsolve((eq1, eq2, eq3), (q5, q6, q7), (1, 1, 1))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "conectado a 19999\n"
     ]
    }
   ],
   "source": [
    "# enviamos los ángulos a las articulaciones\n",
    "clientID = connect(19999)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint1, x, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint2, y, sim.simx_opmode_oneshot)\n",
    "retCode = sim.simxSetJointTargetPosition(clientID, joint3, z, sim.simx_opmode_oneshot)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[0.7522956132888794, 1.794745922088623, 1.1224974393844604]\n"
     ]
    }
   ],
   "source": [
    "# y verificamos la posición del actuador\n",
    "retCode,pos=sim.simxGetObjectPosition(clientID, tip, -1, sim.simx_opmode_blocking)\n",
    "print(pos)"
   ]
  },
  {
   "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}