{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#QUICKSTART Heol-humanoid"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Vérifier que le robot a bien été installé par un : $ pip install heol-humanoid"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import pip"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "heol-humanoid est bien installé\n"
     ]
    }
   ],
   "source": [
    "if 'heol-humanoid' in str(pip.get_installed_distributions()):\n",
    "    print('heol-humanoid est bien installé')\n",
    "else:\n",
    "    print(\"Attention le package heol-humanoid n'est pas installé. Exécutez pip install heol-humanoid\")\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Importer le package Heol"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "from heol_humanoid import HeolHumanoid"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Créer le robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pour un robot dans le simulateur :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false,
    "scrolled": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "<DxlMotor name=l_hip_motor_z id=42 pos=0>\n",
      "<DxlMotor name=l_thigh_x id=43 pos=0>\n",
      "<DxlMotor name=l_ankle_x id=45 pos=0>\n",
      "<DxlMotor name=r_hip_y id=51 pos=0>\n",
      "<DxlMotor name=r_hip_motor_z id=52 pos=0>\n",
      "<DxlMotor name=r_thigh_x id=53 pos=0>\n",
      "<DxlMotor name=r_ankle_x id=55 pos=0>\n",
      "<DxlMotor name=spine_z id=11 pos=0>\n",
      "<DxlMotor name=chest_x id=12 pos=0>\n",
      "<DxlMotor name=l_shoulder_x id=31 pos=0>\n",
      "<DxlMotor name=l_upper_arm_z id=33 pos=0>\n",
      "<DxlMotor name=r_shoulder_x id=21 pos=0>\n",
      "<DxlMotor name=r_upper_arm_z id=23 pos=0>\n",
      "<DxlMotor name=head_z id=13 pos=0>\n",
      "<DxlMotor name=r_shoulder_motor_y id=22 pos=0>\n",
      "<DxlMotor name=l_shoulder_motor_y id=32 pos=0>\n"
     ]
    }
   ],
   "source": [
    "heol = HeolHumanoid(simulator='vrep')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pour un robot réel :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "heol = HeolHumanoid()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "A partir de ce moment le robot physique et le robot software sont connectés. Tous les commandes que tu lances sur l'objet heol sont lancés sur le robot."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Utiliser le robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Affiche un ensemble d'infos sur le robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<Robot motors=[<DxlMotor name=l_hip_motor_z id=42 pos=-0.0>, <DxlMotor name=l_upper_arm_z id=33 pos=0.0>, <DxlMotor name=l_shoulder_motor_y id=32 pos=-0.4>, <DxlMotor name=r_knee_x id=54 pos=-0.2>, <DxlMotor name=r_ankle_x id=55 pos=0.0>, <DxlMotor name=head_z id=13 pos=-0.0>, <DxlMotor name=r_hip_motor_z id=52 pos=-0.0>, <DxlMotor name=r_shoulder_x id=21 pos=0.0>, <DxlMotor name=l_thigh_x id=43 pos=0.0>, <DxlMotor name=r_forearm_y id=24 pos=0.1>, <DxlMotor name=r_upper_arm_z id=23 pos=-0.0>, <DxlMotor name=r_hip_y id=51 pos=-0.0>, <DxlMotor name=l_hip_y id=41 pos=-0.0>, <DxlMotor name=l_foot_y id=46 pos=0.0>, <DxlMotor name=chest_x id=12 pos=-0.0>, <DxlMotor name=l_ankle_x id=45 pos=-0.0>, <DxlMotor name=l_forearm_y id=34 pos=-0.0>, <DxlMotor name=r_shoulder_motor_y id=22 pos=0.0>, <DxlMotor name=r_thigh_x id=53 pos=0.0>, <DxlMotor name=r_foot_y id=56 pos=-0.0>, <DxlMotor name=l_knee_x id=44 pos=0.2>, <DxlMotor name=spine_z id=11 pos=0.0>, <DxlMotor name=l_shoulder_x id=31 pos=-0.0>]>"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "heol"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Mets tous les moteurs en non-compliant"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for m in heol.motors:\n",
    "    m.compliant = False"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Mets tous les moteurs à 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for m in heol.motors:\n",
    "    m.goal_position = 0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Renvoi le type de comportement de la méthode goto_position. Doit être à 'minjerk' pour que la méthode goto_position fonctionne dans le simulateur."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "['minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk',\n",
       " 'minjerk']"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "[m.goto_behavior for m in heol.motors]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Donne tous les noms des moteurs :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[<DxlMotor name=l_hip_motor_z id=42 pos=-0.0>,\n",
       " <DxlMotor name=l_upper_arm_z id=33 pos=0.0>,\n",
       " <DxlMotor name=l_shoulder_motor_y id=32 pos=1.2>,\n",
       " <DxlMotor name=r_knee_x id=54 pos=0.6>,\n",
       " <DxlMotor name=r_ankle_x id=55 pos=0.0>,\n",
       " <DxlMotor name=head_z id=13 pos=-0.0>,\n",
       " <DxlMotor name=r_hip_motor_z id=52 pos=-0.0>,\n",
       " <DxlMotor name=r_shoulder_x id=21 pos=0.0>,\n",
       " <DxlMotor name=l_thigh_x id=43 pos=0.0>,\n",
       " <DxlMotor name=r_forearm_y id=24 pos=0.3>,\n",
       " <DxlMotor name=r_upper_arm_z id=23 pos=-0.0>,\n",
       " <DxlMotor name=r_hip_y id=51 pos=-0.0>,\n",
       " <DxlMotor name=l_hip_y id=41 pos=-0.0>,\n",
       " <DxlMotor name=l_foot_y id=46 pos=0.0>,\n",
       " <DxlMotor name=chest_x id=12 pos=-0.0>,\n",
       " <DxlMotor name=l_ankle_x id=45 pos=-0.0>,\n",
       " <DxlMotor name=l_forearm_y id=34 pos=-0.0>,\n",
       " <DxlMotor name=r_shoulder_motor_y id=22 pos=-0.0>,\n",
       " <DxlMotor name=r_thigh_x id=53 pos=0.1>,\n",
       " <DxlMotor name=r_foot_y id=56 pos=-0.0>,\n",
       " <DxlMotor name=l_knee_x id=44 pos=0.6>,\n",
       " <DxlMotor name=spine_z id=11 pos=0.0>,\n",
       " <DxlMotor name=l_shoulder_x id=31 pos=-0.0>]"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "heol.motors"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Met les bras à l'horizontale :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "heol.l_shoulder_motor_y.goto_position(-90,1)\n",
    "heol.r_shoulder_motor_y.goto_position(90,1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remet les bras le long du corps:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "heol.l_shoulder_motor_y.goto_position(0,0.5)\n",
    "heol.r_shoulder_motor_y.goto_position(0,0.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remet tous les moteurs à zéro :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.l_hip_motor_z.goto_position(0,2)\n",
    "heol.l_upper_arm_z.goto_position(0,2)\n",
    "heol.l_shoulder_motor_y.goto_position(0,2)\n",
    "heol.r_knee_x.goto_position(0,2)\n",
    "heol.r_ankle_x.goto_position(0,2)\n",
    "heol.head_z.goto_position(0,2)\n",
    "heol.r_hip_motor_z.goto_position(0,2)\n",
    "heol.r_shoulder_x.goto_position(0,2)\n",
    "heol.l_thigh_x.goto_position(0,2)\n",
    "heol.r_forearm_y.goto_position(0,2)\n",
    "heol.r_upper_arm_z.goto_position(0,2)\n",
    "heol.r_hip_y.goto_position(0,2)\n",
    "heol.l_hip_y.goto_position(0,2)\n",
    "heol.l_foot_y.goto_position(0,2)\n",
    "heol.chest_x.goto_position(0,2)\n",
    "heol.l_ankle_x.goto_position(0,2)\n",
    "heol.l_forearm_y.goto_position(0,2) \n",
    "heol.r_shoulder_motor_y.goto_position(0,2)\n",
    "heol.r_thigh_x.goto_position(0,2)\n",
    "heol.r_foot_y.goto_position(0,2),\n",
    "heol.l_knee_x.goto_position(0,2)\n",
    "heol.spine_z.goto_position(0,2)\n",
    "heol.l_shoulder_x.goto_position(0,2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remet tous les moteurs à zéro (en position de départ) avec une boucle :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "for m in heol.motors:\n",
    "    m.goto_position(0,1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Plie les genoux : "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "heol.r_knee_x.goto_position(10,2)\n",
    "heol.l_knee_x.goto_position(10,2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "heol.r_knee_x.goto_position(0,2)\n",
    "heol.l_knee_x.goto_position(0,2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Fait un salut :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.l_shoulder_x.goto_position(-30,2)\n",
    "heol.r_shoulder_x.goto_position(-30,2)\n",
    "\n",
    "heol.l_shoulder_motor_y.goto_position(-120,2)\n",
    "heol.r_shoulder_motor_y.goto_position(120,2)\n",
    "heol.l_upper_arm_z.goto_position(150,2)\n",
    "heol.r_upper_arm_z.goto_position(-150,2)\n",
    "\n",
    "\n",
    "heol.r_knee_x.goto_position(-60,2)\n",
    "heol.l_knee_x.goto_position(-60,2)\n",
    "\n",
    "heol.r_ankle_x.goto_position(-40,2)\n",
    "heol.l_ankle_x.goto_position(-40,2)\n",
    "\n",
    "heol.chest_x.goto_position(30,2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for m in heol.motors:\n",
    "    m.goto_position(0,2)    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Se met en équilibre sur une jambe :"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.r_foot_y.goto_position(20,2)\n",
    "heol.r_hip_y.goto_position(15,2)\n",
    "heol.r_shoulder_motor_y.goto_position(90,2,wait=True)\n",
    "heol.l_thigh_x.goto_position(-60,2)\n",
    "heol.l_knee_x.goto_position(60,2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for m in heol.motors:\n",
    "    m.goto_position(0,1)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remet la simulation à 0 (reprend la position de départ):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.reset_simulation()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Lancer des primitives"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Le mettre debout"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.stand.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Faire un signe de la main"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.wave.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true
   },
   "source": [
    "## Enregistrer des mouvements"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Importe les outils nécessaires pour enregistrer et jouer des mouvements"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pypot.primitive.move import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "rec = MoveRecorder(heol, 50, heol.l_arm) # créer un enregistreur à 50Hz des moteurs du bras gauche du robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Passe les moteurs en compliant pour pouvoir les bouger"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "for m in heol.l_arm:\n",
    "    m.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Lancer l'enregistrement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "rec.start() # tous les mouvements sont enregistrés à partir de ce moment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "rec.stop() # arrête l'enregistrement"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Rejouer un mouvement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "move = rec.move # recupere le dernier mouvement enregistre"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Creer un player "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "player = MovePlayer(heol, move) "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "player.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Arreter le mouvement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "player.stop()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Sauvegarder le mouvement sur le disque"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "with open('monmove.move', 'w') as file:\n",
    "    move.save(file)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Recharger un autre mouvement"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "with open('monmove.move') as file:\n",
    "    monnouveaumove = Move.load(file)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "heol.close()"
   ]
  },
  {
   "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
}
