notebooks/kinematicsz1.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import roboticstoolbox as rtb\n",
    "from spatialmath import *\n",
    "import pandas as pd\n",
    "from math import pi\n",
    "import matplotlib.pyplot as plt\n",
    "from matplotlib import cm\n",
    "np.set_printoptions(linewidth=100, formatter={'float': lambda x: f\"{x:8.4g}\" if abs(x) > 1e-10 else f\"{0:8.4g}\"})\n",
    "\n",
    "%matplotlib notebook"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Toolbox supports models defined using a number of different conventions.  We will load a very classical model, a Puma560 robot defined in terms of standard Denavit-Hartenberg parameters"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "p560 = rtb.models.Z1()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can display the simple Denavit-Hartenberg parameter model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "ERobot: Z1_DESCRIPTION (by Unitree), 7 joints (RRRRRRR), dynamics, geometry, collision\n",
      "┌─────┬───────────────┬───────┬───────────────┬───────────────────────────────┐\n",
      "│link │     link      │ joint │    parent     │      ETS: parent to link      │\n",
      "├─────┼───────────────┼───────┼───────────────┼───────────────────────────────┤\n",
      "│   0\u001B[0m │ \u001B[38;5;4mworld\u001B[0m         │      \u001B[0m │ BASE\u001B[0m          │ \u001B[0m                              │\n",
      "│   1\u001B[0m │ \u001B[38;5;4mlink00\u001B[0m        │      \u001B[0m │ world\u001B[0m         │ SE3()\u001B[0m                         │\n",
      "│   2\u001B[0m │ link01\u001B[0m        │     0\u001B[0m │ link00\u001B[0m        │ SE3(0, 0, 0.0585) ⊕ Rz(q0)\u001B[0m    │\n",
      "│   3\u001B[0m │ link02\u001B[0m        │     1\u001B[0m │ link01\u001B[0m        │ SE3(0, 0, 0.045) ⊕ Ry(q1)\u001B[0m     │\n",
      "│   4\u001B[0m │ link03\u001B[0m        │     2\u001B[0m │ link02\u001B[0m        │ SE3(-0.35, 0, 0) ⊕ Ry(q2)\u001B[0m     │\n",
      "│   5\u001B[0m │ link04\u001B[0m        │     3\u001B[0m │ link03\u001B[0m        │ SE3(0.218, 0, 0.057) ⊕ Ry(q3)\u001B[0m │\n",
      "│   6\u001B[0m │ link05\u001B[0m        │     4\u001B[0m │ link04\u001B[0m        │ SE3(0.07, 0, 0) ⊕ Rz(q4)\u001B[0m      │\n",
      "│   7\u001B[0m │ link06\u001B[0m        │     5\u001B[0m │ link05\u001B[0m        │ SE3(0.0492, 0, 0) ⊕ Rx(q5)\u001B[0m    │\n",
      "│   8\u001B[0m │ \u001B[38;5;4mgripperStator\u001B[0m │      \u001B[0m │ link06\u001B[0m        │ SE3(0.051, 0, 0)\u001B[0m              │\n",
      "│   9\u001B[0m │ @gripperMover\u001B[0m │     6\u001B[0m │ gripperStator\u001B[0m │ SE3(0.049, 0, 0) ⊕ Ry(q6)\u001B[0m     │\n",
      "└─────┴───────────────┴───────┴───────────────┴───────────────────────────────┘\n",
      "\n",
      "┌──────┬──────────┬─────────┬─────────┬────────┬───────────┬───────────┬─────┐\n",
      "│ name │ q0       │ q1      │ q2      │ q3     │ q4        │ q5        │ q6  │\n",
      "├──────┼──────────┼─────────┼─────────┼────────┼───────────┼───────────┼─────┤\n",
      "│   qr\u001B[0m │  0.0528°\u001B[0m │  0.039°\u001B[0m │ -0.209°\u001B[0m │ -4.3°\u001B[0m  │ -0.00745°\u001B[0m │  0.00201°\u001B[0m │  0°\u001B[0m │\n",
      "│   qz\u001B[0m │  0.0528°\u001B[0m │  0.039°\u001B[0m │ -0.209°\u001B[0m │ -4.3°\u001B[0m  │ -0.00745°\u001B[0m │  0.00201°\u001B[0m │  0°\u001B[0m │\n",
      "│grab3\u001B[0m │ -14.2°\u001B[0m   │  72.8°\u001B[0m  │ -92.4°\u001B[0m  │ -15.3°\u001B[0m │ -35.4°\u001B[0m    │  52.5°\u001B[0m    │  0°\u001B[0m │\n",
      "│   qn\u001B[0m │  0.0528°\u001B[0m │  0.039°\u001B[0m │ -0.209°\u001B[0m │ -4.3°\u001B[0m  │ -0.00745°\u001B[0m │  0.00201°\u001B[0m │  0°\u001B[0m │\n",
      "│   q1\u001B[0m │  0.0528°\u001B[0m │  0.039°\u001B[0m │ -0.209°\u001B[0m │ -4.3°\u001B[0m  │ -0.00745°\u001B[0m │  0.00201°\u001B[0m │  0°\u001B[0m │\n",
      "└──────┴──────────┴─────────┴─────────┴────────┴───────────┴───────────┴─────┘\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(p560)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The first table shows the kinematic parameters, and from the column titles we can see clearly that this is expressed in terms of standard Denavit-Hartenberg parameters.  The first column shows that the joint variables qi are rotations since they are in the θ column.  Joint limits are also shown.  Joint flip (motion in the opposite sense) would be indicated by the joint variable being shown as for example like `-q3`, and joint offsets by being shown as for example like `q2 + 45°`.\n",
    "\n",
    "The second table shows some named joint configurations.  For example `p560.qr` is "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "array([0.000922,  0.00068, -0.003654, -0.07501, -0.00013,  3.5e-05,        0])"
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.qr"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "outputs": [
    {
     "data": {
      "text/plain": "array([  -0.247,    1.271,   -1.613,   -0.267,   -0.617,    0.916,        0])"
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.grab3\n",
    "#cartesian space posture:  1.299 -0.485 -0.959  0.212 -0.142  0.674\n",
    "#install with pip install -e .\n",
    "#then you can add config in the Z1\n",
    "#restart jupyter kernel after adding configs"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If the robot had a base or tool transform they would be listed in this table also.\n",
    "\n",
    "This object is a subclass of `DHRobot`, equivalent to the `SerialLink` class in the MATLAB version of the Toolbox.\n",
    "This class has many methods and attributes, and we will explore some of them in this notebook."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can easily display the robot graphically"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "p560.plot(p560.grab3);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where `qn` is one of the named configurations shown above, and has the robot positioned to work above a table top.  You can use the mouse to rotate the plot and view the robot from different directions.  The grey line is the _shadow_ which is a projection of the robot onto the xy-plane.\n",
    "\n",
    "In this particular case the end-effector pose is given by the forward kinematics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "  \u001B[38;5;1m 0.5072  \u001B[0m \u001B[38;5;1m-0.03828 \u001B[0m \u001B[38;5;1m-0.861   \u001B[0m \u001B[38;5;4m 0.2117  \u001B[0m  \u001B[0m\n  \u001B[38;5;1m-0.7246  \u001B[0m \u001B[38;5;1m 0.5219  \u001B[0m \u001B[38;5;1m-0.4501  \u001B[0m \u001B[38;5;4m-0.1424  \u001B[0m  \u001B[0m\n  \u001B[38;5;1m 0.4666  \u001B[0m \u001B[38;5;1m 0.8521  \u001B[0m \u001B[38;5;1m 0.237   \u001B[0m \u001B[38;5;4m 0.6744  \u001B[0m  \u001B[0m\n  \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 1       \u001B[0m  \u001B[0m\n"
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.fkine(p560.grab3) #0.212 -0.142  0.674"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is a 4x4 SE(3) matrix displayed in a color coded way with rotation matrix in red, translation vector in blue, and constant elements in grey.  This is an instance of an `SE3` object safely encapsulates the SE(3) matrix.  This class, and related ones, are implemented by the [Spatial Math Toolbox for Python](https://github.com/petercorke/spatialmath-python).\n",
    "\n",
    "You can verify the end-effector position, the blue numbers are from top to bottom the x-, y- and z-coordinates of the end-effector position, match the plot shown above.\n",
    "\n",
    "We can manually adjust the joint angles of this robot (click and drag the sliders) to see how the shape of the robot changes and how the end-effector pose changes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "#p560.teach(); # works from console, hangs in Jupyter"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "An important problem in robotics is _inverse kinematics_, determining the joint angles to put the robot's end effector at a particular pose.\n",
    "\n",
    "Suppose we want the end-effector to be at position (0.5, 0.2, 0.1) and to have its gripper pointing (its _approach vector_) in the x-direction, and its fingers one above the other so that its _orientation vector_ is parallel to the z-axis.\n",
    "\n",
    "We can specify that pose by composing two SE(3) matrices:\n",
    "\n",
    "1. a pure translation\n",
    "2. a pure rotation defined in terms of the orientation and approach vectors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "  \u001B[38;5;1m 0       \u001B[0m \u001B[38;5;1m 0       \u001B[0m \u001B[38;5;1m 1       \u001B[0m \u001B[38;5;4m 0.5     \u001B[0m  \u001B[0m\n  \u001B[38;5;1m 1       \u001B[0m \u001B[38;5;1m 0       \u001B[0m \u001B[38;5;1m 0       \u001B[0m \u001B[38;5;4m 0.2     \u001B[0m  \u001B[0m\n  \u001B[38;5;1m 0       \u001B[0m \u001B[38;5;1m 1       \u001B[0m \u001B[38;5;1m 0       \u001B[0m \u001B[38;5;4m 0.5     \u001B[0m  \u001B[0m\n  \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 1       \u001B[0m  \u001B[0m\n"
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T = SE3(0.5, 0.2, 0.5) * SE3.OA([0,0,1], [1,0,0])\n",
    "T"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can compute the joint angles that results in this pose"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "sol = p560.ikine_LM(T,q0=p560.qn,joint_limits=1) # give it initial from the end of previous traj"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which returns the joint coordinates as well as solution status"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "IKSolution(q=array([  0.1063,    2.367,   -2.849,   0.4826,    1.357,    1.571,   0.1074]), success=False, iterations=2454, searches=100, residual=3.7969755766788736e-07, reason='iteration and search limit reached, solution found but violates joint limits')"
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "indicating, in this case, that there is no failure. The joint coordinates are"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "array([  0.1063,    2.367,   -2.849,   0.4826,    1.357,    1.571,   0.1074])"
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sol.q"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and we can confirm that this is indeed an inverse kinematic solution by computing the forward kinematics"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "  \u001B[38;5;1m-1.101e-06\u001B[0m \u001B[38;5;1m 1.161e-06\u001B[0m \u001B[38;5;1m 1       \u001B[0m \u001B[38;5;4m 0.5     \u001B[0m  \u001B[0m\n  \u001B[38;5;1m 1       \u001B[0m \u001B[38;5;1m 1.454e-05\u001B[0m \u001B[38;5;1m 1.101e-06\u001B[0m \u001B[38;5;4m 0.2     \u001B[0m  \u001B[0m\n  \u001B[38;5;1m-1.454e-05\u001B[0m \u001B[38;5;1m 1       \u001B[0m \u001B[38;5;1m-1.161e-06\u001B[0m \u001B[38;5;4m 0.5     \u001B[0m  \u001B[0m\n  \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 0       \u001B[0m \u001B[38;5;244m 1       \u001B[0m  \u001B[0m\n"
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.fkine(sol.q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which matches the original transform."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A simple trajectory between two joint configuration is"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "q2 = sol.q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "outputs": [],
   "source": [
    "qt = rtb.tools.trajectory.jtraj(p560.qz, q2, 250) #250 is how many steps to generate, take eucledian distance to last traj point and some speed to calculate better"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The result is a _namedtuple_ with attributes `q` containing the joint angles, as well as `qd`, `qdd` and `t` which hold the joint velocity, joint accelerations and time respectively.  \n",
    "\n",
    "The joint angles are a matrix with one column per joint and one row per timestep, and time increasing with row number."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": "array([[       0, -0.003791,   0.6372, ...,        0,        0,        0],\n       [       1, -0.003791,   0.6372, ..., 0.0001105, -1.094e-06, 0.0002896],\n       [       2, -0.003792,   0.6372, ..., 0.0004385, -4.342e-06, 0.001149],\n       ...,\n       [     247,   -0.247,    1.271, ..., 0.0004385, -4.342e-06, 0.001149],\n       [     248,   -0.247,    1.271, ..., 0.0001105, -1.094e-06, 0.0002896],\n       [     249,   -0.247,    1.271, ...,        0,        0,        0]])"
     },
     "execution_count": 54,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "qt.q\n",
    "id=np.arange(250)\n",
    "# zeros=np.zeros(250)\n",
    "np.hstack((id[:,None],qt.q,qt.qd)) #save this to traj file for Z1\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can plot this trajectory as a function of time using the convenience function `qplot`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# rtb.tools.trajectory.xplot(qt.q, block=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "and then we can animate this"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {},
   "outputs": [],
   "source": [
    "p560.plot(qt.q, dt=0.04);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "outputs": [],
   "source": [
    "file = np.genfromtxt('Traj_grab3.csv', dtype=float, delimiter=',')\n",
    "first_traj = file[500:-2500, 1:8]\n",
    "last_pos = first_traj[-1,:]\n",
    "qt = rtb.tools.trajectory.jtraj(last_pos,\n",
    "                                p560.grab3, 250)\n",
    "final_traj = np.vstack((first_traj[:-1,:],qt.q))"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "outputs": [],
   "source": [
    "first_traj_speed = np.vstack((file[500:-2501, 8:15], qt.qd))\n",
    "table = np.hstack((np.arange(500, final_traj.shape[0]+500).reshape(-1, 1),final_traj, first_traj_speed))"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 58,
   "outputs": [],
   "source": [
    "# Save file\n",
    "DF = pd.DataFrame(table)\n",
    "DF.to_csv('traj3.csv')"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "outputs": [
    {
     "ename": "ValueError",
     "evalue": "expecting (None, 7) but got (2502, 15)",
     "output_type": "error",
     "traceback": [
      "\u001B[1;31m---------------------------------------------------------------------------\u001B[0m",
      "\u001B[1;31mValueError\u001B[0m                                Traceback (most recent call last)",
      "Cell \u001B[1;32mIn[59], line 1\u001B[0m\n\u001B[1;32m----> 1\u001B[0m \u001B[43mp560\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mplot\u001B[49m\u001B[43m(\u001B[49m\u001B[43mtable\u001B[49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[43mdt\u001B[49m\u001B[38;5;241;43m=\u001B[39;49m\u001B[38;5;241;43m0.004\u001B[39;49m\u001B[43m)\u001B[49m;\n",
      "File \u001B[1;32mE:\\Project\\test\\robotics-toolbox-python\\roboticstoolbox\\robot\\Robot.py:1570\u001B[0m, in \u001B[0;36mRobot.plot\u001B[1;34m(self, q, backend, block, dt, limits, vellipse, fellipse, fig, movie, loop, **kwargs)\u001B[0m\n\u001B[0;32m   1566\u001B[0m env \u001B[38;5;241m=\u001B[39m \u001B[38;5;28;01mNone\u001B[39;00m\n\u001B[0;32m   1568\u001B[0m env \u001B[38;5;241m=\u001B[39m \u001B[38;5;28mself\u001B[39m\u001B[38;5;241m.\u001B[39m_get_graphical_backend(backend)\n\u001B[1;32m-> 1570\u001B[0m q \u001B[38;5;241m=\u001B[39m \u001B[43mgetmatrix\u001B[49m\u001B[43m(\u001B[49m\u001B[43mq\u001B[49m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[43m(\u001B[49m\u001B[38;5;28;43;01mNone\u001B[39;49;00m\u001B[43m,\u001B[49m\u001B[43m \u001B[49m\u001B[38;5;28;43mself\u001B[39;49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mn\u001B[49m\u001B[43m)\u001B[49m\u001B[43m)\u001B[49m\n\u001B[0;32m   1571\u001B[0m \u001B[38;5;28mself\u001B[39m\u001B[38;5;241m.\u001B[39mq \u001B[38;5;241m=\u001B[39m q[\u001B[38;5;241m0\u001B[39m, :]\n\u001B[0;32m   1573\u001B[0m \u001B[38;5;66;03m# Add the self to the figure in readonly mode\u001B[39;00m\n\u001B[0;32m   1574\u001B[0m \u001B[38;5;66;03m# Add the self to the figure in readonly mode\u001B[39;00m\n",
      "File \u001B[1;32mE:\\Project\\test\\robotics-toolbox-python\\venv\\lib\\site-packages\\spatialmath\\base\\argcheck.py:212\u001B[0m, in \u001B[0;36mgetmatrix\u001B[1;34m(m, shape, dtype)\u001B[0m\n\u001B[0;32m    210\u001B[0m         \u001B[38;5;28;01mreturn\u001B[39;00m np\u001B[38;5;241m.\u001B[39marray(m, dtype\u001B[38;5;241m=\u001B[39mdtype)\n\u001B[0;32m    211\u001B[0m     \u001B[38;5;28;01melse\u001B[39;00m:\n\u001B[1;32m--> 212\u001B[0m         \u001B[38;5;28;01mraise\u001B[39;00m \u001B[38;5;167;01mValueError\u001B[39;00m(\u001B[38;5;124mf\u001B[39m\u001B[38;5;124m\"\u001B[39m\u001B[38;5;124mexpecting \u001B[39m\u001B[38;5;132;01m{\u001B[39;00mshape\u001B[38;5;132;01m}\u001B[39;00m\u001B[38;5;124m but got \u001B[39m\u001B[38;5;132;01m{\u001B[39;00mmshape\u001B[38;5;132;01m}\u001B[39;00m\u001B[38;5;124m\"\u001B[39m)\n\u001B[0;32m    214\u001B[0m \u001B[38;5;28;01melif\u001B[39;00m isvector(m):\n\u001B[0;32m    215\u001B[0m     \u001B[38;5;66;03m# passed a 1D array\u001B[39;00m\n\u001B[0;32m    216\u001B[0m     m \u001B[38;5;241m=\u001B[39m getvector(m, dtype\u001B[38;5;241m=\u001B[39mdtype)\n",
      "\u001B[1;31mValueError\u001B[0m: expecting (None, 7) but got (2502, 15)"
     ]
    }
   ],
   "source": [
    "p560.plot(final_traj, dt=0.004);"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "_Note: animation not working in Jupyter..._"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The inverse kinematic solution was found using an iterative numerical procedure.  It is quite general but it has several drawbacks:\n",
    "- it can be slow\n",
    "- it may not find a solution, if the initial choice of joint coordinates is far from the solution (in the case above the default initial choice of all zeros was used)\n",
    "- it may not find the solution you want, in general there are multiple solutions for inverse kinematics.  For the same end-effector pose, the robot might:\n",
    "    - have it's arm on the left or right of its waist axis, \n",
    "    - the elbow could be up or down, and\n",
    "    - the wrist can flipped or not flipped.  For a two-finger gripper a rotation of \n",
    "      180° about the gripper axis leaves the fingers in the same configuration.\n",
    "\n",
    "Most industrial robots have a _spherical wrist_ which means that the last three joint axes intersect at a single point in the middle of the wrist mechanism.  We can test for this condition"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "# p560.isspherical() # not sperical"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This greatly simplifies things because the last three joints only control orientation and have no effect on the end-effector position.  This means that only the first three joints define position $(x_e, y_e, z_e)$.  Three joints that control three unknowns is relatively easy to solve for, and analytical solutions (complex trigonmetric equations) can be found, and in fact have been published for most industrial robot manipulators.\n",
    "\n",
    "The Puma560 has an analytical solution.  We can request the solution with the arm to the left and the elbow up, and the wrist not flipped by using the configuration string `\"lun\"`\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "#sol = p560.ikine_a(T, \"lun\")\n",
    "#sol"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "which is different to the values found earlier, but we can verify it is a valid solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "   1.7e-05   2.686e-05  1         0.488     \n   1        -0.0002246 -1.7e-05   0.1985    \n   0.0002246  1        -2.687e-05  0.4908    \n   0         0         0         1         \n"
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p560.fkine(sol.q)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In fact the solution we found earlier, but didn't explicitly specify, is the right-handed elbow-up configuration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "#sol = p560.ikine_a(T, \"run\")\n",
    "#sol.q"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Other useful functions include the manipulator Jacobian which maps joint velocity to end-effector velocity expressed in the world frame"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "array([[       0,     0.07,     0.07,     0.02,        0,        0,        0],\n       [    0.09,        0,        0,        0,     0.15,        0,        0],\n       [       0,    -0.09,    -0.44,    -0.22,        0,        0,        0],\n       [       0,        0,        0,        0,    -0.08,        1,        0],\n       [       0,        1,        1,        1,        0,        0,        1],\n       [       1,        0,        0,        0,        1,     0.08,        0]])"
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.round(p560.jacob0(p560.qn),decimals=2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": "array([[       0,     0.07,     0.07,     0.01,        0,        0],\n       [   -0.01,        0,        0,        0,     0.05,        0],\n       [       0,     0.01,    -0.34,    -0.12,        0,        0],\n       [       0,        0,        0,        0,    -0.08,        1],\n       [       0,        1,        1,        1,        0,        0],\n       [       1,        0,        0,        0,        1,     0.08]])"
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "np.round(p560.jacob0(p560.qn,start='link01',end='link06'),decimals=2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 86,
   "outputs": [
    {
     "ename": "AttributeError",
     "evalue": "'Z1' object has no attribute 'grab3'",
     "output_type": "error",
     "traceback": [
      "\u001B[0;31m---------------------------------------------------------------------------\u001B[0m",
      "\u001B[0;31mAttributeError\u001B[0m                            Traceback (most recent call last)",
      "Cell \u001B[0;32mIn [86], line 1\u001B[0m\n\u001B[0;32m----> 1\u001B[0m np\u001B[38;5;241m.\u001B[39mround(p560\u001B[38;5;241m.\u001B[39mjacob0(\u001B[43mp560\u001B[49m\u001B[38;5;241;43m.\u001B[39;49m\u001B[43mgrab3\u001B[49m),decimals\u001B[38;5;241m=\u001B[39m\u001B[38;5;241m2\u001B[39m)\n",
      "\u001B[0;31mAttributeError\u001B[0m: 'Z1' object has no attribute 'grab3'"
     ]
    }
   ],
   "source": [
    "np.round(p560.jacob0(p560.grab3),decimals=2)"
   ],
   "metadata": {
    "collapsed": false
   }
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "outputs": [],
   "source": [],
   "metadata": {
    "collapsed": false
   }
  }
 ],
 "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"
  },
  "toc": {
   "base_numbering": 1,
   "nav_menu": {},
   "number_sections": true,
   "sideBar": true,
   "skip_h1_title": false,
   "title_cell": "Table of Contents",
   "title_sidebar": "Contents",
   "toc_cell": false,
   "toc_position": {},
   "toc_section_display": true,
   "toc_window_display": false
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}