Foward kinematics of a RR planar manipulator

2rr_foward_kinematics.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2 DOF Arm (RR)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import pi\n",
    "from sympy import init_printing\n",
    "from sympy import Matrix, cos, sin\n",
    "\n",
    "init_printing()\n",
    "\n",
    "def translation(v):\n",
    "    return Matrix([\n",
    "        [1, 0, v[0], 0],\n",
    "        [0, 1, v[1], 0],\n",
    "        [0, 0, 1,    0],\n",
    "        [0, 0, 0,    1]\n",
    "    ])\n",
    "\n",
    "def rotation(q):\n",
    "    return Matrix([\n",
    "        [cos(q), -sin(q), 0, 0],\n",
    "        [sin(q),  cos(q), 0, 0],\n",
    "        [0,       0,      1, 0],\n",
    "        [0,       0,      0, 1]\n",
    "    ])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![title](https://i.imgur.com/ahPxroW.png)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$$\\left ( \\left[\\begin{matrix}0 & -1 & 0 & 0\\\\1 & 0 & 0 & 0\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right], \\quad \\left[\\begin{matrix}0 & -1 & 0 & 0\\\\1 & 0 & 1 & 0\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right], \\quad \\left[\\begin{matrix}0 & -1 & 0 & 0\\\\1 & 0 & 2 & 0\\\\0 & 0 & 1 & 0\\\\0 & 0 & 0 & 1\\end{matrix}\\right]\\right )$$"
      ],
      "text/plain": [
       "⎛⎡0  -1  0  0⎤, ⎡0  -1  0  0⎤, ⎡0  -1  0  0⎤⎞\n",
       "⎜⎢           ⎥  ⎢           ⎥  ⎢           ⎥⎟\n",
       "⎜⎢1  0   0  0⎥  ⎢1  0   1  0⎥  ⎢1  0   2  0⎥⎟\n",
       "⎜⎢           ⎥  ⎢           ⎥  ⎢           ⎥⎟\n",
       "⎜⎢0  0   1  0⎥  ⎢0  0   1  0⎥  ⎢0  0   1  0⎥⎟\n",
       "⎜⎢           ⎥  ⎢           ⎥  ⎢           ⎥⎟\n",
       "⎝⎣0  0   0  1⎦  ⎣0  0   0  1⎦  ⎣0  0   0  1⎦⎠"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "def RRfkine(q1,q2):\n",
    "    # homogenous transformations from frames 0 to 4\n",
    "    H01 = rotation(q1)\n",
    "    H12 = translation([1,0])\n",
    "    H23 = rotation(q2)\n",
    "    H34 = translation([1,0])\n",
    "    \n",
    "    # compounding transformations\n",
    "    A1 = H01\n",
    "    A2 = A1 * H12\n",
    "    A3 = A2 * H23\n",
    "    A4 = A3 * H34\n",
    "    H04 = H01 * H12 * H23 * H34\n",
    "    \n",
    "    return A1, A2, A4\n",
    "\n",
    "RRfkine(pi/2,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Oriention: Matrix([[0, -1], [1, 0]])\n",
      "\tEnd effector (x,y) [1 1]\n",
      "\tPoints of frame origins (x,y) p1=[0 0], p2=[1 0]\n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAYQAAAD8CAYAAAB3u9PLAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7f16084dbeb8>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "def drawRR(frames):\n",
    "    %matplotlib inline\n",
    "    import matplotlib.pyplot as plt\n",
    "    import numpy as np\n",
    "    \n",
    "    T1, T2, T3 = frames\n",
    "    \n",
    "    R1 = T1[0:2, 0:2]\n",
    "    D1 = T1[0:2, 2:3]\n",
    "    p1 = np.transpose(D1) * R1\n",
    "    p1 = np.array(p1).flatten()\n",
    "    \n",
    "    R2 = T2[0:2, 0:2]\n",
    "    D2 = T2[0:2, 2:3]\n",
    "    p2 = np.transpose(D2) * R2\n",
    "    p2 = np.array(p2).flatten()\n",
    "    \n",
    "    R3 = T3[0:2, 0:2]\n",
    "    D3 = T3[0:2, 2:3]\n",
    "    end = np.transpose(D3) * R3 \n",
    "    end = np.array(end).flatten()\n",
    "    \n",
    "    print('Oriention: {}\\n\\tEnd effector (x,y) {}'.format(R3,end))\n",
    "    print('\\tPoints of frame origins (x,y) p1={}, p2={}'.format(p1, p2))\n",
    "        \n",
    "    plt.xlim([-0.5,2.5])\n",
    "    plt.ylim([-2.0,2.5]) \n",
    "    \n",
    "    plt.scatter([p1[0], p2[0], end[0]], \n",
    "                [p1[1], p2[1], end[1]], color='red')\n",
    "    plt.plot([p1[0], p2[0], end[0]], \n",
    "             [p1[1], p2[1], end[1]])\n",
    "    \n",
    "drawRR(RRfkine(pi,-pi/2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "anaconda-cloud": {},
  "colab": {
   "collapsed_sections": [],
   "default_view": {},
   "name": "foward_kinematics.ipynb",
   "provenance": [],
   "version": "0.3.2",
   "views": {}
  },
  "kernelspec": {
   "display_name": "Python [default]",
   "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": 1
}