{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Benchmark your Poppy robot"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The goal of this notebook is to help you identify the performance of your robot and where the bottle necks are. We will measure:\n",
    "* the time to read/write the position to one motor (for each of your dynamixel bus)\n",
    "* the time to read/write the positions for all motors (for each of your dynamixel bus)\n",
    "* the regularity of the synchronization loop of pos/speed/load when\n",
    "    * only this loop is runnnig\n",
    "    * all other synchronization loops are running\n",
    "    * everything else is running"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from ipywidgets import interact"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Populating the interactive namespace from numpy and matplotlib\n"
     ]
    }
   ],
   "source": [
    "%pylab inline"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "All bench info will be stored in this dictionary so it's easy to compare with other platforms."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "results = {}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## What's the platform"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Linux-4.19.118-v7l+-armv7l-with-debian-10.4\n"
     ]
    }
   ],
   "source": [
    "import platform\n",
    "\n",
    "p = platform.platform()\n",
    "print(p)\n",
    "\n",
    "results['platform'] = p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3.7.3 (default, Dec 20 2019, 18:57:59) \n",
      "[GCC 8.3.0]\n"
     ]
    }
   ],
   "source": [
    "import sys\n",
    "\n",
    "v = sys.version\n",
    "print(v)\n",
    "\n",
    "results['python'] = v"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Pypot version: 4.0.0\n"
     ]
    }
   ],
   "source": [
    "import pypot\n",
    "\n",
    "results['pypot'] = pypot.__version__\n",
    "print('Pypot version: {}'.format(results['pypot']))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "13c4b64ba42349fd82a2148c8e419b91",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(Dropdown(description='robot', options={'poppy-ergo-jr': <class 'poppy_ergo_jr.poppy_ergo…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from pypot.creatures import installed_poppy_creatures\n",
    "\n",
    "RobotCls = None\n",
    "\n",
    "def robot_selector(robot):\n",
    "    global RobotCls\n",
    "    RobotCls = robot\n",
    "    \n",
    "interact(robot_selector, robot=installed_poppy_creatures);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "poppy_ergo_jr.poppy_ergo_jr.PoppyErgoJr"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "RobotCls"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "robot = RobotCls()\n",
    "results['robot'] = RobotCls"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Make sure all motors are turned off to avoid breaking anything:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "for m in robot.motors:\n",
    "    m.compliant = True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We find the synchronization loop for pos/speed/load and monkey patch them for monitoring."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "\n",
    "from pypot.dynamixel.syncloop import MetaDxlController\n",
    "from pypot.dynamixel.controller import PosSpeedLoadDxlController\n",
    "\n",
    "meta_controllers = [c for c in robot._controllers if isinstance(c, MetaDxlController)]\n",
    "\n",
    "#controllers = [cc for cc in c.controllers for c in meta_controllers if isinstance(cc, PosSpeedLoadDxlController)]\n",
    "controllers = []\n",
    "for c in meta_controllers:\n",
    "    controllers.extend([cc for cc in c.controllers if isinstance(cc, PosSpeedLoadDxlController)])\n",
    "\n",
    "for c in controllers:\n",
    "    c.stop()\n",
    "\n",
    "for c in controllers:    \n",
    "    def wrapped_update():\n",
    "        if not hasattr(c, 't'):\n",
    "            c.t = []\n",
    "            \n",
    "        c.t.append(time.time())\n",
    "        c.update()\n",
    "        \n",
    "    c._update = wrapped_update\n",
    "    \n",
    "\n",
    "for c in controllers:\n",
    "    c.start()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, we define our monitor and plotting functions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "import psutil\n",
    "\n",
    "def monitor(controllers, duration):\n",
    "    for c in controllers:\n",
    "        c.stop()\n",
    "        c.t = []\n",
    "        c.start()\n",
    "        \n",
    "    cpu = []\n",
    "    start = time.time()\n",
    "    while time.time() - start < duration:\n",
    "        time.sleep(1.0)\n",
    "    cpu.append(psutil.cpu_percent())\n",
    "        \n",
    "    print('Avg CPU usage: {}%'.format(mean(cpu)))\n",
    "    return {c: array(c.t) for c in controllers}\n",
    "\n",
    "def freq_plot(logs):\n",
    "    for c, t in logs.items():\n",
    "        dt = diff(t)\n",
    "        freq = 1.0 / dt\n",
    "        \n",
    "        print('Avg frq for controller {}: {:.3f} ms STD={:.3} ms'.format(c.ids, freq.mean(), freq.std()))\n",
    "        hist(freq)\n",
    "        xlim(0, 100)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We also define this follow trajectory function, which applies a sinus on one motor (choosen below) and plot how close is its real position from the target one:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "def follow_trajectory(motor, duration=5, freq=50):\n",
    "    t = linspace(0, duration, duration * freq)\n",
    "\n",
    "    a1, f1 = 10.0, 1.0\n",
    "    a2, f2 = 5.0, 0.5\n",
    "\n",
    "    traj = a1 * sin(2 * pi * f1 * t) + a2 * sin(2 * pi * f2 * t)\n",
    "    rec = []\n",
    "\n",
    "\n",
    "    motor.compliant = False\n",
    "    motor.moving_speed = 0\n",
    "    motor.goal_position = 0\n",
    "    time.sleep(1.)\n",
    "\n",
    "    for p in traj:\n",
    "        motor.goal_position = p\n",
    "        rec.append(motor.present_position)\n",
    "\n",
    "        time.sleep(1.0 / freq)    \n",
    "        \n",
    "    motor.compliant = True\n",
    "\n",
    "    plot(traj)\n",
    "    plot(rec)    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now choose which motor you want to use for the follow trajectory test. It should be able to move freely from -20 to +20 degrees."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "91ba2ce5e78c4161bdbf9ab88f6903ed",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(Dropdown(description='m', options=('m1', 'm2', 'm3', 'm4', 'm5', 'm6'), value='m1'), Out…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "motor = None\n",
    "\n",
    "def motor_selector(m):\n",
    "    global motor\n",
    "    motor = getattr(robot, m)\n",
    "    \n",
    "interact(motor_selector, m=[m.name for m in robot.motors]);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Benchmark"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Our benchmark duration in seconds:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "duration = 30"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Normal usage"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Avg CPU usage: 10.7%\n",
      "Avg frq for controller [1, 2, 3, 4, 5, 6]: 49.264 ms STD=2.85 ms\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6236 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "d = monitor(controllers, duration)\n",
    "freq_plot(d)\n",
    "\n",
    "results['normal'] = d"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 34836 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "follow_trajectory(motor)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Without primitives"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "for p in robot.primitives:\n",
    "    p.stop()\n",
    "    \n",
    "robot._primitive_manager.stop()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Avg CPU usage: 11.7%\n",
      "Avg frq for controller [1, 2, 3, 4, 5, 6]: 49.283 ms STD=2.95 ms\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6244 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "d = monitor(controllers, duration)\n",
    "freq_plot(d)\n",
    "\n",
    "results['without primitive'] = d"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 34576 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "follow_trajectory(motor)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Without all sensors"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "for s in robot.sensors:\n",
    "    s.close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Avg CPU usage: 6.0%\n",
      "Avg frq for controller [1, 2, 3, 4, 5, 6]: 49.729 ms STD=0.118 ms\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 6156 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "d = monitor(controllers, duration)\n",
    "freq_plot(d)\n",
    "\n",
    "results['without sensor'] = d"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 34800 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "follow_trajectory(motor)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.7.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
