{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Two joint arm to point control\n",
    "\n",
    "![TwoJointArmToPointControl](https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/two_joint_arm_to_point_control/animation.gif)\n",
    "\n",
    "This is two joint arm to a point control simulation.\n",
    "\n",
    "This is a interactive simulation.\n",
    "\n",
    "You can set the goal position of the end effector with left-click on the ploting area.\n",
    "\n",
    "\n",
    "\n",
    "### Inverse Kinematics for a Planar Two-Link Robotic Arm\n",
    "\n",
    "A classic problem with robotic arms is getting the end-effector, the mechanism at the end of the arm responsible for manipulating the environment, to where you need it to be. Maybe the end-effector is a gripper and maybe you want to pick up an object and maybe you know where that object is relative to the robot - but you cannot tell the end-effector where to go directly. Instead, you have to determine the joint angles that get the end-effector to where you want it to be. This problem is known as inverse kinematics.\n",
    "\n",
    "Credit for this solution goes to: https://robotacademy.net.au/lesson/inverse-kinematics-for-a-2-joint-robot-arm-using-geometry/\n",
    "\n",
    "First, let's define a class to make plotting our arm easier.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "from math import cos, sin\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "class TwoLinkArm:\n",
    "    def __init__(self, joint_angles=[0, 0]):\n",
    "        self.shoulder = np.array([0, 0])\n",
    "        self.link_lengths = [1, 1]\n",
    "        self.update_joints(joint_angles)\n",
    "        \n",
    "    def update_joints(self, joint_angles):\n",
    "        self.joint_angles = joint_angles\n",
    "        self.forward_kinematics()\n",
    "        \n",
    "    def forward_kinematics(self):\n",
    "        theta0 = self.joint_angles[0]\n",
    "        theta1 = self.joint_angles[1]\n",
    "        l0 = self.link_lengths[0]\n",
    "        l1 = self.link_lengths[1]\n",
    "        self.elbow = self.shoulder + np.array([l0*cos(theta0), l0*sin(theta0)])\n",
    "        self.wrist = self.elbow + np.array([l1*cos(theta0 + theta1), l1*sin(theta0 + theta1)])\n",
    "        \n",
    "    def plot(self):\n",
    "        plt.plot([self.shoulder[0], self.elbow[0]],\n",
    "                 [self.shoulder[1], self.elbow[1]],\n",
    "                 'r-')\n",
    "        plt.plot([self.elbow[0], self.wrist[0]],\n",
    "                 [self.elbow[1], self.wrist[1]],\n",
    "                 'r-')\n",
    "        plt.plot(self.shoulder[0], self.shoulder[1], 'ko')\n",
    "        plt.plot(self.elbow[0], self.elbow[1], 'ko')\n",
    "        plt.plot(self.wrist[0], self.wrist[1], 'ko')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's also define a function to make it easier to draw an angle on our diagram."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from math import sqrt\n",
    "\n",
    "def transform_points(points, theta, origin):\n",
    "    T = np.array([[cos(theta), -sin(theta), origin[0]],\n",
    "                  [sin(theta), cos(theta), origin[1]],\n",
    "                  [0, 0, 1]])\n",
    "    return np.matmul(T, np.array(points))\n",
    "\n",
    "def draw_angle(angle, offset=0, origin=[0, 0], r=0.5, n_points=100):\n",
    "        x_start = r*cos(angle)\n",
    "        x_end = r\n",
    "        dx = (x_end - x_start)/(n_points-1)\n",
    "        coords = [[0 for _ in range(n_points)] for _ in range(3)]\n",
    "        x = x_start\n",
    "        for i in range(n_points-1):\n",
    "            y = sqrt(r**2 - x**2)\n",
    "            coords[0][i] = x\n",
    "            coords[1][i] = y\n",
    "            coords[2][i] = 1\n",
    "            x += dx\n",
    "        coords[0][-1] = r\n",
    "        coords[2][-1] = 1\n",
    "        coords = transform_points(coords, offset, origin)\n",
    "        plt.plot(coords[0], coords[1], 'k-')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Okay, we now have a TwoLinkArm class to help us draw the arm, which we'll do several times during our derivation. Notice there is a method called <i>forward_kinematics</i> - forward kinematics specifies the end-effector position <i>given</i> the joint angles and link lengths. Forward kinematics is easier than inverse kinematics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 17812 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "arm = TwoLinkArm()\n",
    "\n",
    "theta0 = 0.5\n",
    "theta1 = 1\n",
    "\n",
    "arm.update_joints([theta0, theta1])\n",
    "arm.plot()\n",
    "\n",
    "def label_diagram():\n",
    "    plt.plot([0, 0.5], [0, 0], 'k--')\n",
    "    plt.plot([arm.elbow[0], arm.elbow[0]+0.5*cos(theta0)],\n",
    "             [arm.elbow[1], arm.elbow[1]+0.5*sin(theta0)],\n",
    "             'k--')\n",
    "    \n",
    "    draw_angle(theta0, r=0.25)\n",
    "    draw_angle(theta1, offset=theta0, origin=[arm.elbow[0], arm.elbow[1]], r=0.25)\n",
    "    \n",
    "    plt.annotate(\"$l_0$\", xy=(0.5, 0.4), size=15, color=\"r\")\n",
    "    plt.annotate(\"$l_1$\", xy=(0.8, 1), size=15, color=\"r\")\n",
    "    \n",
    "    plt.annotate(r\"$\\theta_0$\", xy=(0.35, 0.05), size=15)\n",
    "    plt.annotate(r\"$\\theta_1$\", xy=(1, 0.8), size=15)\n",
    "\n",
    "label_diagram()\n",
    "\n",
    "plt.annotate(\"Shoulder\", xy=(arm.shoulder[0], arm.shoulder[1]), xytext=(0.15, 0.5),\n",
    "    arrowprops=dict(facecolor='black', shrink=0.05))\n",
    "plt.annotate(\"Elbow\", xy=(arm.elbow[0], arm.elbow[1]), xytext=(1.25, 0.25),\n",
    "    arrowprops=dict(facecolor='black', shrink=0.05))\n",
    "plt.annotate(\"Wrist\", xy=(arm.wrist[0], arm.wrist[1]), xytext=(1, 1.75),\n",
    "    arrowprops=dict(facecolor='black', shrink=0.05))\n",
    "\n",
    "plt.axis(\"equal\")\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It's common to name arm joints anatomically, hence the names <i>shoulder</i>, <i>elbow</i>, and <i>wrist</i>. In this example, the wrist is not itself a joint, but we can consider it to be our end-effector. If we constrain the shoulder to the origin, we can write the forward kinematics for the elbow and the wrist.\n",
    "\n",
    "$elbow_x = l_0\\cos(\\theta_0)$    \n",
    "$elbow_y = l_0\\sin(\\theta_0)$  \n",
    "\n",
    "$wrist_x = elbow_x + l_1\\cos(\\theta_0+\\theta_1) = l_0\\cos(\\theta_0) + l_1\\cos(\\theta_0+\\theta_1)$  \n",
    "$wrist_y = elbow_y + l_1\\sin(\\theta_0+\\theta_1) = l_0\\sin(\\theta_0) + l_1\\sin(\\theta_0+\\theta_1)$  \n",
    "\n",
    "Since the wrist is our end-effector, let's just call its coordinates <i>$x$</i> and <i>$y$</i>. The forward kinematics for our end-effector is then\n",
    "\n",
    "$x = l_0\\cos(\\theta_0) + l_1\\cos(\\theta_0+\\theta_1)$   \n",
    "$y = l_0\\sin(\\theta_0) + l_1\\sin(\\theta_0+\\theta_1)$ \n",
    "\n",
    "A first attempt to find the joint angles $\\theta_0$ and $\\theta_1$ that would get our end-effector to the desired coordinates $x$ and $y$ might be to try solving the forward kinematics for $\\theta_0$ and $\\theta_1$, but that would be the wrong move. An easier path involves going back to the geometry of the arm."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 18680 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from math import pi\n",
    "\n",
    "arm.plot()\n",
    "label_diagram()\n",
    "\n",
    "plt.plot([0, arm.wrist[0]],\n",
    "         [0, arm.wrist[1]],\n",
    "         'k--')\n",
    "\n",
    "plt.plot([arm.wrist[0], arm.wrist[0]],\n",
    "         [0, arm.wrist[1]],\n",
    "         'b--')\n",
    "plt.plot([0, arm.wrist[0]],\n",
    "         [0, 0],\n",
    "         'b--')\n",
    "\n",
    "plt.annotate(\"$x$\", xy=(0.6, 0.05), size=15, color=\"b\")\n",
    "plt.annotate(\"$y$\", xy=(1, 0.2), size=15, color=\"b\")\n",
    "plt.annotate(\"$r$\", xy=(0.45, 0.9), size=15)\n",
    "plt.annotate(r\"$\\alpha$\", xy=(0.75, 0.6), size=15)\n",
    "\n",
    "alpha = pi-theta1\n",
    "draw_angle(alpha, offset=theta0+theta1, origin=[arm.elbow[0], arm.elbow[1]], r=0.1)\n",
    "\n",
    "plt.axis(\"equal\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The distance from the end-effector to the robot base (shoulder joint) is $r$ and can be written in terms of the end-effector position using the Pythagorean Theorem.\n",
    "\n",
    "$r^2$ = $x^2 + y^2$\n",
    "\n",
    "Then, by the law of cosines, $r$<sup>2</sup> can also be written as:\n",
    "\n",
    "$r^2$ = $l_0^2 + l_1^2 - 2l_0l_1\\cos(\\alpha)$\n",
    "\n",
    "Because $\\alpha$ can be written as $\\pi - \\theta_1$, we can relate the desired end-effector position to one of our joint angles, $\\theta_1$.\n",
    "\n",
    "$x^2 + y^2$ = $l_0^2 + l_1^2 - 2l_0l_1\\cos(\\alpha)$  \n",
    "  \n",
    "$x^2 + y^2$ = $l_0^2 + l_1^2 - 2l_0l_1\\cos(\\pi-\\theta_1)$  \n",
    "  \n",
    "$2l_0l_1\\cos(\\pi-\\theta_1) = l_0^2 + l_1^2 - x^2 - y^2$  \n",
    "  \n",
    "$\\cos(\\pi-\\theta_1) = \\frac{l_0^2 + l_1^2 - x^2 - y^2}{2l_0l_1}$  \n",
    "$~$  \n",
    "$~$  \n",
    "$\\cos(\\pi-\\theta_1) = -cos(\\theta_1)$ is a trigonometric identity, so we can also write\n",
    "\n",
    "$\\cos(\\theta_1) = \\frac{x^2 + y^2 - l_0^2 - l_1^2}{2l_0l_1}$  \n",
    "\n",
    "which leads us to an equation for $\\theta_1$ in terms of the link lengths and the desired end-effector position!\n",
    "\n",
    "$\\theta_1 = \\cos^{-1}(\\frac{x^2 + y^2 - l_0^2 - l_1^2}{2l_0l_1})$  \n",
    "\n",
    "This is actually one of two possible solutions for $\\theta_1$, but we'll ignore the other possibility for now. This solution will lead us to the \"arm-down\" configuration of the arm, which is what's shown in the diagram. Now we'll derive an equation for $\\theta_0$ that depends on this value of $\\theta_1$."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 22208 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from math import atan2\n",
    "\n",
    "arm.plot()\n",
    "plt.plot([0, arm.wrist[0]],\n",
    "         [0, arm.wrist[1]],\n",
    "         'k--')\n",
    "\n",
    "p = 1 + cos(theta1)\n",
    "plt.plot([arm.elbow[0], p*cos(theta0)],\n",
    "         [arm.elbow[1], p*sin(theta0)],\n",
    "         'b--', linewidth=5)\n",
    "plt.plot([arm.wrist[0], p*cos(theta0)],\n",
    "         [arm.wrist[1], p*sin(theta0)],\n",
    "         'b--', linewidth=5)\n",
    "\n",
    "beta = atan2(arm.wrist[1], arm.wrist[0])-theta0\n",
    "draw_angle(beta, offset=theta0, r=0.45)\n",
    "\n",
    "plt.annotate(r\"$\\beta$\", xy=(0.35, 0.35), size=15)\n",
    "plt.annotate(\"$r$\", xy=(0.45, 0.9), size=15)\n",
    "plt.annotate(r\"$l_1sin(\\theta_1)$\",xy=(1.25, 1.1), size=15, color=\"b\")\n",
    "plt.annotate(r\"$l_1cos(\\theta_1)$\",xy=(1.1, 0.4), size=15, color=\"b\")\n",
    "\n",
    "label_diagram()\n",
    "\n",
    "plt.axis(\"equal\")\n",
    "\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Consider the angle between the displacement vector $r$ and the first link $l_0$; let's call it $\\beta$. If we extend the first link to include the component of the second link in the same direction as the first, we form a right triangle with components $l_0+l_1cos(\\theta_1)$ and $l_1sin(\\theta_1)$, allowing us to express $\\beta$ as\n",
    "  \n",
    "$\\beta = \\tan^{-1}(\\frac{l_1\\sin(\\theta_1)}{l_0+l_1\\cos(\\theta_1)})$  \n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now have an expression for this angle $\\beta$ in terms of one of our arm's joint angles. Now, can we relate $\\beta$ to $\\theta_0$? Yes!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 19864 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "arm.plot()\n",
    "label_diagram()\n",
    "plt.plot([0, arm.wrist[0]],\n",
    "         [0, arm.wrist[1]],\n",
    "         'k--')\n",
    "\n",
    "plt.plot([arm.wrist[0], arm.wrist[0]],\n",
    "         [0, arm.wrist[1]],\n",
    "         'b--')\n",
    "plt.plot([0, arm.wrist[0]],\n",
    "         [0, 0],\n",
    "         'b--')\n",
    "\n",
    "gamma = atan2(arm.wrist[1], arm.wrist[0])\n",
    "draw_angle(beta, offset=theta0, r=0.2)\n",
    "draw_angle(gamma, r=0.6)\n",
    "\n",
    "plt.annotate(\"$x$\", xy=(0.7, 0.05), size=15, color=\"b\")\n",
    "plt.annotate(\"$y$\", xy=(1, 0.2), size=15, color=\"b\")\n",
    "plt.annotate(r\"$\\beta$\", xy=(0.2, 0.2), size=15)\n",
    "plt.annotate(r\"$\\gamma$\", xy=(0.6, 0.2), size=15)\n",
    "\n",
    "plt.axis(\"equal\")\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Our first joint angle $\\theta_0$ added to $\\beta$ gives us the angle between the positive $x$-axis and the displacement vector $r$; let's call this angle $\\gamma$.\n",
    "\n",
    "$\\gamma = \\theta_0+\\beta$  \n",
    "\n",
    "$\\theta_0$, our remaining joint angle, can then be expressed as \n",
    "\n",
    "$\\theta_0 = \\gamma-\\beta$  \n",
    "\n",
    "We already know $\\beta$. $\\gamma$ is simply the inverse tangent of $\\frac{y}{x}$, so we have an equation of $\\theta_0$! \n",
    "\n",
    "$\\theta_0 = \\tan^{-1}(\\frac{y}{x})-\\tan^{-1}(\\frac{l_1\\sin(\\theta_1)}{l_0+l_1\\cos(\\theta_1)})$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We now have the inverse kinematics for a planar two-link robotic arm. If you're planning on implementing this in a programming language, it's best to use the <i>atan2</i> function, which is included in most math libraries and correctly accounts for the signs of $y$ and $x$. Notice that $\\theta_1$ must be calculated before $\\theta_0$.\n",
    "\n",
    "$\\theta_1 = \\cos^{-1}(\\frac{x^2 + y^2 - l_0^2 - l_1^2}{2l_0l_1})$  \n",
    "$\\theta_0 = atan2(y, x)-atan2(l_1\\sin(\\theta_1), l_0+l_1\\cos(\\theta_1))$"
   ]
  }
 ],
 "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.6.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
