diff --git a/ArmNavigation/Planar_Two_Link_IK.ipynb b/ArmNavigation/Planar_Two_Link_IK.ipynb new file mode 100644 index 00000000..5f66cfb0 --- /dev/null +++ b/ArmNavigation/Planar_Two_Link_IK.ipynb @@ -0,0 +1,411 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# 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 forward_kinematics - forward kinematics specifies the end-effector position given the joint angles and link lengths. Forward kinematics is easier than inverse kinematics." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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\n", + "text/plain": [ + "
" + ] + }, + "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 shoulder, elbow, and wrist. 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 $x$ and $y$. 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": "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\n", + "text/plain": [ + "
" + ] + }, + "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$2 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": "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\n", + "text/plain": [ + "
" + ] + }, + "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": "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\n", + "text/plain": [ + "
" + ] + }, + "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 atan2 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.5.2" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +}