{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#                          Mathematical method to let poppy vertical"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you want to move the motors of the leg, you can not do whatever you want, because Poppy can fall if it is not balance.\n",
    "So a very simple way to move the leg without any external perturbation (no wind, flat foor, no move of the upper body) is to let the upper body to the veticale of the ankle."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To do that, simple calculation of the angle of ankle, knee and hip can do the job."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Basically, the shin and the thigh form a triangle with a knee angle. So if you determine the angle of the knee, what you want is just to calcul the angle of the ankle and the hip to let Poppy to the verticale position."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "![alt text](./image/triangle.png)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To calcul the missing angle, we can use the sinus law and the Al-Kashi theorem. \n",
    "More information [here](http://en.wikipedia.org/wiki/Solution_of_triangles)."
   ]
  },
  {
   "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": [
    "%pylab inline\n",
    "from math import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 103,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "class leg_angle:\n",
    "    def __init__(self,knee=0):\n",
    "        # different length of poppy in cm\n",
    "        self.upper_body = 40.0\n",
    "        self.shin = 18.0\n",
    "        self.thigh = 18.0\n",
    "        # the angle of the knee\n",
    "        self.knee = radians(knee)\n",
    "        gamma = radians(180 - knee)\n",
    "        # Al-Kashi theorem to calcul the c side and the missing angle\n",
    "        c = sqrt(self.shin**2+self.thigh**2-2*self.shin*self.thigh*cos(gamma))\n",
    "        self.c = c\n",
    "        self.hip = -acos((self.thigh**2+c**2-self.shin**2)/(2*self.thigh*c))\n",
    "        self.ankle = -acos((self.shin**2+c**2-self.thigh**2)/(2*self.shin*c))\n",
    "        # The high of the leg and the foot gap\n",
    "        self.high = c\n",
    "        self.foot_gap = 0.0\n",
    "    def update_knee(self,knee):\n",
    "        self.knee = radians(knee)\n",
    "        gamma = radians(180 - knee)\n",
    "        # Al-Kashi theorem to calcul the c side\n",
    "        c = sqrt(self.shin**2+self.thigh**2-2*self.shin*self.thigh*cos(gamma))\n",
    "        self.c = c\n",
    "        self.hip = -acos((self.thigh**2+c**2-self.shin**2)/(2*self.thigh*c))\n",
    "        self.ankle = -acos((self.shin**2+c**2-self.thigh**2)/(2*self.shin*c))\n",
    "        self.high = sqrt(c**2-self.foot_gap**2)\n",
    "    def update_foot_gap(self,foot_gap):\n",
    "        if foot_gap >= 0 :\n",
    "            s = 1\n",
    "        else :\n",
    "            s=-1\n",
    "        self.foot_gap = foot_gap\n",
    "        # move the foot but let the high constant\n",
    "        c = sqrt(foot_gap**2+self.high**2)\n",
    "        self.c = c\n",
    "        alpha = acos((self.thigh**2+c**2-self.shin**2)/(2*self.thigh*c))\n",
    "        beta = acos((self.shin**2+c**2-self.thigh**2)/(2*self.shin*c))\n",
    "        gamma = acos((self.shin**2+self.thigh**2-self.c**2)/(2*self.shin*self.thigh))\n",
    "        self.knee = pi - gamma\n",
    "        self.hip = -(alpha + s*acos(self.high/c))\n",
    "        self.ankle = -(beta - s*acos(self.high/c))\n",
    "    def update_high(self,high):\n",
    "        if self.foot_gap >= 0 :\n",
    "            s = 1\n",
    "        else :\n",
    "            s=-1\n",
    "        self.high = high\n",
    "        c = sqrt(self.foot_gap**2+self.high**2)\n",
    "        self.c = c\n",
    "        alpha = acos((self.thigh**2+c**2-self.shin**2)/(2*self.thigh*c))\n",
    "        beta = acos((self.shin**2+c**2-self.thigh**2)/(2*self.shin*c))\n",
    "        gamma = acos((self.shin**2+self.thigh**2-self.c**2)/(2*self.shin*self.thigh))\n",
    "        self.knee = pi - gamma\n",
    "        self.hip = -(alpha + s*acos(self.high/c))\n",
    "        self.ankle = -(beta - s*acos(self.high/c))\n",
    "    def gravity_center_front(self,d_thigh):\n",
    "        c = sqrt(self.foot_gap**2+self.high**2)\n",
    "        self.c = c\n",
    "        alpha = acos(((self.thigh+d_thigh)**2+c**2-self.shin**2)/(2*(self.thigh+d_thigh)*c))\n",
    "        beta = acos((self.shin**2+c**2-(self.thigh+d_thigh)**2)/(2*self.shin*c))\n",
    "        gamma = acos((self.shin**2+(self.thigh+d_thigh)**2-self.c**2)/(2*self.shin*(self.thigh+d_thigh)))\n",
    "        self.knee = pi - gamma\n",
    "        self.hip = -(alpha + acos(self.high/c))\n",
    "        self.ankle = -(beta - acos(self.high/c))\n",
    "        gamma = pi+self.hip\n",
    "        self.hip = -(pi-gamma-asin(((d_thigh*sin(gamma)))/self.upper_body))\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now,You need the robot and the V-REP time."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from poppy.creatures import PoppyHumanoid\n",
    "\n",
    "poppy = PoppyHumanoid(simulator='vrep')\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 104,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "58.1762504578\n",
      "58.2012519836\n"
     ]
    }
   ],
   "source": [
    "import time as real_time\n",
    "class time:\n",
    "    def __init__(self,robot):\n",
    "        self.robot=robot\n",
    "    def time(self):\n",
    "        t_simu = self.robot.current_simulation_time\n",
    "        return t_simu\n",
    "    def sleep(self,t):\n",
    "        t0 = self.robot.current_simulation_time\n",
    "        while (self.robot.current_simulation_time - t0) < t-0.01:\n",
    "            real_time.sleep(0.001)\n",
    "\n",
    "time = time(poppy)\n",
    "print time.time()\n",
    "time.sleep(0.025) #0.025 is the minimum step according to the V-REP defined dt  \n",
    "print time.time()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is now possible to define a mobility in percentage, according to the angle limit of ankle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 105,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "class leg_move(leg_angle):\n",
    "    def __init__(self,motor_limit,knee=0):\n",
    "        self.ankle_limit_front=radians(motor_limit.angle_limit[1])\n",
    "        self.ankle_limit_back=radians(motor_limit.angle_limit[0])\n",
    "        leg_angle.__init__(self,knee)\n",
    "        \n",
    "    def update_foot_gap_percent(self,foot_gap_percent):\n",
    "        #calcul of foot_gap_max to convert foot_gap_percent into value\n",
    "        if foot_gap_percent>=0:# si le foot_gap est positif\n",
    "            if  acos(self.high/(self.shin+self.thigh)) > self.ankle_limit_front:\n",
    "                # construction 1 knee!=0\n",
    "                gap1 = sin(self.ankle_limit_front)*self.shin\n",
    "                high1 = cos(self.ankle_limit_front)*self.shin\n",
    "                high2 = self.high - high1\n",
    "                gap2 = sqrt(self.thigh**2-high2**2)\n",
    "                foot_gap_max = gap1 + gap2\n",
    "                foot_gap = foot_gap_percent * foot_gap_max / 100\n",
    "                self.update_foot_gap(foot_gap)\n",
    "            else:\n",
    "                #construction 2 knee=0\n",
    "                foot_gap_max = sqrt((self.shin+self.thigh)**2-self.high**2)\n",
    "                foot_gap = foot_gap_percent * foot_gap_max / 100\n",
    "                self.update_foot_gap(foot_gap)\n",
    "        if foot_gap_percent<0:\n",
    "            if -acos((self.high-self.thigh)/self.shin )< self.ankle_limit_back:\n",
    "                #construction 1 knee!=0\n",
    "                print degrees(self.ankle_limit_back)\n",
    "                print degrees(-acos((self.high-self.thigh)/self.shin ))\n",
    "                gap1 = sin(self.ankle_limit_back)*self.shin\n",
    "                high1 = cos(self.ankle_limit_back)*self.shin\n",
    "                high2 = self.high - high1\n",
    "                print gap1,high1,high2\n",
    "                gap2 = sqrt(self.thigh**2-high2**2)\n",
    "                print gap1,gap2,high1,high2\n",
    "                foot_gap_max = gap1 + gap2\n",
    "                foot_gap = -foot_gap_percent * foot_gap_max / 100\n",
    "                self.update_foot_gap(foot_gap)\n",
    "            else:\n",
    "                #constrution 2 knee=0\n",
    "                foot_gap_max = sqrt((self.shin+self.thigh)**2-self.high**2)\n",
    "                foot_gap = foot_gap_percent * foot_gap_max / 100\n",
    "                self.update_foot_gap(foot_gap)\n",
    "                \n",
    "    def update_high_percent(self,high_percent,high_min,high_max):\n",
    "        high_var = high_max-high_min\n",
    "        high = (high_percent*high_var/100)+high_min\n",
    "        self.update_high(high)\n",
    "        \n",
    "    def high_limit(self):\n",
    "        high_max = sqrt((self.shin+self.thigh)**2-self.foot_gap**2)\n",
    "        high1_min = cos(self.ankle_limit_back)*self.shin\n",
    "        gap2 = self.foot_gap-sin(self.ankle_limit_back)*self.shin\n",
    "        # si gap2 est supérieur a thigh alors ce n'est plus la flexion de la cheville qui est limitante\n",
    "        # dans ce cas on met la hauteur a zero\n",
    "        if gap2 <= self.thigh:\n",
    "            high2_min = sqrt(self.thigh**2-gap2**2)\n",
    "            high_min = high1_min + high2_min\n",
    "        else:\n",
    "            high_min = 0\n",
    "        return [high_min,high_max]\n",
    "        \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finaly, a primitive can set the high and the foot gap of poppy."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 106,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from pypot.primitive import Primitive\n",
    "\n",
    "class leg_primitive(Primitive):\n",
    "    def __init__(self,robot,speed,knee=0):\n",
    "        self.right = leg_move(robot.l_ankle_y,knee)# il faudrait mettre r_ankle_y mais les angles limites semblent faux, c'est l'opposé\n",
    "        self.left = leg_move(robot.l_ankle_y,knee)\n",
    "        self.robot = robot\n",
    "        Primitive.__init__(self, robot)\n",
    "        self.high_percent = 100\n",
    "        self.r_foot_gap_percent = 0\n",
    "        self.l_foot_gap_percent = 0\n",
    "        self.speed = speed\n",
    "                \n",
    "    def run(self):    \n",
    "        if self.high_percent !=-1:\n",
    "            high_limit=(max([self.right.high_limit()[0],self.left.high_limit()[0]]),min([self.right.high_limit()[1],self.left.high_limit()[1]]))\n",
    "            self.right.update_high_percent(self.high_percent,high_limit[0],high_limit[1])\n",
    "            self.left.update_high_percent(self.high_percent,high_limit[0],high_limit[1])\n",
    "        \n",
    "        if self.r_foot_gap_percent !=-1:  \n",
    "            self.right.update_foot_gap_percent(self.r_foot_gap_percent)\n",
    "        \n",
    "        if self.l_foot_gap_percent !=-1: \n",
    "            self.left.update_foot_gap_percent(self.l_foot_gap_percent)\n",
    "        \n",
    "        print \"left - ankle\" ,degrees(self.left.ankle),'knee', degrees(self.left.knee),'hip', degrees(self.left.hip), 'high', self.left.high,'foot_gap',self.left.foot_gap\n",
    "        print \"right - ankle\" ,degrees(self.right.ankle),'knee', degrees(self.right.knee),'hip', degrees(self.right.hip), 'high', self.right.high,'foot_gap',self.right.foot_gap\n",
    "        \n",
    "        \n",
    "        self.robot.l_ankle_y.goto_position(degrees(self.left.ankle),self.speed)\n",
    "        self.robot.r_ankle_y.goto_position(degrees(self.right.ankle),self.speed)\n",
    "\n",
    "        self.robot.l_knee_y.goto_position(degrees(self.left.knee),self.speed)\n",
    "        self.robot.r_knee_y.goto_position(degrees(self.right.knee),self.speed)\n",
    "\n",
    "        self.robot.l_hip_y.goto_position(degrees(self.left.hip),self.speed)\n",
    "        self.robot.r_hip_y.goto_position(degrees(self.right.hip),self.speed,wait=True)\n",
    "        \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is now possible to set the high and the foot gap using the leg_primitive."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 107,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "leg=leg_primitive(poppy,speed=3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 148,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "left - ankle -45.0000012522 knee 74.9143377502 hip -29.914336498 high 28.3298176149 foot_gap -3.75123885076\n",
      "right - ankle -29.914336498 knee 74.9143377502 hip -45.0000012522 high 28.3298176149 foot_gap 3.75123885076\n"
     ]
    }
   ],
   "source": [
    "leg.start()\n",
    "time.sleep(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 158,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "left - ankle -37.6559179142 knee 61.3914751643 hip -23.7355572501 high 30.7279217832 foot_gap -3.75123885076\n",
      "right - ankle -23.7355572501 knee 61.3914751643 hip -37.6559179142 high 30.7279217832 foot_gap 3.75123885076\n",
      "left - ankle -5.98113927708 knee 0.0 hip 5.98113927708 high 35.8040250123 foot_gap -3.75123885076\n",
      "right - ankle 5.98113927708 knee 0.0 hip -5.98113927708 high 35.8040250123 foot_gap 3.75123885076\n",
      "left - ankle -45.0000012522 knee 74.9143377502 hip -29.914336498 high 28.3298176149 foot_gap -3.75123885076\n",
      "right - ankle -29.914336498 knee 74.9143377502 hip -45.0000012522 high 28.3298176149 foot_gap 3.75123885076\n",
      "left - ankle -13.6256234662 knee 34.5247348032 hip -20.899111337 high 34.3091835328 foot_gap 2.18064142224\n",
      "right - ankle -20.899111337 knee 34.5247348032 hip -13.6256234662 high 34.3091835328 foot_gap -2.18064142224\n",
      "left - ankle -35.8708422009 knee 80.8708434532 hip -45.0000012522 high 27.3140407067 foot_gap 2.18064142224\n",
      "right - ankle -45.0000012522 knee 80.8708434532 hip -35.8708422009 high 27.3140407067 foot_gap -2.18064142224\n",
      "left - ankle -3.47272447478 knee 6.94544894955 hip -3.47272447478 high 35.9338949042 foot_gap 0.0\n",
      "right - ankle -3.47272447478 knee 6.94544894955 hip -3.47272447478 high 35.9338949042 foot_gap 0.0\n"
     ]
    }
   ],
   "source": [
    "time.sleep(1)\n",
    "leg.speed=3\n",
    "leg.high_percent=50\n",
    "leg.r_foot_gap_percent=20\n",
    "leg.l_foot_gap_percent=-20\n",
    "leg.start()\n",
    "time.sleep(3)\n",
    "leg.high_percent=100\n",
    "leg.r_foot_gap_percent=-1\n",
    "leg.l_foot_gap_percent=-1\n",
    "leg.start()\n",
    "time.sleep(3)\n",
    "leg.high_percent=0\n",
    "leg.start()\n",
    "time.sleep(3)\n",
    "leg.high_percent=80\n",
    "leg.r_foot_gap_percent=-20\n",
    "leg.l_foot_gap_percent=20\n",
    "leg.start()\n",
    "time.sleep(3)\n",
    "leg.r_foot_gap_percent=-1\n",
    "leg.l_foot_gap_percent=-1\n",
    "leg.high_percent=0\n",
    "leg.start()\n",
    "time.sleep(3)\n",
    "leg.high_percent=100\n",
    "leg.r_foot_gap_percent=0\n",
    "leg.l_foot_gap_percent=0\n",
    "leg.start()\n",
    "time.sleep(3)\n",
    "\n",
    "\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.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 0
}