{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    }
   ],
   "source": [
    "from poppy.creatures import PoppyHumanoid\n",
    "\n",
    "poppy = PoppyHumanoid(simulator='vrep',scene='poppy_humanoid_inertia.ttt')\n",
    "\n",
    "%pylab inline\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Prepared poppy for experiments"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.head_y.goto_position(0, 4, wait=False)\n",
    "poppy.r_shoulder_y.goto_position(0, 4, wait=False)\n",
    "poppy.l_shoulder_y.goto_position(0, 4, wait=True)\n",
    "poppy.l_shoulder_x.goto_position(20, 4, wait=False)\n",
    "poppy.r_shoulder_x.goto_position(-20, 4, wait=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Put poppy's hands in the air and plot the movement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.1\n",
      "0.4\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "[<matplotlib.lines.Line2D at 0x7a26950>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": [
       "iVBORw0KGgoAAAANSUhEUgAAAYAAAAEACAYAAAC6d6FnAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz\n",
       "AAALEgAACxIB0t1+/AAAFW9JREFUeJzt3X+s3Xd93/Hni+uGNT/Kj5E2w1xqStKWdJ0KFI+Rtrql\n",
       "HjJsw0xMChGMdlSpxeTA1G4ybafGaJrWaqqGprTIpV7XsTBPy5LI1RKchPZWWZcmNjg/SOzILljY\n",
       "TigmoRkBoV037/1xviYnN/eeH/f8utff50OyfL4/Pt/zPh9ff1/n++t+UlVIktrnJbMuQJI0GwaA\n",
       "JLWUASBJLWUASFJLGQCS1FIGgCS1VN8ASLI9ybEkx5PsXmH5QpJnkhxp/vzrQdtKkmYnvZ4DSDIH\n",
       "PA5sA84Ah4Drqupo1zoLwC9X1buHbStJmp1+RwBbgRNVdbKqloD9wI4V1ssIbSVJM9AvADYDp7qm\n",
       "TzfzuhXwtiQPJbkjydVDtJUkzcimPssH+T0RXwDmq+rbSd4J3A788MiVSZImql8AnAHmu6bn6XyT\n",
       "/66q+mbX6zuT/G6SVzbr9WwLkMRfRiRJa1BVK51+H1i/ADgMXJVkC/AEcC1wXfcKSX4A+FpVVZKt\n",
       "dC4sP52kb9vzRv0QG0HCjcBLq/i11dfJnqraM72q1i/74nn2xfPsi+eN48tzzwCoqnNJdgEHgTlg\n",
       "X1UdTbKzWb4X+CfAh5OcA74NvK9X21EL3sAeBf7prIuQpPP6HQFQVXcCdy6bt7fr9e8AvzNo2xZ7\n",
       "FPixWRchSef5JPD0nAA2J1zcY53FKdWyESzOuoB1ZHHWBawji7Mu4ELS80GwqRSQVBuuAQAkPAL8\n",
       "QhWfn3Utkja2cew7PQKYri/iaSBJ64QBMF1eB5C0bhgA02UASFo3DIDpMgAkrRteBJ6ihDngWeDy\n",
       "Kp6ddT2SNi4vAm8wVfw1cAy4ut+6kjRpBsD0eRpI0rpgAEyfASBpXTAAps8AkLQuGADT9yjwt2dd\n",
       "hCQZANP3ZeCVCS+bdSGS2s0AmLIqngOO4p1AkmbMAJgNrwNImjkDYDYMAEkz1zcAkmxPcizJ8SS7\n",
       "e6z3liTnkry3a97JJA8nOZLkgXEVfQHwQrCkmes5IliSOeAmYBudAeIPJTmwfGjHZr3fAj67bBMF\n",
       "LFTV0+Mr+YLgr4WWNHP9jgC2Aieq6mRVLQH7gR0rrHcDcAtwdoVlrfg9P0P6CnBZwitmXYik9uoX\n",
       "AJuBU13Tp5t535VkM51Q+GQzq/u3yxVwT5LDSa4fsdYLRhUFPIZHAZJmqN+g8IP8qtBPAB+rqkoS\n",
       "XviN/5qqejLJ5cDdSY5V1b3LN5BkT9fkYlUtDvC+G935C8H/e9aFSFr/kiwAC+PcZr8AOAPMd03P\n",
       "0zkK6PZmYH9n38+rgHcmWaqqA1X1JEBVnU1yG51TSi8KgKras7byNzQvBEsaWPPFePH8dJIbR91m\n",
       "v1NAh4GrkmxJchFwLXBgWVE/VFWvq6rX0bkO8OGqOpDk4iSXNYVeArwDeGTUgi8gXgiWNFM9jwCq\n",
       "6lySXcBBYA7YV1VHk+xslu/t0fwK4NbmyGATcHNV3TWesi8IPgsgaaYcEWxGEgJ8A7iyiq/Puh5J\n",
       "G4sjgm1g3gkkadYMgNnyQrCkmTEAZssLwZJmxgCYLS8ES5oZA2C2HgV+rLkgLElTZQDM1lfp/Bt8\n",
       "/6wLkdQ+BsAMNXcCfREvBEuaAQNg9rwOIGkmDIDZMwAkzYQBMHsGgKSZMABmzzuBJM2EATBjVXwN\n",
       "WAJePetaJLWLAbA+eBpI0tQZAOuDASBp6gyA9cEAkDR1BsD6YABImrq+AZBke5JjSY4n2d1jvbck\n",
       "OZfkvcO2lXcCSZq+ngGQZA64CdgOXA1cl+QNq6z3W8Bnh20rqOIp4FvA/KxrkdQe/Y4AtgInqupk\n",
       "VS0B+4EdK6x3A50B4c+uoa06PA0kaap6DgoPbAZOdU2fBv5u9wpJNtPZsb8deAtQg7bVCzwK/OOk\n",
       "77+JpHa4s4pzk3yDfjubQUaM/wTwsaqqJIHvnsceeLT5JHu6JheranHQtheQW4F/CfzSrAuRtC58\n",
       "Dp4PgCQLwMI436BfAJzhheel5+l8k+/2ZmB/Z9/Pq4B3JlkasC0AVbVn8JIvTFX8KfCns65D0vrU\n",
       "fDFePD+d5MZRt9kvAA4DVyXZAjwBXAtct6yoH+oq6A+AP6qqA0k29WsrSZqdngFQVeeS7AIOAnPA\n",
       "vqo6mmRns3zvsG3HV7okaRSpGvhU/WQKSKqqvP9dkoYwjn2nTwJLUksZAJLUUgaAJLWUASBJLWUA\n",
       "SFJLGQCS1FIGgCS1lAEgSS1lAEhSSxkAktRSBoAktZQBIEktZQBIUksZAJLUUgaAJLWUASBJLdU3\n",
       "AJJsT3IsyfEku1dYviPJQ0mOJPl8krd3LTuZ5OFm2QPjLl6StHY9RwRLMgc8DmyjM8j7IeC67qEd\n",
       "k1xSVd9qXv84cFtVXdlMfxl4c1U93eM9HBFMkoY0jRHBtgInqupkVS0B+4Ed3Suc3/k3LgW+vrzO\n",
       "UQqUJE1GvwDYDJzqmj7dzHuBJO9JchS4E/hI16IC7klyOMn1oxYrSRqfTX2WDzRifFXdDtye5KeB\n",
       "TwM/0iy6pqqeTHI5cHeSY1V17/L2SfZ0TS5W1eIg7ytJbZFkAVgY5zb7BcAZYL5rep7OUcCKqure\n",
       "JJuS/M2qeqqqnmzmn01yG51TSi8KgKraM3TlktQizRfjxfPTSW4cdZv9TgEdBq5KsiXJRcC1wIHu\n",
       "FZK8Pkma129qCn0qycVJLmvmXwK8A3hk1IIlSePR8wigqs4l2QUcBOaAfVV1NMnOZvle4L3AB5Ms\n",
       "Ac8C72uaXwHc2mTDJuDmqrprMh9DkjSsnreBTqUAbwOVpKFN4zZQSdIFygCQpJYyACSppQwASWop\n",
       "A0CSWsoAkKSWMgAkqaUMAElqKQNAklrKAJCkljIAJKmlDABJaikDQJJaygCQpJYyACSppfoGQJLt\n",
       "SY4lOZ5k9wrLdyR5KMmRJJ9P8vZB20qSZqfngDBJ5oDHgW10xgc+BFxXVUe71rmkqr7VvP5x4Laq\n",
       "unKQtk0bB4SRpCFNY0CYrcCJqjpZVUvAfmBH9wrnd/6NS4GvD9pWkjQ7/QJgM3Cqa/p0M+8Fkrwn\n",
       "yVHgTuAjw7SVJM1GvwAYaMDgqrq9qt4A/CPg02lGgpckrV+b+iw/A8x3Tc/T+Sa/oqq6N8km4JXN\n",
       "egO1TbKna3Kxqhb71CVJrZJkAVgY6zb7XATeROdC7s8BTwAP8OKLwK8HvlRVleRNwP+oqtcP0rZp\n",
       "70VgSRrSOPadPY8Aqupckl3AQWAO2FdVR5PsbJbvBd4LfDDJEvAs8L5ebUcpVpI0Pj2PAKZSgEcA\n",
       "kjS0adwGKkm6QBkAktRSBoAktZQBIEktZQBIUksZAJLUUgaAJLWUASBJLWUASFJLGQCS1FIGgCS1\n",
       "lAEgSS1lAEhSSxkAktRSBoAktZQBIEkt1TcAkmxPcizJ8SS7V1j+/iQPJXk4yZ8l+Ttdy042848k\n",
       "eWDcxUuS1q7nkJBJ5oCbgG10Bog/lOTAsqEdvwT8TFU9k2Q78HvAW5tlBSxU1dPjL12SNIp+RwBb\n",
       "gRNVdbKqloD9wI7uFarqvqp6ppm8H3jNsm043KMkrUP9AmAzcKpr+nQzbzW/CNzRNV3APUkOJ7l+\n",
       "bSVKkiah5ykgOjvwgST5WeBDwDVds6+pqieTXA7cneRYVd27hjolSWPWLwDOAPNd0/N0jgJeoLnw\n",
       "+ylge1V94/z8qnqy+ftsktvonFJ6UQAk2dM1uVhViwPWL0mtkGQBWBjrNqtW/5KfZBPwOPBzwBPA\n",
       "A8B13ReBk7wW+GPgA1X1513zLwbmquqbSS4B7gI+XlV3LXuPqiqvE0jSEMax7+x5BFBV55LsAg4C\n",
       "c8C+qjqaZGezfC/wG8ArgE8mAViqqq3AFcCtzbxNwM3Ld/6SpNnpeQQwlQI8ApCkoY1j3+mTwJLU\n",
       "UgaAJLWUASBJLWUASFJLGQCS1FIGgCS1lAEgSS1lAEhSSxkAktRSBoAktZQBIEktZQBIUksZAJLU\n",
       "UgaAJLWUASBJLWUASFJL9Q2AJNuTHEtyPMnuFZa/P8lDSR5O8mfN+MADtZUkzU6/MYHn6IwJvI3O\n",
       "APGHePGYwH8PeKyqnkmyHdhTVW8dpG3T3hHBJGlI0xgRbCtwoqpOVtUSsB/Y0b1CVd1XVc80k/cD\n",
       "rxm0rSRpdvoFwGbgVNf06Wbean4RuGONbSVJU7Spz/KBR4xP8rPAh4Br1tB2T9fkYlUtDtpWktog\n",
       "yQKwMM5t9guAM8B81/Q8nW/yL9Bc+P0UsL2qvjFMW4Cq2jNgvZLUSs0X48Xz00luHHWb/U4BHQau\n",
       "SrIlyUXAtcCB7hWSvBa4FfhAVZ0Ypq0kaXZ6HgFU1bkku4CDwBywr6qOJtnZLN8L/AbwCuCTSQCW\n",
       "qmrram0n+FkkSUPoeRvoVArwNlBJGto0bgOVJF2gDABJaikDQJJaygCQpJYyACSppQwASWopA0CS\n",
       "WsoAkKSWMgAkqaUMAElqKQNAklrKAJCkljIAJKmlDABJaikDQJJaygCQpJbqGwBJtic5luR4kt0r\n",
       "LP/RJPcl+U6SX1m27GSSh5McSfLAOAuXJI2m55CQSeaAm4BtdAZ5P5TkwLKhHZ8CbgDes8ImClio\n",
       "qqfHVK8kaUz6HQFsBU5U1cmqWgL2Azu6V6iqs1V1GFhaZRsO9yhJ61C/ANgMnOqaPt3MG1QB9yQ5\n",
       "nOT6YYuTJE1Oz1NAdHbgo7imqp5Mcjlwd5JjVXXv8pWS7OmaXKyqxRHfV5IuKEkWgIVxbrNfAJwB\n",
       "5rum5+kcBQykqp5s/j6b5DY6p5ReFABVtWfQbUpSGzVfjBfPTye5cdRt9jsFdBi4KsmWJBcB1wIH\n",
       "Vln3Bef6k1yc5LLm9SXAO4BHRqxXkjQmPY8Aqupckl3AQWAO2FdVR5PsbJbvTXIFcAj4PuC5JB8F\n",
       "rga+H7g1yfn3ubmq7prcR5EkDSNVo57mH7GApKrKO4UkaQjj2Hf6JLAktZQBIEktZQBIUksZAJLU\n",
       "UgaAJLWUASBJLWUASFJLGQCS1FIGgCS1lAEgSS1lAEhSSxkAktRSBoAktZQBIEktZQBIUksZAJLU\n",
       "Un0DIMn2JMeSHE+ye4XlP5rkviTfSfIrw7SVJM1OzxHBkswBjwPb6AwQfwi4rqqOdq1zOfCDwHuA\n",
       "b1TVbw/atlnPEcEkaUjTGBFsK3Ciqk5W1RKwH9jRvUJVna2qw8DSsG0lSbPTLwA2A6e6pk838wYx\n",
       "SltJ0oRt6rN8lBHjB26bZE/X5GJVLY7wvi/e/sczyueQpJmoG58/xZNkAVgY5/b7BcAZYL5rep7O\n",
       "N/lBDNy2qvYMuM016e5ESdqImi/Gi+enk9w46jb7nQI6DFyVZEuSi4BrgQOrrLt8JztMW0nSlPU8\n",
       "Aqiqc0l2AQeBOWBfVR1NsrNZvjfJFXTu8Pk+4LkkHwWurqpnV2o7yQ8jSRpcz9tAp1KAt4FK0tCm\n",
       "cRuoJOkCZQBIUksZAJLUUgaAJLWUASBJLWUASFJLGQCS1FIGgCS1lAEgSS1lAEhSSxkAktRSBoAk\n",
       "tZQBIEktZQBIUksZAJLUUn0DIMn2JMeSHE+ye5V1/mOz/KEkb+yafzLJw0mOJHlgnIVLkkbTMwCS\n",
       "zAE3AduBq4Hrkrxh2TrvAq6sqquAXwI+2bW4gIWqemNVbR1r5RegZtBnYV90sy+eZ1+MV78jgK3A\n",
       "iao6WVVLwH5gx7J13g38IUBV3Q+8PMkPdC13tK/BLcy6gHVkYdYFrCMLsy5gHVmYdQEXkn4BsBk4\n",
       "1TV9upk36DoF3JPkcJLrRylUkjRePQeFp7MDH8Rq3/J/qqqeSHI5cHeSY1V17+DlSZImpV8AnAHm\n",
       "u6bn6XzD77XOa5p5VNUTzd9nk9xG55TSiwIgyWxHpl9Hktw46xrWC/viefbF8+yL8ekXAIeBq5Js\n",
       "AZ4ArgWuW7bOAWAXsD/JW4G/qqq/THIxMFdV30xyCfAO4OPL32DUUe0lSWvTMwCq6lySXcBBYA7Y\n",
       "V1VHk+xslu+tqjuSvCvJCeBbwD9rml8B3Jrk/PvcXFV3TeqDSJKGkyrPvkhSG03sSeAkfyPJ/Uke\n",
       "TPJYkn/XzP/3SY42D43dmuRlq7T/1SSPJnkkyWeSvHRStU5aj774N00/PJjkc0nmV2nf92G8jWKU\n",
       "vkgyn+RPmp+LLyb5yPQ/wfiM+nPRrDvXPGj5R9OrfPzG8H/k5UluafYtjzWnozekMfTF4PvOqprY\n",
       "H+Di5u9NwJ8DPwX8feAlzfzfBH5zhXZbgC8BL22m/zvw85OsddJ/VumLy7qW3wD8/grt5oATTZ98\n",
       "D/Ag8IZZf54Z9cUVwE80ry8FHm9rX3Qt/2XgZuDArD/LLPuCzrNIH+pq/7JZf55Z9MWw+86J/i6g\n",
       "qvp28/IiOjuyp6vq7qp6rpl/P527hpb7v8AScHGSTcDFNHcWbVSr9MU3u1a5FPj6Ck0HeRhvQ1lr\n",
       "X1TVV6vqweb1s8BR4NUTLneiRvi5IMlrgHcBv88F8MDlWvuiOYvw01X1n5rtnKuqZyZd7ySN8HMx\n",
       "1L5zogGQ5CVJHgT+EviTqnps2SofAu5Y3q6qngZ+G/gKnbuP/qqq7plkrZO2Wl8k+bdJvgL8PJ0j\n",
       "ouUGeRhvQxmhL7q3sQV4I50vERvWiH3xH4B/BTy3yvINZYS+eB1wNskfJPlCkk81dyFuWGvti2H3\n",
       "nZM+Aniuqn6Czrf8n0nX7/FI8uvA/6uqzyxvl+T1wL+gczjzauDSJO+fZK2TtlpfVNWvV9Vrgf9M\n",
       "5z/0i5pOrcgpGaEvAEhyKXAL8NHmSGDDWmtfJPmHwNeq6ggXwLd/GOnnYhPwJuB3q+pNdO5G/NhU\n",
       "ip6QEX4uhtp3TuXXQTeHY/8L+EmAJL9A59B1tcJ+Evg/VfVUVZ0DbgXeNoVSJ255X3T5DPCWFZoM\n",
       "8jDehrSGviDJ9wD/E/ivVXX7ZCucnjX0xduAdyf5MvDfgLcn+S+TrXI61tAXp4HTVXWomb6FTiBs\n",
       "eGvoi6H2nZO8C+hVSV7evP5eOhd/jyTZTuewdUdVfWeV5seAtyb53iQBtgHLTx9tGD364squ1XYA\n",
       "R1Zo/t2H8ZJcROdhvAOTrnlSRumL5mdhH/BYVX1iGvVO0ih9UVW/VlXzVfU64H3AH1fVB6dR9ySM\n",
       "2BdfBU4l+eFm1jbg0QmXPDEj7i+G2nf2exJ4FH8L+MMkL6ETNJ+uqs8lOU7nwsbdnfq4r6r+eZJX\n",
       "A5+qqn9QVQ8132YO0zm/+QXg9yZY66St1he3JPkR4K+BvwA+DLCsL1Z8GG82H2Ms1twXwDXAB4CH\n",
       "k5z/4f/Vqvrs1D/FeIzSF8tt9FOFo/bFDcDNzZekv+D5B1I3olH2F0PtO30QTJJayiEhJamlDABJ\n",
       "aikDQJJaygCQpJYyACSppQwASWopA0CSWsoAkKSW+v8HoPFN/YKF2QAAAABJRU5ErkJggg==\n"
      ],
      "text/plain": [
       "<matplotlib.figure.Figure at 0x7997670>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "time.sleep(1)\n",
    "poppy.l_shoulder_x.goal_position = 120\n",
    "poppy.r_shoulder_x.goal_position = -120\n",
    "pos = []\n",
    "pos2 = []\n",
    "t = []\n",
    "t2 = []\n",
    "t0 = time.time()\n",
    "while time.time()-t0 <3:\n",
    "    t_simu = poppy.current_simulation_time\n",
    "    time.sleep(0.01)\n",
    "    if poppy.current_simulation_time != t_simu:\n",
    "        \n",
    "        pos.append(poppy.l_shoulder_x.present_position - poppy.l_shoulder_x.goal_position)\n",
    "        t.append(poppy.current_simulation_time)\n",
    "        pos2.append(poppy.r_shoulder_x.present_position - poppy.r_shoulder_x.goal_position)\n",
    "        t2.append(poppy.current_simulation_time)\n",
    "    \n",
    "print poppy.r_shoulder_x.present_position - poppy.r_shoulder_x.goal_position\n",
    "print poppy.l_shoulder_x.present_position - poppy.l_shoulder_x.goal_position\n",
    "plot(t,pos)\n",
    "plot(t2,pos2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}