notebooks/joint_angles_ik_optimized.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "from sympy import symbols, cos, sin, pi, simplify, pprint, tan, expand_trig, sqrt, trigsimp, atan2\n",
    "from sympy.matrices import Matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "q1, q2, q3, q4, q5, q6= symbols('q1:7')\n",
    "alpha, beta, gamma = symbols('alpha beta gamma', real = True)\n",
    "px, py, pz = symbols('px py pz', real = True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "R03 = Matrix([\n",
    "[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",
    "R03T = Matrix([\n",
    "[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": 4,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "R36 = 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)]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "R0u = Matrix([\n",
    "[1.0*cos(alpha)*cos(beta), -1.0*sin(alpha)*cos(gamma) + sin(beta)*sin(gamma)*cos(alpha), 1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma)],\n",
    "[1.0*sin(alpha)*cos(beta),  sin(alpha)*sin(beta)*sin(gamma) + 1.0*cos(alpha)*cos(gamma), sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha)],\n",
    "[          -1.0*sin(beta),                                     1.0*sin(gamma)*cos(beta),                                    1.0*cos(beta)*cos(gamma)]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Rgu_eval = Matrix([[0, 0, 1], [0, -1.00000000000000, 0], [1.00000000000000, 0, 0]])\n",
    "RguT_eval = Matrix([[0, 0, 1], [0, -1.00000000000000, 0], [1.00000000000000, 0, 0]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Total transform wrt gripper given\n",
    "# yaw (alpha), pitch (beta), roll (beta)\n",
    "# position px, py, pz\n",
    "\n",
    "T0g_b = Matrix([\n",
    "[1.0*sin(alpha)*sin(gamma) + sin(beta)*cos(alpha)*cos(gamma),  1.0*sin(alpha)*cos(gamma) - 1.0*sin(beta)*sin(gamma)*cos(alpha), 1.0*cos(alpha)*cos(beta), px],\n",
    "[sin(alpha)*sin(beta)*cos(gamma) - 1.0*sin(gamma)*cos(alpha), -1.0*sin(alpha)*sin(beta)*sin(gamma) - 1.0*cos(alpha)*cos(gamma), 1.0*sin(alpha)*cos(beta), py],\n",
    "[                                   1.0*cos(beta)*cos(gamma),                                        -1.0*sin(gamma)*cos(beta),           -1.0*sin(beta), pz],\n",
    "[                                                          0,                                                                0,                        0,  1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Total transform wrt gripper given \n",
    "# angles q1, q2, q3, q4, q5, q6\n",
    "\n",
    "T0g_a = 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(q5) + 1.25*sin(q2)*cos(q1) - 0.303*sin(q5)*sin(q2 + q3)*cos(q1)*cos(q4) - 0.054*sin(q2 + q3)*cos(q1) + 0.303*cos(q1)*cos(q5)*cos(q2 + q3) + 1.5*cos(q1)*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(q1)*sin(q2) - 0.303*sin(q1)*sin(q5)*sin(q2 + q3)*cos(q4) - 0.054*sin(q1)*sin(q2 + q3) + 0.303*sin(q1)*cos(q5)*cos(q2 + q3) + 1.5*sin(q1)*cos(q2 + q3) + 0.35*sin(q1) + 0.303*sin(q4)*sin(q5)*cos(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),                                                                                 -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": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_hypotenuse(a, b):\n",
    "  return sqrt(a*a + b*b)\n",
    "\n",
    "def get_cosine_law_angle(a, b, c):    \n",
    "  cos_gamma = (a*a + b*b - c*c) / (2*a*b)\n",
    "  sin_gamma = sqrt(1 - cos_gamma * cos_gamma)\n",
    "  gamma = atan2(sin_gamma, cos_gamma)\n",
    "\n",
    "  return gamma\n",
    "\n",
    "def get_wrist_center(gripper_point, R0g, dg = 0.303):\n",
    "\n",
    "  xu, yu, zu = gripper_point \n",
    "    \n",
    "  nx, ny, nz = R0g[0, 2], R0g[1, 2], R0g[2, 2]\n",
    "  xw = xu - dg * nx\n",
    "  yw = yu - dg * ny\n",
    "  zw = zu - dg * nz \n",
    "\n",
    "  return xw, yw, zw\n",
    "def get_first_three_angles(wrist_center):\n",
    "    \n",
    "  x, y, z  = wrist_center\n",
    "    \n",
    "  a1, a2, a3 = 0.35, 1.25, -0.054\n",
    "  d1, d4 = 0.75, 1.5\n",
    "  l = 1.50097168527591\n",
    "  phi = 1.53481186671284\n",
    "  \n",
    "  x_prime = get_hypotenuse(x, y)\n",
    "  mx = x_prime -  a1\n",
    "  my = z - d1 \n",
    "  m = get_hypotenuse(mx, my)\n",
    "  alpha = atan2(my, mx)\n",
    "  \n",
    "  gamma = get_cosine_law_angle(l, a2, m)\n",
    "  beta = get_cosine_law_angle(m, a2, l)\n",
    "  \n",
    "  q1 = atan2(y, x)\n",
    "  q2 = pi/2 - beta - alpha \n",
    "  q3 = -(gamma - phi)\n",
    "    \n",
    "  return q1, q2, q3 \n",
    "\n",
    "def get_last_three_angles(R):\n",
    "    \n",
    "  sin_q4 = R[2, 2]\n",
    "  cos_q4 =  -R[0, 2]\n",
    "    \n",
    "  sin_q5 = sqrt(R[0, 2]**2 + R[2, 2]**2) \n",
    "  cos_q5 = R[1, 2]\n",
    "    \n",
    "  sin_q6 = -R[1, 1]\n",
    "  cos_q6 = R[1, 0] \n",
    "  \n",
    "  q4 = atan2(sin_q4, cos_q4)\n",
    "  q5 = atan2(sin_q5, cos_q5)\n",
    "  q6 = atan2(sin_q6, cos_q6)\n",
    "    \n",
    "  return q4, q5, q6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "def ik(x, y, z, roll, pitch, yaw, debug = False):\n",
    "    \n",
    "  # input: given position and orientation of the gripper wrt to URDFrame\n",
    "  # output: angles q1, q2, q3, q4, q5, q6\n",
    "    \n",
    "  gripper_point = x, y, z\n",
    "\n",
    "  R0u_eval = R0u.evalf(subs = {alpha: yaw, beta: pitch, gamma: roll})\n",
    "  R0g_eval = R0u_eval * RguT_eval\n",
    "\n",
    "  wrist_center = get_wrist_center(gripper_point, R0g_eval, dg = 0.303)\n",
    "\n",
    "  j1, j2, j3 = get_first_three_angles(wrist_center)\n",
    "\n",
    "  R03T_eval = R03T.evalf(subs = {q1: j1.evalf(), q2: j2.evalf(), q3: j3.evalf()})\n",
    "  R36_eval = R03T_eval * R0g_eval\n",
    "\n",
    "  j4, j5, j6 = get_last_three_angles(R36_eval)\n",
    "\n",
    "  j1 = j1.evalf()\n",
    "  j2 = j2.evalf()\n",
    "  j3 = j3.evalf()\n",
    "  j4 = j4.evalf()\n",
    "  j5 = j5.evalf()\n",
    "  j6 = j6.evalf()\n",
    "\n",
    "  if debug:\n",
    "  \n",
    "    print()\n",
    "    print(\"\\n x:\", x, \"\\n y:\", y, \"\\n z:\", z)\n",
    "    print(\"\\n roll:\", roll, \"\\n pitch:\", pitch, \"\\n yaw:\", yaw)\n",
    "        \n",
    "    print()\n",
    "    print(\" j1:\", j1, \"\\n j2:\", j2, \"\\n j3:\", j3)\n",
    "    print(\" j4:\", j4, \"\\n j5:\", j5, \"\\n j6:\", j6)\n",
    "    \n",
    "    print()\n",
    "    print(\"wrist_center\", wrist_center)\n",
    "\n",
    "    print()\n",
    "    print(\"evaluated R0g:\")\n",
    "    pprint(R0g_eval)\n",
    "\n",
    "    print()\n",
    "    print(\"Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz\")\n",
    "    pprint(T0g_b.evalf(subs = {\n",
    "      gamma: roll, beta: pitch, alpha: yaw, px: x, py: y, pz: z\n",
    "    }))\n",
    "\n",
    "    print()\n",
    "    print(\"Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6\")\n",
    "    pprint(T0g_a.evalf(subs = {\n",
    "      q1: j1, q2: j2, q3: j3, q4: j4, q5: j5, q6: j6    \n",
    "    }))\n",
    "    \n",
    "  return j1, j2, j3, j4, j5, j6"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "\n",
      " x: 0.49792 \n",
      " y: 1.3673 \n",
      " z: 2.4988\n",
      "\n",
      " roll: 0.366 \n",
      " pitch: -0.078 \n",
      " yaw: 2.561\n",
      "\n",
      " j1: 1.01249809363771 \n",
      " j2: -0.275800363737724 \n",
      " j3: -0.115686651053751\n",
      " j4: 1.63446527240323 \n",
      " j5: 1.52050002599430 \n",
      " j6: -0.815781306199679\n",
      "\n",
      "wrist_center (0.750499428337951, 1.20160389781975, 2.47518995758694)\n",
      "\n",
      "evaluated R0g:\n",
      "⎡0.257143295038827   0.48887208255965   -0.833595473062543⎤\n",
      "⎢                                                         ⎥\n",
      "⎢0.259329420712765  0.796053601157403    0.54685182237706 ⎥\n",
      "⎢                                                         ⎥\n",
      "⎣0.93092726749696   -0.356795110642117  0.0779209320563015⎦\n",
      "\n",
      "Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz\n",
      "⎡0.257143295038827   0.48887208255965   -0.833595473062543  0.49792⎤\n",
      "⎢                                                                  ⎥\n",
      "⎢0.259329420712765  0.796053601157403    0.54685182237706   1.3673 ⎥\n",
      "⎢                                                                  ⎥\n",
      "⎢0.93092726749696   -0.356795110642117  0.0779209320563015  2.4988 ⎥\n",
      "⎢                                                                  ⎥\n",
      "⎣        0                  0                   0             1.0  ⎦\n",
      "\n",
      "Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6\n",
      "⎡0.257143295038827   0.48887208255965   -0.833595473062543  0.497919999999998⎤\n",
      "⎢                                                                            ⎥\n",
      "⎢0.259329420712765  0.796053601157403    0.54685182237706        1.3673      ⎥\n",
      "⎢                                                                            ⎥\n",
      "⎢0.93092726749696   -0.356795110642117  0.0779209320563016  2.49880000000001 ⎥\n",
      "⎢                                                                            ⎥\n",
      "⎣        0                  0                   0                  1.0       ⎦\n"
     ]
    }
   ],
   "source": [
    "qs = ik(x = 0.49792, y = 1.3673, z = 2.4988,\n",
    "        roll = 0.366, pitch = -0.078, yaw = 2.561, debug = True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "\n",
      " x: 2.3537 \n",
      " y: -0.1255546 \n",
      " z: 2.841452\n",
      "\n",
      " roll: 0.131008 \n",
      " pitch: -0.10541 \n",
      " yaw: 0.0491503\n",
      "\n",
      " j1: -0.0682697289101386 \n",
      " j2: 0.434273483083027 \n",
      " j3: -1.13476160607020\n",
      " j4: 0.206486955261342 \n",
      " j5: 0.604353673052791 \n",
      " j6: -0.0272724984420472\n",
      "\n",
      "wrist_center (2.05274568076731, -0.140358517861560, 2.80957188470638)\n",
      "\n",
      "evaluated R0g:\n",
      "⎡-0.0977692193362551  0.0624374999845176  0.993248578325719 ⎤\n",
      "⎢                                                           ⎥\n",
      "⎢-0.135600778861087   -0.989558155295888  0.0488578147246194⎥\n",
      "⎢                                                           ⎥\n",
      "⎣ 0.985927790724374   -0.129908490419534  0.105214901959143 ⎦\n",
      "\n",
      "Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz\n",
      "⎡-0.0977692193362551  0.0624374999845176  0.993248578325719     2.3537  ⎤\n",
      "⎢                                                                       ⎥\n",
      "⎢-0.135600778861087   -0.989558155295888  0.0488578147246194  -0.1255546⎥\n",
      "⎢                                                                       ⎥\n",
      "⎢ 0.985927790724374   -0.129908490419534  0.105214901959143    2.841452 ⎥\n",
      "⎢                                                                       ⎥\n",
      "⎣         0                   0                   0              1.0    ⎦\n",
      "\n",
      "Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6\n",
      "⎡-0.0977692193362551  0.0624374999845176  0.993248578325719   2.35369999999999\n",
      "⎢                                                                             \n",
      "⎢-0.135600778861087   -0.989558155295888  0.0488578147246194     -0.1255546   \n",
      "⎢                                                                             \n",
      "⎢ 0.985927790724374   -0.129908490419534  0.105214901959143       2.841452    \n",
      "⎢                                                                             \n",
      "⎣         0                   0                   0                 1.0       \n",
      "\n",
      "⎤\n",
      "⎥\n",
      "⎥\n",
      "⎥\n",
      "⎥\n",
      "⎥\n",
      "⎦\n"
     ]
    }
   ],
   "source": [
    "qs = ik(x = 2.3537, y = -0.1255546, z = 2.841452,\n",
    "        roll = 0.131008, pitch = -0.10541, yaw = 0.0491503, debug = True)\n",
    "\n",
    "# -0.07, 0.41, -1.07, 0.32, 0.46, 0 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "\n",
      " x: 2.3537 \n",
      " y: -0.1255546 \n",
      " z: 2.841452\n",
      "\n",
      " roll: 0.0491503 \n",
      " pitch: -0.10541 \n",
      " yaw: 0.131008\n",
      "\n",
      " j1: -0.0800813023075095 \n",
      " j2: 0.439897727669121 \n",
      " j3: -1.14290690404898\n",
      " j4: 0.363106480125535 \n",
      " j5: 0.626901765463759 \n",
      " j6: -0.226818468786107\n",
      "\n",
      "wrist_center (2.05496387941051, -0.164916872597119, 2.80957188470638)\n",
      "\n",
      "evaluated R0g:\n",
      "⎡-0.0977692193362551   0.135600778861087   0.985927790724374⎤\n",
      "⎢                                                           ⎥\n",
      "⎢-0.0624374999845176  -0.989558155295888   0.129908490419534⎥\n",
      "⎢                                                           ⎥\n",
      "⎣ 0.993248578325719   -0.0488578147246194  0.105214901959143⎦\n",
      "\n",
      "Total transform wrt gripper: given yaw (alpha), pitch (beta), roll (beta), px, py, pz\n",
      "⎡-0.0977692193362551   0.135600778861087   0.985927790724374    2.3537  ⎤\n",
      "⎢                                                                       ⎥\n",
      "⎢-0.0624374999845176  -0.989558155295888   0.129908490419534  -0.1255546⎥\n",
      "⎢                                                                       ⎥\n",
      "⎢ 0.993248578325719   -0.0488578147246194  0.105214901959143   2.841452 ⎥\n",
      "⎢                                                                       ⎥\n",
      "⎣         0                    0                   0             1.0    ⎦\n",
      "\n",
      "Total transform wrt gripper: given angles q1, q2, q3, q4, q5, q6\n",
      "⎡-0.0977692193362552   0.135600778861087   0.985927790724374   2.3536999999999\n",
      "⎢                                                                             \n",
      "⎢-0.0624374999845176  -0.989558155295888   0.129908490419534  -0.1255545999999\n",
      "⎢                                                                             \n",
      "⎢ 0.993248578325719   -0.0488578147246194  0.105214901959143   2.8414520000000\n",
      "⎢                                                                             \n",
      "⎣         0                    0                   0                 1.0      \n",
      "\n",
      "9 ⎤\n",
      "  ⎥\n",
      "99⎥\n",
      "  ⎥\n",
      "1 ⎥\n",
      "  ⎥\n",
      "  ⎦\n"
     ]
    }
   ],
   "source": [
    "qs = ik(x = 2.3537, y = -0.1255546, z = 2.841452,\n",
    "        yaw = 0.131008, pitch = -0.10541, roll = 0.0491503, debug = True)\n",
    "\n",
    "# -0.07, 0.41, -1.07, 0.32, 0.46, 0 "
   ]
  },
  {
   "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
}