{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false,
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "WARNING:pypot.robot.config:Limits of 'head_z' changed from (-150.0, 0.15) to [-180, 0]\n",
      "WARNING:pypot.robot.config:Limits of 'l_wrist_x' changed from (-150.0, 150.0) to [-180, 180]\n",
      "WARNING:pypot.robot.config:Limits of 'r_wrist_x' changed from (-150.0, 150.0) to [-180, 180]\n"
     ]
    }
   ],
   "source": [
    "from time import sleep\n",
    "from pypot.robot import from_json\n",
    "poppy = from_json(\"/home/poppy/poppy_torso2.json\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Pour bloquer Poppy\n",
    "for m in poppy.motors:\n",
    "    m.compliant=False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Pour libérer Poppy\n",
    "# /!\\ il faut tenir Poppy pour éviter que ses bras tombent et se congnent contre la table\n",
    "for m in poppy.motors:\n",
    "    m.compliant=True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "# Remet Poppy dans la position de base\n",
    "# /!\\ Le montage du buste n'est pas bon sur le premier poppy, il est légèrement tourné vers la gauche\n",
    "speed=3\n",
    "poppy.abs_z.goto_position(0,speed)  # <<< valeur à modifier pour remettre Poppy droit\n",
    "poppy.bust_x.goto_position(0,speed)\n",
    "poppy.bust_y.goto_position(0,speed)\n",
    "poppy.l_shoulder_y.goto_position(0,speed)\n",
    "poppy.l_shoulder_x.goto_position(0,speed)\n",
    "poppy.l_elbow_y.goto_position(0,speed)\n",
    "poppy.l_arm_z.goto_position(0,speed)\n",
    "poppy.r_shoulder_y.goto_position(0,speed)\n",
    "poppy.r_shoulder_x.goto_position(0,speed)\n",
    "poppy.r_elbow_y.goto_position(0,speed)\n",
    "poppy.r_arm_z.goto_position(0,speed)\n",
    "poppy.head_z.goto_position(0,speed)\n",
    "poppy.head_y.goto_position(10,speed)\n",
    "poppy.l_wrist_x.goto_position(0,speed)\n",
    "poppy.r_wrist_x.goto_position(0,speed)\n",
    "poppy.r_hand_z.goto_position(60,speed) #0 position ouverte 60 posiion fermer \n",
    "poppy.l_hand_z.goto_position(0,speed)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#position démo\n",
    "\n",
    "poppy.l_shoulder_y.goto_position(-33,3)\n",
    "poppy.l_shoulder_x.goto_position(-15,3)\n",
    "poppy.l_arm_z.goto_position(-25,3)\n",
    "poppy.l_elbow_y.goto_position(44,3)\n",
    "poppy.l_wrist_x.goto_position(25,3)\n",
    "poppy.l_hand_z.goto_position(0,3)\n",
    "poppy.r_shoulder_y.goto_position(-33,3)\n",
    "poppy.r_shoulder_x.goto_position(20,3)\n",
    "poppy.r_arm_z.goto_position(25,3)\n",
    "poppy.r_elbow_y.goto_position(35,3)\n",
    "poppy.r_wrist_x.goto_position(33,3)\n",
    "poppy.r_hand_z.goto_position(60,3,wait=True)\n",
    "poppy.l_hand_z.goto_position(60,0.5)\n",
    "poppy.l_wrist_x.goto_position(90,1)\n",
    "poppy.r_wrist_x.goto_position(90,1)\n",
    "poppy.r_hand_z.goto_position(0,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(0,0.5)\n",
    "poppy.l_wrist_x.goto_position(-90,1)\n",
    "poppy.r_wrist_x.goto_position(-90,1)\n",
    "poppy.r_hand_z.goto_position(60,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(60,0.5)\n",
    "poppy.l_wrist_x.goto_position(90,1)\n",
    "poppy.r_wrist_x.goto_position(90,1)\n",
    "poppy.r_hand_z.goto_position(0,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(0,0.5)\n",
    "poppy.l_wrist_x.goto_position(90,1)\n",
    "poppy.r_wrist_x.goto_position(90,1)\n",
    "poppy.r_hand_z.goto_position(60,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(60,0.5)\n",
    "poppy.l_wrist_x.goto_position(-90,1)\n",
    "poppy.r_wrist_x.goto_position(-90,1)\n",
    "poppy.r_hand_z.goto_position(0,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(0,0.5)\n",
    "poppy.l_wrist_x.goto_position(90,1)\n",
    "poppy.r_wrist_x.goto_position(90,1)\n",
    "poppy.r_hand_z.goto_position(60,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(0,0.5)\n",
    "poppy.l_wrist_x.goto_position(-90,1)\n",
    "poppy.r_wrist_x.goto_position(-90,1)\n",
    "poppy.r_hand_z.goto_position(60,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(60,0.5)\n",
    "poppy.l_wrist_x.goto_position(90,1)\n",
    "poppy.r_wrist_x.goto_position(90,1)\n",
    "poppy.r_hand_z.goto_position(0,0.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(0,0.5)\n",
    "poppy.l_wrist_x.goto_position(-90,1)\n",
    "poppy.r_wrist_x.goto_position(-90,1)\n",
    "poppy.r_hand_z.goto_position(60,0.5,wait=True)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#positioner les verres\n",
    "poppy.l_shoulder_y.goto_position(22,1)\n",
    "poppy.l_shoulder_x.goto_position(-2,1)\n",
    "poppy.l_arm_z.goto_position(7,1)\n",
    "poppy.l_elbow_y.goto_position(13,1)\n",
    "poppy.l_wrist_x.goto_position(0,1)\n",
    "poppy.l_hand_z.goto_position(65,1)\n",
    "poppy.r_shoulder_y.goto_position(22,1)\n",
    "poppy.r_shoulder_x.goto_position(0,1)\n",
    "poppy.r_arm_z.goto_position(0,1)\n",
    "poppy.r_elbow_y.goto_position(13,1)\n",
    "poppy.r_wrist_x.goto_position(0,1)\n",
    "poppy.r_hand_z.goto_position(0,1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "#verser bouteille gauche dans verre droite\n",
    "\n",
    "poppy.l_elbow_y.goto_position(-5,0.5)\n",
    "poppy.l_shoulder_y.goto_position(-5,1)\n",
    "poppy.l_shoulder_x.goto_position(1.87,0.5)\n",
    "poppy.head_z.goto_position(20,1.5)\n",
    "poppy.head_y.goto_position(20,1.5,wait=True)\n",
    "poppy.l_hand_z.goto_position(65,1)\n",
    "poppy.r_hand_z.goto_position(0,1)\n",
    "poppy.r_elbow_y.goto_position(-7,0.5)\n",
    "poppy.r_shoulder_y.goto_position(-4,1)\n",
    "poppy.r_shoulder_x.goto_position(1.87,0.5)\n",
    "poppy.r_arm_z.goto_position(-5.41,0.5)\n",
    "poppy.l_arm_z.goto_position(5.41,0.5)\n",
    "poppy.l_wrist_x.goto_position(0.15,2)\n",
    "poppy.r_wrist_x.goto_position(0.15,2,wait=True)\n",
    "\n",
    "\n",
    "\n",
    "#position bras droit service\n",
    "poppy.r_hand_z.goto_position(50,0.5,wait=True)\n",
    "poppy.r_shoulder_y.goto_position(-18,1)\n",
    "poppy.r_shoulder_x.goto_position(-19,1)\n",
    "poppy.r_arm_z.goto_position(46,1)\n",
    "poppy.r_elbow_y.goto_position(0,1)\n",
    "poppy.r_wrist_x.goto_position(-30,1)\n",
    "poppy.head_z.goto_position(20,0.5)\n",
    "poppy.head_y.goto_position(20,0.5)\n",
    "poppy.l_hand_z.goto_position(2,0.5,wait=True)\n",
    "poppy.l_shoulder_y.goto_position(-20,1)\n",
    "poppy.l_shoulder_x.goto_position(21,1)\n",
    "poppy.l_arm_z.goto_position(-15,1)\n",
    "poppy.l_elbow_y.goto_position(10,1)\n",
    "poppy.l_wrist_x.goto_position(0,1)\n",
    "\n",
    "poppy.l_wrist_x.goto_position(40,1,wait=True)\n",
    "poppy.head_z.goto_position(0,2)\n",
    "poppy.l_elbow_y.goto_position(-2,1)\n",
    "poppy.head_y.goto_position(15,2)\n",
    "poppy.l_shoulder_y.goto_position(-59,3)\n",
    "poppy.l_shoulder_x.goto_position(42,3)\n",
    "poppy.l_arm_z.goto_position(-52,3,wait=True)\n",
    "poppy.l_wrist_x.goto_position(55,3,wait=True) \n",
    "poppy.l_wrist_x.goto_position(-10,3,sleep(1))\n",
    "poppy.l_wrist_x.goto_position(0,0.5,wait=True)\n",
    "\n",
    "\n",
    "poppy.head_z.goto_position(-20,0.5)\n",
    "poppy.head_y.goto_position(20,0.5)\n",
    "poppy.r_shoulder_y.goto_position(-3.56,1)\n",
    "poppy.r_shoulder_x.goto_position(1.87,1)\n",
    "poppy.r_arm_z.goto_position(-5.41,1)\n",
    "poppy.r_elbow_y.goto_position(-7,1)\n",
    "poppy.r_wrist_x.goto_position(0,1)\n",
    "poppy.l_shoulder_y.goto_position(-26,1)\n",
    "poppy.l_shoulder_x.goto_position(31,1)\n",
    "poppy.l_arm_z.goto_position(-5,1)\n",
    "poppy.l_elbow_y.goto_position(0,1,wait=True)\n",
    "\n",
    "#reposer bouteille main gauche\n",
    "poppy.l_shoulder_y.goto_position(-4,1)  \n",
    "poppy.l_shoulder_x.goto_position(1.87,1)\n",
    "poppy.l_arm_z.goto_position(6,1)\n",
    "poppy.l_elbow_y.goto_position(-7,1)\n",
    "poppy.l_wrist_x.goto_position(0.15,1)\n",
    "poppy.r_wrist_x.goto_position(0.15,1, wait=True)\n",
    "poppy.r_hand_z.goto_position(0,3)\n",
    "poppy.l_hand_z.goto_position(65,3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "poppy.l_shoulder_y.goto_position(-2,0.5)\n",
    "poppy.l_shoulder_x.goto_position(0,0.5)\n",
    "poppy.l_arm_z.goto_position(6.5,0.5)\n",
    "poppy.l_elbow_y.goto_position(-3,0.5)\n",
    "poppy.l_wrist_x.goto_position(0,0.5)\n",
    "poppy.r_shoulder_y.goto_position(-6.5,0.5)\n",
    "poppy.r_shoulder_x.goto_position(2,0.5)\n",
    "poppy.r_arm_z.goto_position(-5,0.5)\n",
    "poppy.r_elbow_y.goto_position(-6,0.5)\n",
    "poppy.r_wrist_x.goto_position(0,1,wait=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "#verser bouteille au bras droit dans verre gauche\n",
    "\n",
    "poppy.l_hand_z.goto_position(0,0.5)\n",
    "poppy.r_hand_z.goto_position(65,0.5,wait=True)\n",
    "\n",
    "#position bras gauche service\n",
    "poppy.l_shoulder_y.goto_position(-18,0.5)\n",
    "poppy.l_shoulder_x.goto_position(26,0.5)\n",
    "poppy.l_arm_z.goto_position(-50,0.5)\n",
    "poppy.l_elbow_y.goto_position(10,0.5)\n",
    "poppy.l_wrist_x.goto_position(-22,0.5)\n",
    "\n",
    "#position bras droit service\n",
    "poppy.r_shoulder_y.goto_position(-30,0.5)\n",
    "poppy.r_shoulder_x.goto_position(-37,0.5)\n",
    "poppy.r_arm_z.goto_position(25,0.5)\n",
    "poppy.r_elbow_y.goto_position(16,0.5,wait=True)\n",
    "poppy.r_wrist_x.goto_position(0,0.5)\n",
    "\n",
    "poppy.r_wrist_x.goto_position(65,1)\n",
    "poppy.r_shoulder_y.goto_position(-25,1,sleep(0.3))\n",
    "poppy.r_shoulder_x.goto_position(-35,1)\n",
    "poppy.r_elbow_y.goto_position(18,1)\n",
    "poppy.r_arm_z.goto_position(22,2,wait=True)\n",
    "poppy.r_wrist_x.goto_position(0,0.5,wait=True)\n",
    "poppy.l_shoulder_y.goto_position(0,1)\n",
    "poppy.l_shoulder_x.goto_position(0,1)\n",
    "poppy.l_arm_z.goto_position(7,1)\n",
    "poppy.l_elbow_y.goto_position(-8,1)\n",
    "poppy.l_wrist_x.goto_position(0,0.5)\n",
    "poppy.r_shoulder_y.goto_position(-5,0.5)\n",
    "poppy.r_shoulder_x.goto_position(2,0.5)\n",
    "poppy.r_arm_z.goto_position(-6.5,0.5)\n",
    "poppy.r_elbow_y.goto_position(-5,0.5)\n",
    "poppy.r_wrist_x.goto_position(0,0.7,wait=True)\n",
    "poppy.l_hand_z.goto_position(65,0.5)\n",
    "poppy.r_hand_z.goto_position(0,0.5)\n"
   ]
  },
  {
   "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.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}