1. Position and Orientation.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Robotics (Pose and Orientation) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Technology alone can't solve problems. \n",
    "* Human alone is not enough any more. \n",
    "* We need Balance between using technology supervised by human.\n",
    "* What is the robot ?\n",
    "    * It is a programmable mechanical device that can perform tasks and interact with its environment. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Medical Applications of Robotics "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Surgical robots \n",
    "\n",
    "![](images/Cmglee_Cambridge_Science_Festival_2015_da_Vinci.jpg)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Provide precise movement\n",
    "* Controlled by surgon\n",
    "    * Even from continents away \n",
    "    * Telemedical robots (Virtual care) "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exoskeletal Robots "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![](images/Ekso-Bionics-Exoskeleton-Wearable-Robot-1.jpg)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* For paralyzed persons \n",
    "* Other uses (Powered body)\n",
    "    * Military \n",
    "    * Industry "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Faraway (Futuristic applications)\n",
    "* Robotic Nurse assistant\n",
    "* Micro and nano robots "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Robotics Problems \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Forward Kinematics (Identifying Position and Orientation of objects)\n",
    "* Inverse Kinematics (Finding required movements to perform a specific transformation)\n",
    "* Velocity Kinematics (Speed of movements) \n",
    "* Path Planning and Trajectory Generation \n",
    "* Vision \n",
    "* Dynamics \n",
    "* Position and force control "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Pose and Orientation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* What is the difference between\n",
    "    * Coordinate frame \n",
    "    * Coordinate vector \n",
    "    * Point in space"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![](images/Selection_090.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The point is the space is constant but coordinate vector is determined based on its coordinate frame"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**The position of the point must be defined with respect to reference frame**"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**$\\xi$ : Relative pose of a frame with repect to another frame**\n",
    "\n",
    "The pose is described by position ($x$, $y$) and orientation $\\theta$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Example**\n",
    "![](images/Selection_091.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$A_{\\xi_B}$ is the pose (vecotor) of coordinate frame $B$ with respect to coordinate frame $A$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Question:**\n",
    "\n",
    "Provide 3 representations of P with respect to the two coordinate frames $A$ and $B$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Algebric Rules "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\\xi \\oplus  0 = \\xi$$\n",
    "$$\\xi  \\ominus  0 = \\xi$$\n",
    "$$ \\ominus X_{\\xi_Y} = Y_{\\xi_X}$$\n",
    "$$  X_{\\xi_Y} \\oplus Y_{\\xi_Z}  = X_{\\xi_Z}$$\n",
    "$$  \\xi_1 \\oplus \\xi_2  \\neq \\xi_2 \\oplus \\xi_1$$\n",
    "$$ X_p = X_{\\xi_Y} . Y_p$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Spatial Ecludian group SE\n",
    "* Mathematical representation\n",
    "* SE(2) group of two dimentional vector\n",
    "* SE(3) group of three dimentional vector\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Represneting pose in 2D plane "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* Represented by a transformation matrix \n",
    "    * Rotation\n",
    "    * Translation\n",
    "* Homogenous Coordinates\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![](images/Selection_092.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "$$\\xi(x, y, \\theta) = \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & x\\\\\n",
    "sin(\\theta) & cos(\\theta) & y \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where $R$ is the 2D rotation matrix $R \\in SO(2)$ (Spatial Orthogonal group)\n",
    "\n",
    "$$R = \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) \\\\\n",
    "sin(\\theta) & cos(\\theta) \n",
    "\\end{bmatrix}$$\n",
    "\n",
    "* $R^{-1} = R^T$ \n",
    "\n",
    "and \n",
    "\n",
    "$$t = \\begin{bmatrix}\n",
    "x \\\\ y \n",
    "\\end{bmatrix}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rotation matrix proof\n",
    "\n",
    "For points (1,0) and (0, 1) the new points with rotation angle $\\theta$ will be \n",
    "$(cos(\\theta), sin(\\theta))$ and $(-sin(\\theta), cos(\\theta))$ \n",
    "\n",
    "for any point (x, y) we can represent it with \n",
    "$(x, y) = x\\times(1,0)+ y \\times(0, 1)$\n",
    "\n",
    "After rotation with angle $\\theta$ \n",
    "\n",
    "$(x', y') = x \\times(cos(\\theta), sin(\\theta))+ y \\times (-sin(\\theta), cos(\\theta))$\n",
    "\n",
    "In matrix form \n",
    "\n",
    "$$\\begin{bmatrix}x' \\\\ y'\\end{bmatrix} = \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) \\\\\n",
    "sin(\\theta) & cos(\\theta) \n",
    "\\end{bmatrix} \\begin{bmatrix}x \\\\ y\\end{bmatrix}$$\n",
    "\n",
    "In homogenous coordinates \n",
    "\n",
    "$$\\begin{bmatrix}x' \\\\ y' \\\\1 \\end{bmatrix} = \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & 0 \\\\\n",
    "sin(\\theta) & cos(\\theta) & 0 \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix} \\begin{bmatrix}x \\\\ y \\\\ 1 \\end{bmatrix}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Translation matrix in homogenous coordinates\n",
    "$$\n",
    "\\begin{bmatrix}\n",
    "x' \\\\ y' \\\\ 1\n",
    "\\end{bmatrix} = \\begin{bmatrix}\n",
    "1 & 0 & t_x \\\\\n",
    "0 & 1 & t_y \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix} \\begin{bmatrix} \n",
    "x \\\\ \n",
    "y \n",
    "\\end{bmatrix} \n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Combined transformation \n",
    "$$T = t \\times R $$\n",
    "$$ T = \\begin{bmatrix}\n",
    "1 & 0 & t_x \\\\\n",
    "0 & 1 & t_y \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}  \\times \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & 0 \\\\\n",
    "sin(\\theta) & cos(\\theta) & 0 \\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}$$\n",
    "\n",
    "$$\n",
    "T = \\begin{bmatrix}\n",
    "cos(\\theta) & -sin(\\theta) & x \\\\\n",
    "sin(\\theta) & cos(\\theta) & y\\\\\n",
    "0 & 0 & 1\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The Transformation matrix is defined as\n",
    "\n",
    "$$T = \\begin{bmatrix}\n",
    "R & t\\\\\n",
    "0 & 1\n",
    "\\end{bmatrix}$$\n",
    "\n",
    "\n",
    "$$T^{-1} = \\begin{bmatrix}\n",
    "R & t\\\\\n",
    "0 & 1\n",
    "\\end{bmatrix}^{-1}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And \n",
    "\n",
    "$$T_1 \\oplus T_2 = T_1.T_2 =  \\begin{bmatrix}\n",
    "R_1 & t_1\\\\\n",
    "0 & 1\n",
    "\\end{bmatrix} \\begin{bmatrix}\n",
    "R_2 & t_2\\\\\n",
    "0 & 1\n",
    "\\end{bmatrix}$$ "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Example\n",
    "\n",
    "a frame {B} that is rotated relative to frame {A} about Z by\n",
    "30 degrees. Here, Z is pointing out of the page.\n",
    "\n",
    "Given \n",
    "\n",
    "$$B_p = \\begin{bmatrix}\n",
    "0 \\\\ 2  \n",
    "\\end{bmatrix}$$\n",
    "\n",
    "Calculate \n",
    "$$A_p$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![](images/Selection_093.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "theta = 30*np.pi/180\n",
    "A_xi_B = np.array([[np.cos(theta), -np.sin(theta), 0],\n",
    "                   [np.sin(theta), np.cos(theta), 0],\n",
    "                   [0            , 0            , 1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[ 0.8660254 -0.5        0.       ]\n",
      " [ 0.5        0.8660254  0.       ]\n",
      " [ 0.         0.         1.       ]]\n"
     ]
    }
   ],
   "source": [
    "print(A_xi_B)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Homogenous coordinates\n",
    "Bp = np.array([[0],[2],[1]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[0],\n",
       "       [2],\n",
       "       [1]])"
      ]
     },
     "execution_count": 33,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "Bp"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [],
   "source": [
    "# In homogenous coord\n",
    "Ap = A_xi_B.dot(Bp)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-1.        ]\n",
      " [ 1.73205081]]\n"
     ]
    }
   ],
   "source": [
    "Ap = Ap[:2]\n",
    "print(Ap)"
   ]
  }
 ],
 "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.7.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}