Laboratoire 2.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "# Laboratoire 2\n",
    "## Gyroscopes à taux (rate gyroscope)\n",
    "\n",
    "**Matériel nécessaire fourni** : table tournante, surface antidérapante,\n",
    "règle de 30 cm, masking tape, LEGOs inclinés.\n",
    "\n",
    "Ce laboratoire utilise le gyro embarqué de la plateforme robotique.\n",
    "Il s'agit du TDK InvenSense [ICM20948](https://invensense.tdk.com/wp-content/uploads/2024/03/DS-000189-ICM-20948-v1.6.pdf)\n",
    "<!-- Il s'agit du STMicroelectronics [L3G4200D](http://www.st.com/content/ccc/resource/technical/document/datasheet/04/46/d6/00/be/d9/46/ae/CD00265057.pdf/files/CD00265057.pdf/jcr:content/translations/en.CD00265057.pdf). -->\n",
    "\n",
    "![img](img/gyro_schema.png)\n",
    "![img](img/gyro_new_datasheet.png)\n",
    "\n",
    "### Partie 1 - Familiarisation avec le capteur\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-09-25T18:37:00.738656Z",
     "start_time": "2024-09-25T18:37:00.454627Z"
    },
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# Importation des modules\n",
    "\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "%matplotlib inline\n",
    "\n",
    "from robmob.robot import Robot\n",
    "from robmob.rover.sensors import RobotEspSensor\n",
    "from robmob.visualization import Visualizer\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-09-25T18:37:04.684966Z",
     "start_time": "2024-09-25T18:37:04.470894Z"
    },
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "error from callback <bound method Robot._on_error of <robmob.robot.Robot object at 0x7b8a29ce0dc0>>: Échec de la connexion au robot\n"
     ]
    }
   ],
   "source": [
    "# Connexion au robot\n",
    "ip_robot = 'localhost'\n",
    "robot = Robot(ip_robot)\n",
    "robot.connect()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Il faut ensuite ajouter le gyroscope aux capteurs écoutés par le robot."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "gyro = RobotEspSensor()\n",
    "robot.add_sensor(gyro)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "En guise de *sanity check*, le code suivant affiche la dernière mesure du gyro. Assurez-vous que le robot est immobile avant de prendre la mesure."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "gyro.peek_gyro()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Vous devriez remarquer qu'il y a passablement de bruit dans le capteur. Maintenant, installer le robot sur un plateau rotatif et faites le tourner à différentes vitesses. Remarquez le comportement de la mesure en z. Comment se comporte-t-elle à des vitesses de rotation plus élevées?\n",
    "\n",
    "Le code suivant affiche la mesure en temps réel. Appuyer deux fois sur la touche *i* ou appuyez sur le stop dans le haut de la page pour arrêter la boucle."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "import time\n",
    "from IPython.display import clear_output\n",
    "\n",
    "while True:\n",
    "    mesure = gyro.peek_gyro()\n",
    "    clear_output(wait=True)\n",
    "    print(\"x: %0.10f deg/sec\" % mesure['x'])\n",
    "    print(\"y: %0.10f deg/sec\" % mesure['y'])\n",
    "    print(\"z: %0.10f deg/sec\" % mesure['z'])\n",
    "    time.sleep(0.05)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "> **NOTE** Selon le manufacturier, le gyroscope est utilisé en mode 250 dps."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "### Partie 2 - Calibration du gyroscope\n",
    "Selon la datasheet, la valeur de Initial ZRO Tolerance, ou *zero-rate output*, se situe entre -5 et 5 dps. Autrement dit, lorsque la plateforme est immobile, la mesure ne sera pas zéro, mais se situera entre -5 et +5 degrés par seconde.\n",
    "Cette valeur peut varier selon la température à un taux de +/- 0.05 dps/°C.\n",
    "\n",
    "\n",
    "Pour cette partie, laissez le robot immobile durant 30 secondes.\n",
    ">**Attention!** Accrocher, faire vibrer (ou même souffler!) sur le capteur faussera le résultat.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "samples = gyro.sample_gyro_for_x_sec(30)  # samples a 3 colonnes: x, y et z\n",
    "z_samples = samples[:, 2]  # notation numpy pour toutes les lignes, colonne 2\n",
    "print(\"Dimension de z_samples: \", z_samples.shape)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Puisque le roboto se déplace sur un environnement 2D, nous utiliserons uniquement l'axe des z (voir le schéma au haut du notebook)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "z_offset = ...  # TODO\n",
    "print('Offset:', z_offset)\n",
    "\n",
    "plt.title('Gyro calibration')\n",
    "plt.xlabel('Sample index')\n",
    "plt.ylabel('Angular speed (dps)')\n",
    "plt.plot(samples[:, 2])\n",
    "plt.axhline(y=z_offset, color='r', linestyle='--')\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Cette valeur `z_offset` permettra d'ajuster les mesures dans les parties suivantes."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "### Partie 3 - Intégration des mesures"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Dans cette partie, nous allons écrire un programme qui donne l'angle du robot en degré en intégrant les mesures du gyro. Pour ce faire, prenez notes que la frequence d'aquisition des données est 108Hz, tel que stocké dans la constante `gyro.SAMPLE_RATE`. Le temps d'intégration `dt` sera donc `dt = 1 / gyro.SAMPLE_RATE`.\n",
    "\n",
    "Pour être plus précis avec la fréquence d'échantillonnage, vous pouvez lancer un autre terminal par SSH et exécuter la commande suivante:\n",
    "\n",
    "```bash\n",
    "ros2 topic hz /rover/state\n",
    "```\n",
    "\n",
    "#### Exercice\n",
    "Écrivez une fonction qui prend en entrée une mesure du gyro, intègre la mesure angulaire et l'accumule dans une variable `current_angle`. N'oubliez pas de tenir compte du biais `z_offset`. Il s'agit ici de faire une intégration numérique simple.\n",
    "\n",
    "> **PROTIP** Utilisez l'opérateur modulo % pour ramener l'angle en 0 et 360\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    },
    "tags": []
   },
   "outputs": [],
   "source": [
    "current_angle = 0\n",
    "\n",
    "\n",
    "def integrate_gyro_measure(measure):\n",
    "    global current_angle\n",
    "    # modifiez current_angle de sorte qu'il intègre la nouvelle mesure\n",
    "    # chaque fois que cette fonction est appelée."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# Le programme suivant met à jour current_angle en appelant votre fonction à une fréquence de 62.4Hz\n",
    "\n",
    "from IPython.display import clear_output\n",
    "\n",
    "sleep_time_sec = 1.0 / gyro.SAMPLE_RATE\n",
    "\n",
    "\n",
    "def show_robot_angle():\n",
    "    while True:\n",
    "        measure = gyro.peek_gyro()['z']  # read_data() retire la donnée lue, peak_data() la laisse dans le buffer\n",
    "        integrate_gyro_measure(measure)\n",
    "\n",
    "        clear_output(wait=True)\n",
    "        print(current_angle)\n",
    "        time.sleep(sleep_time_sec)\n",
    "\n",
    "\n",
    "show_robot_angle()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Pour tester l'intégration des mesures, placez le robot sur un plateau de verre rotatif. Fixez un bout de papier comme point de repère sur le robot et placez le robot au centre de 4 tuiles de plancher (les joints de tuiles représentant 0 deg, 90 deg, 180 deg et 270 deg). Faites tourner le robot et observez les mesures.\n",
    "\n",
    "> **PROTIP** Il est possible que vous observiez que l'angle dérive rapidement. Ce la peut être causé par un changement de température du robot (voir ZRO Variation Over Temperature dans la datasheet). Si cela est le cas, réévaluez le code de la partie 2 pour recalculer `z_offset`"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Maintenant, produisez un graphique montrant la vitesse angulaire du robot en fonction du temps, et un deuxième montrant l'angle du robot dans le temps."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TODO graphique vitesse angulaire\n",
    "\n",
    "# TODO graphique position"
   ]
  },
  {
   "metadata": {},
   "cell_type": "markdown",
   "source": [
    "#### Exercice\n",
    "Si vous faites un tour complet de 360 degrés, est-ce que l'angle final est de 360 degrés? Pourquoi?\n",
    "\n",
    "Sachant que votre robot a fait un tour complet, comment pouvez-vous corriger l'angle final pour qu'il soit de 360 degrés?\n",
    "Calculez un gain multiplicatif sur la mesure de l'angle pour corriger cette erreur.\n"
   ]
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": "# Votre code ici"
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "### Partie 4 - Influence de l'angle du gyroscope\n",
    "\n",
    "\n",
    "Faites de nouveaux la partie précédente, mais en plaçant le capteur sur un plan\n",
    "incliné (utilisez les Lego inclinés à cette fin). Faites quelques tours de 360 degrés, toujours en marquant\n",
    "un tour complet d’une pause de quelques secondes. Comparez les résultats de l'angle final obtenu avec\n",
    "l'intégration du signal calibré par rapport à un estimé de 360 degrés. En particulier, est-ce\n",
    "capteur vient changer la mesure?\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "### Partie 5 -  Estimation de la dérive du gyroscope"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "#### Exercice\n",
    "Orientez le robot dans un angle connu à l'aide d'une référence. Faites une capture d’environ une\n",
    "minute en vous inspirant du code de la partie 2, pendant laquelle vous faites des rotations horaires et antihoraires de\n",
    "10-100 degrés autour de cette référence, donc sans jamais compléter un tour. Ceci simulera un robot\n",
    "qui se déplace en tournant régulièrement, par exemple, mais qui ne fait jamais un tour complet. Avant\n",
    "la fin de l’enregistrement, revenez exactement à l’angle de référence du départ, et conservez le robot\n",
    "dans cet angle.\n",
    "Une fois l’enregistrement terminé, faites l'intégration du signal calibré. S’il n’y avait pas\n",
    "d’erreur, l’angle du début devrait être le même que celui de la fin. Quelle est l’erreur accumulée\n",
    "(dérive) sur l’angle que vous constatez?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# Votre code ici"
   ]
  },
  {
   "metadata": {},
   "cell_type": "markdown",
   "source": [
    "### Partie 6- Impact de mouvements brusques sur l'angle du robot\n",
    "\n",
    "Capturez des données de gyroscope pendant 30 secondes.\n",
    "Pendant la capture, faites des mouvements linéaires brusques avec le robot (par exemple, en le déplaçant rapidement d'un côté à l'autre).\n",
    "Affichez la vitesse angulaire du robot en fonction du temps avec `matplotlib`.\n",
    "Que constatez-vous?"
   ]
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": "# Votre code ici"
  },
  {
   "metadata": {},
   "cell_type": "markdown",
   "source": [
    "Maintenant, faites des mouvements de rotation brusques avec le robot (par exemple, en le faisant tourner rapidement sur lui-même).\n",
    "À la fin de votre capture, le robot devrait être dans la même orientation qu'au début.\n",
    "Affichez la vitesse angulaire du robot en fonction du temps avec `matplotlib`.\n",
    "Que constatez-vous?"
   ]
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": "# Votre code ici"
  },
  {
   "metadata": {},
   "cell_type": "markdown",
   "source": [
    "#### Integration de l'angle avec mouvements de rotation brusques\n",
    "\n",
    "En utilisant le code d'intégration de l'angle, intégrez les mesures du gyro avec les mouvements de rotation brusques.\n",
    "Est-ce que le robot revient à son angle initial?"
   ]
  },
  {
   "metadata": {},
   "cell_type": "code",
   "outputs": [],
   "execution_count": null,
   "source": "# Votre code ici"
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": "### Partie 7 – Création d’une carte de l’environnement"
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "pycharm": {
     "name": "#%% md\n"
    }
   },
   "source": [
    "Vous allez maintenant faire une carte de l'environnement en utilisant le capteur infrarouge et le gyroscope. La carte sera construite en faisant tourner le robot. Ainsi, vous allez\n",
    "scanner l’environnement en 2D, avec des mesures en coordonnées polaires (i.e. distance et angle).\n",
    "Déposez la table tournante et le robot sur le plancher. Assurez-vous d’avoir des obstacles intéressants\n",
    "(boites de carton, poubelle ronde, jambes stables, mur, etc) autour du robot. Assurez-vous également\n",
    "que tous les obstacles soient situés à 20 cm ou plus du robot (le capteur IR fonctionne pour des\n",
    "distances de 20-150 cm). Pour tracer cette carte, l'angle du robot est calculé en fonction du temps.Amusez-vous à faire plusieurs cartes en déplaçant les objets entre les scans! Il se peut que des\n",
    "objets plats apparaissent bombés : ceci sera le symptôme attribuable à la calibration en distance du\n",
    "capteur infrarouge par défaut dans le script qui est différente de votre capteur."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# Aquisition de 1000 points de la carte à environ 10Hz\n",
    "\n",
    "import time, math\n",
    "from robmob.sensors import SharpSensor\n",
    "from scipy.interpolate import interp1d\n",
    "\n",
    "# Listes des points de la carte à remplir\n",
    "map_points = []\n",
    "\n",
    "# Ajout du capteur sharp\n",
    "ir_sensor = SharpSensor(0)\n",
    "robot.add_sensor(ir_sensor)\n",
    "\n",
    "current_angle = 0\n",
    "last_measure_time = time.time()\n",
    "\n",
    "fn_volt_to_cm = interp1d(ir_sensor.HIGH_RANGE_CALIB_TABLE[:, 1], ir_sensor.HIGH_RANGE_CALIB_TABLE[:, 0])\n",
    "\n",
    "for i in range(200):\n",
    "    time.sleep(0.1)\n",
    "\n",
    "    gyro_measures_z = gyro.peek_buffer()[:, 2]\n",
    "    time_since_last_measure, last_measure_time = time.time() - last_measure_time, time.time()\n",
    "    number_of_measures = math.floor(time_since_last_measure * gyro.SAMPLE_RATE)\n",
    "\n",
    "    #Intégration des dernière mesures depuis la dernière mise-à-jour\n",
    "    current_angle += np.sum((gyro_measures_z[-number_of_measures:] - z_offset) * (1 / gyro.SAMPLE_RATE))\n",
    "\n",
    "    current_ir_volt = ir_sensor.peek_gyro()\n",
    "    map_points.append([math.radians(current_angle), fn_volt_to_cm(current_ir_volt)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": false,
    "jupyter": {
     "outputs_hidden": false
    },
    "pycharm": {
     "name": "#%%\n"
    }
   },
   "outputs": [],
   "source": [
    "# Affichage de la carte\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "points = np.asarray(map_points)\n",
    "\n",
    "fig, ax = plt.subplots(1, 1, subplot_kw=dict(projection='polar'))\n",
    "\n",
    "ax.grid(True)\n",
    "ax.set_ylim(0.0, 200.0)  # Changez cette valeur pour \"zoomer\" sur les environs du robot\n",
    "ax.scatter(points[:, 0], points[:, 1])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.10.15"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}