{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Si vous utilisez ipython 3.x, cette ligne ne sert à rien mais ne gêne pas !\n",
    "# Avec ipython 2.7, cette ligne est nécessaire pour la syntaxe 'fonction print'.\n",
    "from __future__ import print_function, division "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Il faut installer les modules Python *pypot*, *poppy-creature* et *poppy-torso* :\n",
    "- votre PC doit être connecté sur internet\n",
    "- **Windows** : ouvrir la fenêtre de commande et taper `pip install pypot poppy-creature poppy-torso`\n",
    "- **Mac OS X** : ouvrir un terminal et taper `sudo pip install pypot poppy-creature poppy-torso`\n",
    "- **GNU/Linux** : ouvrir un terminal et taper `sudo pip install pypot poppy-creature poppy-torso` (`pip` ou `pip3` en fonction de votre installation)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pypot.vrep import from_vrep\n",
    "from poppy.creatures import PoppyTorso"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Cette ligne crée un objet de type PoppyTorso et le connecte sur le simulateur V-Rep :\n",
    "poppy = PoppyTorso(simulator='vrep')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<DxlMotor name=r_elbow_y id=54 pos=0.0>,\n",
       " <DxlMotor name=r_arm_z id=53 pos=-0.0>,\n",
       " <DxlMotor name=l_arm_z id=43 pos=-0.0>,\n",
       " <DxlMotor name=head_z id=36 pos=0.0>,\n",
       " <DxlMotor name=r_shoulder_y id=51 pos=1.4000000000000057>,\n",
       " <DxlMotor name=r_shoulder_x id=52 pos=0.0>,\n",
       " <DxlMotor name=bust_y id=34 pos=0.3>,\n",
       " <DxlMotor name=bust_x id=35 pos=0.0>,\n",
       " <DxlMotor name=l_shoulder_x id=42 pos=-2.4000000000000057>,\n",
       " <DxlMotor name=abs_z id=33 pos=0.0>,\n",
       " <DxlMotor name=head_y id=37 pos=-3.3000000000000007>,\n",
       " <DxlMotor name=l_elbow_y id=44 pos=90.0>,\n",
       " <DxlMotor name=l_shoulder_y id=41 pos=-90>]"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Echo de la liste des moteurs:\n",
    "poppy.motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On peut accéder à un moteur directement par son nom :"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": false
   },
   "source": [
    "poppy.l_shoulder_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#On peut accéder à la position courante (en degrés) d'un moteur :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0.0"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "poppy.l_shoulder_y.present_position"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "La liste par compréhension de toutes les positions des moteurs :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[0.0,\n",
       " -0.0,\n",
       " -0.0,\n",
       " 0.0,\n",
       " 1.0,\n",
       " 0.0,\n",
       " 0.20000000000000001,\n",
       " 0.0,\n",
       " -1.7000000000000028,\n",
       " 0.0,\n",
       " -2.1999999999999993,\n",
       " 0.0,\n",
       " 0.0]"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[m.present_position for m in poppy.motors]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Le dictionnaire en compréhension des paires (nom_moteur, posiition_moteur) :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'abs_z': 0.0,\n",
       " 'bust_x': 0.0,\n",
       " 'bust_y': 0.20000000000000001,\n",
       " 'head_y': -2.1999999999999993,\n",
       " 'head_z': 0.0,\n",
       " 'l_arm_z': -0.0,\n",
       " 'l_elbow_y': 0.0,\n",
       " 'l_shoulder_x': -1.7000000000000028,\n",
       " 'l_shoulder_y': 0.0,\n",
       " 'r_arm_z': -0.0,\n",
       " 'r_elbow_y': 0.0,\n",
       " 'r_shoulder_x': 0.0,\n",
       " 'r_shoulder_y': 1.0}"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "{m.name: m.present_position for m in poppy.motors}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "À tout moment, vous pouvez ré-initialiser le simulateur :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.reset_simulation()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Maintenant on fait tourner la tête de -45° en 2 secondes :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.head_z.goto_position(-45, 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "goto_position peut prendre un argument wait : ceci permet d'enchainer des mouvements. (wait=True va attendre que le mouvement soit fini avant dexécuter la ligne suivante; wait=False envoie la nouvelle position et passe directement à la ligne suivante:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.head_z.goto_position(45, 2, wait=False)\n",
    "poppy.head_y.goto_position(-30, 2, wait=True)\n",
    "poppy.head_z.goto_position(0, 2, wait=True)\n",
    "poppy.head_y.goto_position(20, 1, wait=True)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Interaction avec des object"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On va utiliser l'interface *io* du robot pour interagir avec des objets."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "io = poppy._controllers[0].io"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Créons un cube devant Poppy :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "ename": "TypeError",
     "evalue": "'str' does not support the buffer interface",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mTypeError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[1;32m<ipython-input-22-7527116d9aa7>\u001b[0m in \u001b[0;36m<module>\u001b[1;34m()\u001b[0m\n\u001b[0;32m      3\u001b[0m \u001b[0msizes\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;33m[\u001b[0m\u001b[1;36m0.1\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;36m0.1\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;36m0.1\u001b[0m\u001b[1;33m]\u001b[0m \u001b[1;31m# in meters\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m      4\u001b[0m \u001b[0mmass\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;36m0.1\u001b[0m \u001b[1;31m# in kg\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m----> 5\u001b[1;33m \u001b[0mio\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0madd_cube\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mname\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mposition\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0msizes\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmass\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m",
      "\u001b[1;32mC:\\Users\\Jon\\Anaconda3\\lib\\site-packages\\pypot\\vrep\\io.py\u001b[0m in \u001b[0;36madd_cube\u001b[1;34m(self, name, position, sizes, mass)\u001b[0m\n\u001b[0;32m    228\u001b[0m     \u001b[1;32mdef\u001b[0m \u001b[0madd_cube\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mself\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mname\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mposition\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0msizes\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmass\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    229\u001b[0m         \u001b[1;34m\"\"\" Add Cube \"\"\"\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 230\u001b[1;33m         \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_create_pure_shape\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;36m239\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0msizes\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmass\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    231\u001b[0m         \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mset_object_position\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m\"Cuboid\"\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mposition\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    232\u001b[0m         \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mchange_object_name\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m\"Cuboid\"\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mname\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Users\\Jon\\Anaconda3\\lib\\site-packages\\pypot\\vrep\\io.py\u001b[0m in \u001b[0;36m_create_pure_shape\u001b[1;34m(self, primitive_type, options, sizes, mass, precision)\u001b[0m\n\u001b[0;32m    261\u001b[0m         \u001b[1;34m\"\"\" Create Pure Shape \"\"\"\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    262\u001b[0m         \u001b[0mlua_code\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;34m\"simCreatePureShape({}, {}, {{{}, {}, {}}}, {}, {{{}, {}}})\"\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mformat\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mprimitive_type\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0moptions\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0msizes\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0msizes\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m1\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0msizes\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m2\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmass\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mprecision\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m0\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mprecision\u001b[0m\u001b[1;33m[\u001b[0m\u001b[1;36m1\u001b[0m\u001b[1;33m]\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 263\u001b[1;33m         \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0m_inject_lua_code\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mlua_code\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    264\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    265\u001b[0m     \u001b[1;32mdef\u001b[0m \u001b[0m_inject_lua_code\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mself\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mlua_code\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;32mC:\\Users\\Jon\\Anaconda3\\lib\\site-packages\\pypot\\vrep\\io.py\u001b[0m in \u001b[0;36m_inject_lua_code\u001b[1;34m(self, lua_code)\u001b[0m\n\u001b[0;32m    265\u001b[0m     \u001b[1;32mdef\u001b[0m \u001b[0m_inject_lua_code\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mself\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mlua_code\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m:\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    266\u001b[0m         \u001b[1;34m\"\"\" Sends raw lua code and evaluate it wihtout any checking! \"\"\"\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[1;32m--> 267\u001b[1;33m         \u001b[0mmsg\u001b[0m \u001b[1;33m=\u001b[0m \u001b[1;33m(\u001b[0m\u001b[0mctypes\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mc_ubyte\u001b[0m \u001b[1;33m*\u001b[0m \u001b[0mlen\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mlua_code\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mfrom_buffer_copy\u001b[0m\u001b[1;33m(\u001b[0m\u001b[0mlua_code\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0m\u001b[0;32m    268\u001b[0m         \u001b[0mself\u001b[0m\u001b[1;33m.\u001b[0m\u001b[0mcall_remote_api\u001b[0m\u001b[1;33m(\u001b[0m\u001b[1;34m'simxWriteStringStream'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[1;34m'my_lua_code'\u001b[0m\u001b[1;33m,\u001b[0m \u001b[0mmsg\u001b[0m\u001b[1;33m)\u001b[0m\u001b[1;33m\u001b[0m\u001b[0m\n\u001b[0;32m    269\u001b[0m \u001b[1;33m\u001b[0m\u001b[0m\n",
      "\u001b[1;31mTypeError\u001b[0m: 'str' does not support the buffer interface"
     ]
    }
   ],
   "source": [
    "name = 'cube'\n",
    "position = [0., -0.2, 0.8] # X, Y, Z\n",
    "sizes = [0.1, 0.1, 0.1] # in meters\n",
    "mass = 0.1 # in kg\n",
    "io.add_cube(name, position, sizes, mass)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Faisons bouger le bras gauche de Poppy pour entrer en contact avec le cube :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.goto_position(-30,3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "poppy.l_arm_z.goto_position(0,3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "io.get_object_position('cube')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "io.get_object_orientation('cube')"
   ]
  }
 ],
 "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.4.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}