{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "

\n", "\"Duckietown\n", "

" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# 🚙 💻 Encoder-based Odometry\n", "\n", "\"Odometry\" is the problem of \"measuring the path\", or evolution of the pose in time, of the robot. \n", "\n", "We can solve the odometry problem by using the measurements from wheel encoders, and a so called \"dead-reckoning\" model, to estimate the evolution of the pose in time through an iterative procedure, such that:\n", "\n", "

\"odometry-1\"

\n", "\n", "$$ x_{k+1} = x_k + \\Delta x_k $$\n", "$$ y_{k+1} = y_k + \\Delta y_k $$\n", "$$ \\theta_{k+1} = \\theta_k + \\Delta \\theta_k $$\n", "\n", "Where initial conditions ($x_0$, $y_0$, $\\theta_0$) are assumed to be known. The increments can be calculated by:\n", "\n", "1. **Determining the rotation of each wheel through the wheel encoder measurements**\n", "\n", "$$\\Delta \\phi_k = N_k \\cdot \\alpha$$\n", "\n", "where $N_k$ is the number of pulses, or \"ticks\", measured from the encoders in the $k-th$ time interval, $\\alpha = \\frac{2 \\pi}{N_{tot}}$ is the rotation per tick, and $N_{tot}$ is the total number of ticks per revolution ($N_{tot} = 135$ for the wheel encoders we will be using). This relation is evaluated for each wheel, yielding $\\Delta \\phi_{l,k}$ and $\\Delta \\phi_{r,k}$ for the left and right wheels respectively.\n", "\n", "2. **Deriving the total distance travelled by each wheel**\n", "\n", "

\"odometry-d\"

\n", "\n", "Assuming the wheel radii are the same (equal to $R$) for both wheels, the distance travelled by each wheel is given by:\n", "\n", "$$ d_{l/r, k} = R \\cdot \\Delta \\phi_{l/r,k}$$\n", "\n", "3. **Finding the rotation and distance travelled by the robot (frame)**\n", "\n", "

\"odometry-2\"

\n", "\n", "Under the assumption of no slipping of the robot wheels, we can derive the distance travelled by the origin of the robot frame (point $A$) and the rotation of the robot $\\Delta \\theta$:\n", "\n", "$$ d_{A, k} = \\frac{d_{r,k} + d_{l,k}}{2} $$\n", "$$ \\Delta \\theta_{k} = \\frac{d_{r,k} - d_{l,k}}{2L}$$\n", "\n", "4. **Expressing the robot motion in the world reference frame**\n", "\n", "

\"odometry-3\"

\n", "\n", "Finally, we can express the estimated motion in the world reference frame and find:\n", "\n", "$$ \\Delta x_k = d_{A, k} \\cos\\theta_k $$\n", "$$ \\Delta y_k = d_{A, k} \\sin\\theta_k $$" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "# 🚙 💻 Let's get started!\n", "\n", "You will write a function that produces an estimate of the pose of the Duckiebot, given measurements from the wheel encoders and an initial position:" ] }, { "cell_type": "code", "execution_count": null, "metadata": { "collapsed": false, "pycharm": { "name": "#%%\n" } }, "outputs": [], "source": [ "# Run and do not edit this magic cell. \n", "# It helps getting things to work throughout the Jupyter notebook - in particular importing changes in functions made in files other than this workspace.\n", "\n", "%load_ext autoreload\n", "%autoreload 2" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "\n", "x0 = y0 = 0.0 # meters\n", "theta0 = np.deg2rad(0) # radians\n", "\n", "print(x0,y0, theta0)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Determine the rotation of each wheel through the wheel encoder measurements\n", "\n", "We have seen how to read wheel encoder data in the [wheel encoder tutorial](../../notebooks/03-Wheel-Encoders-Tutorial/wheel_encoders_tutorial.ipynb). We can now use this data to measure the rotation of each wheel. \n", "\n", "### Wheel encoder calibration factor\n", "\n", "Remember that there are 135 ticks per revolution on the wheel encoders we are using. " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Write the correct expressions \n", "import numpy as np \n", "\n", "N_tot = 135 # total number of ticks per revolution\n", "alpha = 2 * np.pi / N_tot # wheel rotation per tick in radians\n", "\n", "print(f\"The angular resolution of our encoders is: {np.rad2deg(alpha)} degrees\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Assume that at the current update the left and right motor encoders have produced the following measurements:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Feel free to play with the numbers to get an idea of the expected outcome\n", "\n", "ticks_left = 1\n", "prev_tick_left = 0\n", "\n", "ticks_right = 0\n", "prev_tick_right = 0" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "How much did each wheel rotate? " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# How much would the wheels rotate with the above tick measurements? \n", "\n", "# Repetita iuvant: don't confuse degrees and radians when expressing angles\n", "# Machines always use radians, humans make sense of degrees better. \n", "# Mixing these up is a very very common source of error!\n", "\n", "delta_ticks_left = None # calculate delta ticks of left wheel \n", "delta_ticks_right = None # calculate delta ticks of right wheel \n", "rotation_wheel_left = None # calculate total rotation of left wheel \n", "rotation_wheel_right = None # calculate total rotation of right wheel \n", "\n", "print(f\"The left wheel rotated: {np.rad2deg(rotation_wheel_left)} degrees\")\n", "print(f\"The right wheel rotated: {np.rad2deg(rotation_wheel_right)} degrees\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 🚙 💻 Evaluate distance travelled by each wheel\n", "\n", "Now let's calculate the distance travelled by each wheel. It depends on the wheel radii. We need to determine them! We could use advanced odometry calibration procedures, but let's take it a step at the time. \n", "\n", "If you have a robot, take a ruler and measure your wheel radii (let's assume they are the same):" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# What is the radius of your wheels (assuming they are identical)? \n", "\n", "R = 0.0318 # you could double check your robot but this is probably correct" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Note: the default value used in simulation and on the robot is $R = 0.0318 \\text{m}$." ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# What is the distance travelled by each wheel?\n", "\n", "d_left = None # Calculate the distance traveled by the left wheel\n", "d_right = None # Calculate the distance traveled by the right wheel\n", "\n", "print(f\"The left wheel travelled: {d_left} meters\")\n", "print(f\"The right wheel rotated: {d_right} meters\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 🚙 (Optional) Save your new value of `R`\n", "\n", "If you have a Duckiebot, let's make sure it remembers its new wheel radius! \n", "\n", "Power your Duckiebot on, make sure it is connected to the network and you can ping it, then open a terminal **on your computer** and type:\n", "\n", " dts gui ROBOTNAME\n", " \n", " rosparam set /ROBOTNAME/kinematics_node/radius R-value\n", " \n", "where `R-value` is the value of the wheel radius you measured (expressed in meters). You can then save it with: \n", "\n", " rosservice call /ROBOTNAME/kinematics_node/save_calibration\n", " \n", "and finally verify that it has been saved by opening the `ROBOTNAME.yaml` file in your Dashboard > File Manager > Calibrations > Kinematics page.\n", "\n", "You can keep the terminal you just used open, so we can save the baseline measurement too. Let's keep going!" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## 3. 🚙 💻 Find the rotation and distance travelled by the Duckiebot\n", "\n", "If you have previously set your robot's gain so that the wheels do not slip, the travelled distance of point $A$ (origin of the robot frame) will be given by the average of the distances travelled by the wheels:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# How much has the robot travelled? \n", "\n", "d_A = None # robot distance travelled in robot frame (meters)\n", "\n", "print(f\"The robot has travelled: {d_A} meters\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "To calculate the rotation of the robot we need to measure the baseline too - or the distance between the center of the two wheels: \n", "\n", "

\"odometry-baseline\"

\n", "\n", "If you have a robot, take a ruler and measure it! " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# What is the baseline length of your robot? \n", "\n", "baseline_wheel2wheel = 0.1 # This is probably correct but feel free to double check " ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Note: the default value, and that used in simulation, is $baseline = 0.1m$." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "We are now ready to calculate the rotation of the Duckiebot:" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "# Of what angle has the robot rotated? \n", "\n", "Delta_Theta = None # [radians]\n", "\n", "print(f\"The robot has rotated: {np.rad2deg(Delta_Theta)} degrees\")" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### 🚙 (Optional) Save your new value of `baseline`\n", "\n", "Let's make sure it remembers its new wheel baseline! \n", "\n", "Power your Duckiebot on, make sure it is connected to the network and you can ping it, then open a terminal **on your computer** and type:\n", "\n", " dts start_gui_tools ROBOTNAME\n", " \n", " rosparam set /ROBOTNAME/kinematics_node/baseline baseline-value\n", " \n", "where `baseline-value` is the value of `baseline_wheel2wheel` you just measured (expressed in meters). You can then save it with: \n", "\n", " rosservice call /ROBOTNAME/kinematics_node/save_calibration\n", " \n", "and finally verify that it has been saved by opening the `ROBOTNAME.yaml` file in your Dashboard > File Manager > Calibrations > Kinematics page." ] }, { "cell_type": "markdown", "metadata": { "tags": [ "skip" ] }, "source": [ "# 💻 🚙 Write the odometry function\n", "\n", "Now it is time to write the functions that will actually be running on the robot. \n", "\n", "You will write two functions:\n", "\n", "1. A function that calculates the rotation of a wheel given a message from the wheel encoders and the previous number of ticks measured;\n", "\n", "2. The actual odometry function, that will receive as inputs the kinematic model parameters, the pose estimate at the previous iteration, and the rotation of each wheel. The initial position is assumed to be $q_0 = [0,0,0]^T$." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Calculating the rotation of each wheel\n", "\n", "Implement the function `delta_phi` inside the file [odometry.py](../../packages/encoder_pose/include/odometry/odometry.py).\n", "\n", "This function should output the wheel rotation (in radians) since last measurements, receiving as input the current and previous update wheel encoder readings. This function is called from a ROS node [encoder_pose_node.py](../../packages/encoder_pose/src/encoder_pose_node.py) which we have written, but you might want to take a look at it to make sure you understand what's going on. " ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Estimating the odometry\n", "\n", "Similarly, implement the function `estimate_pose` inside the file [odometry.py](../../packages/encoder_pose/include/odometry/odometry.py)..\n", "This function computes the `(x, y, theta)` estimate by aggregating computed wheel rotations and the (known) geometry of the robot." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Test the `pose_estimation()` function\n", "\n", "Unit tests are useful to check if a piece of code does its intended job. Although the interaction of different functions might yield surprises even when each function produces the expected outcome, it is good practice to test them in isolation before prime time! These are called \"unit tests\", and:\n", "\n", "> If it ain't tested, it's broken.\n", ">\n", "> --Roboticists, level 9\n", "\n", "Let's see if the function you wrote above passes the following test!" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import sys\n", "sys.path.append('../../tests')\n", "sys.path.append('../../packages/encoder_pose/include/')\n", "from unit_test import UnitTestOdometry\n", "\n", "from odometry import estimate_pose\n", "\n", "\n", "# UnitTestOdometry tests the `pose_estimation` function.\n", "# The test is successful if you get a circle in the plot. \n", "# Anything different from a circle indicates that the odometry function has something wrong.\n", "\n", "UnitTestOdometry(R, baseline_wheel2wheel, estimate_pose)" ] }, { "cell_type": "markdown", "metadata": { "tags": [ "skip" ] }, "source": [ "# 💻 🚙 Running Everything\n", "\n", "Let's now see how the odometry estimation is working in practice. \n", "\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Follow the instructions in the [README](../../README.md) to run the code and the `noVNC` browser.\n", "\n", "Double-click on the \"Odometry\" icon on the Desktop\n", "\n", "

\"test-odometry-icon\"

\n", "\n", "You will see three things opening: \n", "\n", "- a terminal\n", "- a pre-configured RVIZ window\n", "- a virtual joystick\n", "\n", "
\n", "
\n", " \n", "

Starting the Odometry activity.

\n", "
\n", "
\n", "\n", "In the RVIZ window you will see what your robot sees, and a reference frame in the gridmap. That frame represents the position and orientation of your robot, calculated _according to the `pose_estimation` and `delta_phi` functions written above_ (they are beliefs, not \"real\" states).\n", "\n", "**Note**: it may take some time (>30s) for the images and the odometry to appear, depending on the specifications of your host machine. \n", "\n", "Tips: \n", "\n", " - You can change the graphical settings of the reference frame (bigger, shorter, more or less frequently updated, etc.) through the Odometry > Shape options in the top left quadrant of the RVIZ window;\n", " \n", " - You can press `Alt` while clicking and dragging anywhere in the RVIZ terminal to move the window;\n", " \n", " - VNC opens with the resolution of your browser window when you launch it. If things look crammed, put your browser in full screen and re-copy and paste the URL. \n", " \n", " - The terminal on your computer where you ran `dts code workbench` will be streaming some debugging data. \n", " \n", "Click on the virtual joystick and start driving. You will see the marker move too according to the wheel encoder data that the robot is receiving. You can monitor these (and other) messages by following the procedure learned in the [wheel encoders tutorial](../02-Wheel-Encoders/wheel_encoders.ipynb). \n", "\n", "Drive a loop. Get back to the initial position and look at the resulting odometry. Is your robot's _belief_ accurate? Why, or why not? \n", "\n", "Do you want to modify your odometry functions before proceeding? Update the `pose_estimation` and `delta_phi` functions and re-launch `dts code workbench -b ROBOTNAME`. \n", "\n", "\n", "\n", "\n", "
\n", "
\n", " \n", " \n", "

Odometry with different markers on different loops.

\n", "
\n", "
" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "If you were testing in the Duckiematrix you should try it on your robot now! Don't worry if things are not as perfect on the real Duckiebot!\n", "\n", "
\n", "
\n", " \"successful-test-odometry-real\"\n", " \n", "

DB21 Duckiebot good and less good odometries.

\n", "
\n", "
" ] } ], "metadata": { "kernelspec": { "display_name": "Python 3.8.10 64-bit", "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.8.10" }, "vscode": { "interpreter": { "hash": "916dbcbb3f70747c44a77c7bcd40155683ae19c65e1c03b4aa3499c5328201f1" } } }, "nbformat": 4, "nbformat_minor": 4 }