RobotArm.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from mpmath import *\n",
    "from sympy import *\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[cos(p)*cos(y), sin(p)*sin(r)*cos(y) - sin(y)*cos(r), sin(p)*cos(r)*cos(y) + sin(r)*sin(y)],\n",
      "[sin(y)*cos(p), sin(p)*sin(r)*sin(y) + cos(r)*cos(y), sin(p)*sin(y)*cos(r) - sin(r)*cos(y)],\n",
      "[      -sin(p),                        sin(r)*cos(p),                        cos(p)*cos(r)]])\n"
     ]
    }
   ],
   "source": [
    "r, p, y = symbols(['r','p','y'])\n",
    "\n",
    "R_roll = Matrix([[ 1,      0,      0],\n",
    "                 [ 0, cos(r),-sin(r)],\n",
    "                 [ 0, sin(r), cos(r)]])\n",
    "\n",
    "R_pitch = Matrix([[ cos(p), 0,  sin(p)],\n",
    "                  [      0, 1,       0],\n",
    "                  [-sin(p), 0,  cos(p)]])\n",
    "\n",
    "R_yaw = Matrix([[ cos(y), -sin(y), 0],\n",
    "                [ sin(y),  cos(y), 0],\n",
    "                [      0,       0, 1]])\n",
    "\n",
    "R0_6 = simplify(R_yaw * R_pitch * R_roll)\n",
    "# R0_6 = simplify(R_roll * R_pitch * R_yaw)\n",
    "\n",
    "print(R0_6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[0,  0, 1],\n",
      "[0, -1, 0],\n",
      "[1,  0, 0]])\n"
     ]
    }
   ],
   "source": [
    "R_z = Matrix([[ cos(pi),-sin(pi), 0],\n",
    "              [ sin(pi), cos(pi), 0],\n",
    "              [       0,       0, 1]])\n",
    "\n",
    "R_y = Matrix([[ cos(-pi/2), 0, sin(-pi/2)],\n",
    "              [          0, 1,          0],\n",
    "              [-sin(-pi/2), 0, cos(-pi/2)]])\n",
    "\n",
    "R_corr = simplify(R_z * R_y)\n",
    "\n",
    "print(R_corr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[sin(p)*cos(r)*cos(y) + sin(r)*sin(y), -sin(p)*sin(r)*cos(y) + sin(y)*cos(r), cos(p)*cos(y)],\n",
      "[sin(p)*sin(y)*cos(r) - sin(r)*cos(y), -sin(p)*sin(r)*sin(y) - cos(r)*cos(y), sin(y)*cos(p)],\n",
      "[                       cos(p)*cos(r),                        -sin(r)*cos(p),       -sin(p)]])\n"
     ]
    }
   ],
   "source": [
    "R0_6_corr = R0_6 * R_corr\n",
    "print(R0_6_corr)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)], [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3), cos(q1)], [cos(q2 + q3), -sin(q2 + q3), 0]])\n"
     ]
    }
   ],
   "source": [
    "q1, q2, q3 = symbols('q1:4')\n",
    "\n",
    "R0_3 = Matrix([[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],\n",
    "               [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],\n",
    "               [        cos(q2 + q3),        -sin(q2 + q3),        0]])\n",
    "\n",
    "print(R0_3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# print(R0_3) =>\n",
    " \n",
    "# Matrix([[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)], \n",
    "#         [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)], \n",
    "#         [cos(q2 + q3),                -sin(q2 + q3),        0]])\n",
    "\n",
    "\n",
    "# print(R0_3.T) =>\n",
    "\n",
    "# Matrix([[sin(q2 + q3)*cos(q1), sin(q1)*sin(q2 + q3),  cos(q2 + q3)], \n",
    "#         [cos(q1)*cos(q2 + q3), sin(q1)*cos(q2 + q3), -sin(q2 + q3)],\n",
    "#         [            -sin(q1),              cos(q1),             0]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[(sin(p)*sin(y)*cos(r) - sin(r)*cos(y))*sin(q1)*sin(q2 + q3) + (sin(p)*cos(r)*cos(y) + sin(r)*sin(y))*sin(q2 + q3)*cos(q1) + cos(p)*cos(r)*cos(q2 + q3), (-sin(p)*sin(r)*sin(y) - cos(r)*cos(y))*sin(q1)*sin(q2 + q3) + (-sin(p)*sin(r)*cos(y) + sin(y)*cos(r))*sin(q2 + q3)*cos(q1) - sin(r)*cos(p)*cos(q2 + q3), -sin(p)*cos(q2 + q3) + sin(q1)*sin(y)*sin(q2 + q3)*cos(p) + sin(q2 + q3)*cos(p)*cos(q1)*cos(y)],\n",
      "[(sin(p)*sin(y)*cos(r) - sin(r)*cos(y))*sin(q1)*cos(q2 + q3) + (sin(p)*cos(r)*cos(y) + sin(r)*sin(y))*cos(q1)*cos(q2 + q3) - sin(q2 + q3)*cos(p)*cos(r), (-sin(p)*sin(r)*sin(y) - cos(r)*cos(y))*sin(q1)*cos(q2 + q3) + (-sin(p)*sin(r)*cos(y) + sin(y)*cos(r))*cos(q1)*cos(q2 + q3) + sin(r)*sin(q2 + q3)*cos(p),  sin(p)*sin(q2 + q3) + sin(q1)*sin(y)*cos(p)*cos(q2 + q3) + cos(p)*cos(q1)*cos(y)*cos(q2 + q3)],\n",
      "[                                                       (sin(p)*sin(y)*cos(r) - sin(r)*cos(y))*cos(q1) - (sin(p)*cos(r)*cos(y) + sin(r)*sin(y))*sin(q1),                                                        (-sin(p)*sin(r)*sin(y) - cos(r)*cos(y))*cos(q1) - (-sin(p)*sin(r)*cos(y) + sin(y)*cos(r))*sin(q1),                                                 -sin(q1)*cos(p)*cos(y) + sin(y)*cos(p)*cos(q1)]])\n"
     ]
    }
   ],
   "source": [
    "R3_6 = R0_3.T * R0_6_corr\n",
    "print(R3_6)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "q1, q2, q3, q4, q5, q6, q7 = symbols('q1:8')\n",
    "d1, d2, d3, d4, d5, d6, d7 = symbols('d1:8')\n",
    "a0, a1, a2, a3, a4, a5, a6 = symbols('a0:7')\n",
    "alpha0, alpha1, alpha2, alpha3, alpha4, alpha5, alpha6 = symbols('alpha0:7')\n",
    "\n",
    "# DH Parameters\n",
    "s = {alpha0:     0, a0:      0, d1:  0.75, \n",
    "     alpha1: -pi/2, a1:   0.35, d2:     0, q2: q2-pi/2,  \n",
    "     alpha2:     0, a2:   1.25, d3:     0,\n",
    "     alpha3: -pi/2, a3: -0.054, d4:  1.50,\n",
    "     alpha4:  pi/2, a4:      0, d5:     0,\n",
    "     alpha5: -pi/2, a5:      0, d6:     0,\n",
    "     alpha6:     0, a6:      0, d7: 0.303, q7: 0\n",
    "     }\n",
    "\n",
    "#### Homogeneous Transforms\n",
    "T0_1 = Matrix([[             cos(q1),            -sin(q1),            0,              a0],\n",
    "               [ sin(q1)*cos(alpha0), cos(q1)*cos(alpha0), -sin(alpha0), -sin(alpha0)*d1],\n",
    "               [ sin(q1)*sin(alpha0), cos(q1)*sin(alpha0),  cos(alpha0),  cos(alpha0)*d1],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T0_1 = T0_1.subs(s)\n",
    "\n",
    "T1_2 = Matrix([[             cos(q2),            -sin(q2),            0,              a1],\n",
    "               [ sin(q2)*cos(alpha1), cos(q2)*cos(alpha1), -sin(alpha1), -sin(alpha1)*d2],\n",
    "               [ sin(q2)*sin(alpha1), cos(q2)*sin(alpha1),  cos(alpha1),  cos(alpha1)*d2],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T1_2 = T1_2.subs(s)\n",
    "\n",
    "T2_3 = Matrix([[             cos(q3),            -sin(q3),            0,              a2],\n",
    "               [ sin(q3)*cos(alpha2), cos(q3)*cos(alpha2), -sin(alpha2), -sin(alpha2)*d3],\n",
    "               [ sin(q3)*sin(alpha2), cos(q3)*sin(alpha2),  cos(alpha2),  cos(alpha2)*d3],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T2_3 = T2_3.subs(s)\n",
    "\n",
    "T3_4 = Matrix([[             cos(q4),            -sin(q4),            0,              a3],\n",
    "               [ sin(q4)*cos(alpha3), cos(q4)*cos(alpha3), -sin(alpha3), -sin(alpha3)*d4],\n",
    "               [ sin(q4)*sin(alpha3), cos(q4)*sin(alpha3),  cos(alpha3),  cos(alpha3)*d4],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T3_4 = T3_4.subs(s)\n",
    "\n",
    "T4_5 = Matrix([[             cos(q5),            -sin(q5),            0,              a4],\n",
    "               [ sin(q5)*cos(alpha4), cos(q5)*cos(alpha4), -sin(alpha4), -sin(alpha4)*d5],\n",
    "               [ sin(q5)*sin(alpha4), cos(q5)*sin(alpha4),  cos(alpha4),  cos(alpha4)*d5],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T4_5 = T4_5.subs(s)\n",
    "\n",
    "T5_6 = Matrix([[             cos(q6),            -sin(q6),            0,              a5],\n",
    "               [ sin(q6)*cos(alpha5), cos(q6)*cos(alpha5), -sin(alpha5), -sin(alpha5)*d6],\n",
    "               [ sin(q6)*sin(alpha5), cos(q6)*sin(alpha5),  cos(alpha5),  cos(alpha5)*d6],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T5_6 = T5_6.subs(s)\n",
    "\n",
    "T6_G = Matrix([[             cos(q7),            -sin(q7),            0,              a6],\n",
    "               [ sin(q7)*cos(alpha6), cos(q7)*cos(alpha6), -sin(alpha6), -sin(alpha6)*d7],\n",
    "               [ sin(q7)*sin(alpha6), cos(q7)*sin(alpha6),  cos(alpha6),  cos(alpha6)*d7],\n",
    "               [                   0,                   0,            0,               1]])\n",
    "T6_G = T6_G.subs(s)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "T0_2 = simplify(T0_1 * T1_2)\n",
    "T0_3 = simplify(T0_2 * T2_3)\n",
    "T0_4 = simplify(T0_3 * T3_4)\n",
    "T0_5 = simplify(T0_4 * T4_5)\n",
    "T0_6 = simplify(T0_5 * T5_6)\n",
    "T0_G = simplify(T0_6 * T6_G)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([[cos(q1), -sin(q1), 0, 0], [sin(q1), cos(q1), 0, 0], [0, 0, 1, 0.750000000000000], [0, 0, 0, 1]])\n",
      "Matrix([\n",
      "[sin(q2)*cos(q1), cos(q1)*cos(q2), -sin(q1), 0.35*cos(q1)],\n",
      "[sin(q1)*sin(q2), sin(q1)*cos(q2),  cos(q1), 0.35*sin(q1)],\n",
      "[        cos(q2),        -sin(q2),        0,         0.75],\n",
      "[              0,               0,        0,            1]])\n",
      "Matrix([\n",
      "[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1), (1.25*sin(q2) + 0.35)*cos(q1)],\n",
      "[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1), (1.25*sin(q2) + 0.35)*sin(q1)],\n",
      "[        cos(q2 + q3),        -sin(q2 + q3),        0,           1.25*cos(q2) + 0.75],\n",
      "[                   0,                    0,        0,                             1]])\n",
      "Matrix([\n",
      "[sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4),  sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1)],\n",
      "[sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1), -sin(q1)*sin(q4)*sin(q2 + q3) - cos(q1)*cos(q4), sin(q1)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1)],\n",
      "[                          cos(q4)*cos(q2 + q3),                           -sin(q4)*cos(q2 + q3),        -sin(q2 + q3),          -1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
      "[                                             0,                                               0,                    0,                                                                     1]])\n",
      "Matrix([\n",
      "[(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -sin(q1)*cos(q4) + sin(q4)*sin(q2 + q3)*cos(q1), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1)],\n",
      "[(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3),  sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1)],\n",
      "[                                   -sin(q5)*sin(q2 + q3) + cos(q4)*cos(q5)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                            sin(q4)*cos(q2 + q3),          -1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
      "[                                                                                      0,                                                                                        0,                                               0,                                                                     1]])\n",
      "Matrix([\n",
      "[((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1)],\n",
      "[((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1)],\n",
      "[                                                               -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),          -1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
      "[                                                                                                                                                           0,                                                                                                                                                             0,                                                                                        0,                                                                     1]])\n",
      "Matrix([\n",
      "[((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -0.303*(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3)],\n",
      "[((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3), -0.303*(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3)],\n",
      "[                                                               -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                                               -0.303*sin(q5)*cos(q4)*cos(q2 + q3) - 0.303*sin(q2 + q3)*cos(q5) - 1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
      "[                                                                                                                                                           0,                                                                                                                                                             0,                                                                                        0,                                                                                                                                                                            1]])\n"
     ]
    }
   ],
   "source": [
    "print(T0_1)\n",
    "print(T0_2)\n",
    "print(T0_3)\n",
    "print(T0_4)\n",
    "print(T0_5)\n",
    "print(T0_6)\n",
    "print(T0_G)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Matrix([[cos(q1), -sin(q1), 0, 0], [sin(q1), cos(q1), 0, 0], [0, 0, 1, 0.750000000000000], [0, 0, 0, 1]])\n",
    "\n",
    "# Matrix([\n",
    "# [sin(q2)*cos(q1), cos(q1)*cos(q2), -sin(q1), 0.35*cos(q1)],\n",
    "# [sin(q1)*sin(q2), sin(q1)*cos(q2),  cos(q1), 0.35*sin(q1)],\n",
    "# [        cos(q2),        -sin(q2),        0,         0.75],\n",
    "# [              0,               0,        0,            1]])\n",
    "\n",
    "# Matrix([\n",
    "# [sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1), (1.25*sin(q2) + 0.35)*cos(q1)],\n",
    "# [sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1), (1.25*sin(q2) + 0.35)*sin(q1)],\n",
    "# [        cos(q2 + q3),        -sin(q2 + q3),        0,           1.25*cos(q2) + 0.75],\n",
    "# [                   0,                    0,        0,                             1]])\n",
    "\n",
    "# Matrix([\n",
    "# [sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4),  sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1)],\n",
    "# [sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1), -sin(q1)*sin(q4)*sin(q2 + q3) - cos(q1)*cos(q4), sin(q1)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1)],\n",
    "# [                          cos(q4)*cos(q2 + q3),                           -sin(q4)*cos(q2 + q3),        -sin(q2 + q3),          -1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
    "# [                                             0,                                               0,                    0,                                                                     1]])\n",
    "\n",
    "# Matrix([\n",
    "# [(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -sin(q1)*cos(q4) + sin(q4)*sin(q2 + q3)*cos(q1), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1)],\n",
    "# [(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3),  sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1)],\n",
    "# [                                   -sin(q5)*sin(q2 + q3) + cos(q4)*cos(q5)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                            sin(q4)*cos(q2 + q3),          -1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
    "# [                                                                                      0,                                                                                        0,                                               0,                                                                     1]])\n",
    "\n",
    "# Matrix([\n",
    "# [((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1)],\n",
    "# [((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3), (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1)],\n",
    "# [                                                               -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),          -1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
    "# [                                                                                                                                                           0,                                                                                                                                                             0,                                                                                        0,                                                                     1]])\n",
    "\n",
    "# Matrix([\n",
    "# [((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*cos(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*sin(q6), -((sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*cos(q5) + sin(q5)*cos(q1)*cos(q2 + q3))*sin(q6) + (sin(q1)*cos(q4) - sin(q4)*sin(q2 + q3)*cos(q1))*cos(q6), -(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + cos(q1)*cos(q5)*cos(q2 + q3), -0.303*(sin(q1)*sin(q4) + sin(q2 + q3)*cos(q1)*cos(q4))*sin(q5) + (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3)],\n",
    "# [((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*cos(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*sin(q6), -((sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*cos(q5) + sin(q1)*sin(q5)*cos(q2 + q3))*sin(q6) - (sin(q1)*sin(q4)*sin(q2 + q3) + cos(q1)*cos(q4))*cos(q6), -(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + sin(q1)*cos(q5)*cos(q2 + q3), -0.303*(sin(q1)*sin(q2 + q3)*cos(q4) - sin(q4)*cos(q1))*sin(q5) + (1.25*sin(q2) - 0.054*sin(q2 + q3) + 1.5*cos(q2 + q3) + 0.35)*sin(q1) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3)],\n",
    "# [                                                               -(sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*cos(q6) - sin(q4)*sin(q6)*cos(q2 + q3),                                                                  (sin(q5)*sin(q2 + q3) - cos(q4)*cos(q5)*cos(q2 + q3))*sin(q6) - sin(q4)*cos(q6)*cos(q2 + q3),                                     -sin(q5)*cos(q4)*cos(q2 + q3) - sin(q2 + q3)*cos(q5),                                               -0.303*sin(q5)*cos(q4)*cos(q2 + q3) - 0.303*sin(q2 + q3)*cos(q5) - 1.5*sin(q2 + q3) + 1.25*cos(q2) - 0.054*cos(q2 + q3) + 0.75],\n",
    "# [                                                                                                                                                           0,                                                                                                                                                             0,                                                                                        0,                                                                                                                                                                            1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), sin(q4 - q5)/2 - sin(q4 + q5)/2],\n",
      "[                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),                         cos(q5)],\n",
      "[-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),                 sin(q4)*sin(q5)]])\n"
     ]
    }
   ],
   "source": [
    "R3_6_sym = simplify(T0_3.T * T0_6)[:3,:3]\n",
    "print(R3_6_sym)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],\n",
      "[                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),          cos(q5)],\n",
      "[-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),  sin(q4)*sin(q5)]])\n"
     ]
    }
   ],
   "source": [
    "R3_6_sym_2 = simplify((T0_3 ** -1) * T0_6)[:3,:3]\n",
    "print(R3_6_sym_2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([\n",
      "[-sin(q5)*cos(q4),  sin(q4)*cos(q6) + sin(q6)*cos(q4)*cos(q5), -sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6)],\n",
      "[         cos(q5),                            sin(q5)*sin(q6),                            sin(q5)*cos(q6)],\n",
      "[ sin(q4)*sin(q5), -sin(q4)*sin(q6)*cos(q5) + cos(q4)*cos(q6), -sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4)]])\n"
     ]
    }
   ],
   "source": [
    "T0_6_corr = T0_6[:3,:3] * R_corr\n",
    "R3_6_sym_3 = simplify((T0_3[:3,:3] ** -1) * T0_6_corr)\n",
    "print(R3_6_sym_3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "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.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}