├── .github └── workflows │ └── main.yml ├── .gitignore ├── .pre-commit-config.yaml ├── LICENSE ├── README.md ├── homeworks ├── hw1_kalman_filter.ipynb ├── hw2_particle_filter.ipynb └── hw3 │ ├── Dockerfile │ ├── README.md │ ├── control.py │ ├── entrypoint.sh │ └── pid.py ├── pyproject.toml ├── setup.cfg ├── week01_ros_intro ├── lect_01_introduction_to_robotics_sensors.pdf └── sem_01_ros_intro.pdf ├── week02_localization ├── lect_02_localization.pdf ├── sem_02_file_system_first_package_communication_types.pdf └── turtle.sh ├── week03_motion_models ├── first_node.sh ├── lect_03_kinematics_probabilistic_motion_models.pdf ├── sem_03_services_actions_parameter_server_roslaunch.pdf ├── signal_filter_node.py └── signal_generator_node.py ├── week04_observation_models ├── GetWindowMedian.srv ├── Signal.msg ├── lect_04_probabilistic_observation_models.pdf ├── second_node.sh ├── sem_04_names_time_debugging_visualization.pdf ├── signal_filter_node.py └── signal_generator_node.py ├── week05_mapping_turtlebot_simulation ├── lect_05_mapping.pdf ├── signal_filter_node.py ├── signal_pipeline.launch ├── turtle_action.sh └── turtlebot_simulation.sh └── week06_planning_control ├── lect_06_path planning.pdf └── lect_07_control algorithms.pdf /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | name: Mirroring 2 | 3 | on: [push, delete] 4 | 5 | jobs: 6 | to_gitlab: 7 | runs-on: ubuntu-latest 8 | steps: 9 | - uses: actions/checkout@v2 10 | with: 11 | fetch-depth: 0 12 | - uses: pixta-dev/repository-mirroring-action@v1 13 | with: 14 | target_repo_url: 15 | git@gitlab.girafe.ai:courses/robotics.git 16 | ssh_private_key: 17 | ${{ secrets.GITLAB_SSH_PRIVATE_KEY }} 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ 130 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | default_language_version: 2 | python: python3.8 3 | repos: 4 | - repo: https://github.com/pre-commit/pre-commit-hooks 5 | rev: v4.0.1 6 | hooks: 7 | - id: check-yaml 8 | - id: check-json 9 | - id: check-added-large-files 10 | - id: end-of-file-fixer 11 | - id: trailing-whitespace 12 | - id: check-case-conflict 13 | - id: mixed-line-ending 14 | 15 | - repo: https://github.com/psf/black 16 | rev: 21.8b0 17 | hooks: 18 | - id: black 19 | 20 | - repo: https://github.com/timothycrosley/isort 21 | rev: 5.9.3 22 | hooks: 23 | - id: isort 24 | 25 | - repo: https://gitlab.com/pycqa/flake8 26 | rev: 3.9.2 27 | hooks: 28 | - id: flake8 29 | additional_dependencies: [flake8-bugbear] 30 | 31 | - repo: https://github.com/nbQA-dev/nbQA 32 | rev: 1.1.0 33 | hooks: 34 | - id: nbqa-black 35 | additional_dependencies: [black==21.8b0] 36 | - id: nbqa-isort 37 | additional_dependencies: [isort==5.9.3] 38 | - id: nbqa-flake8 39 | additional_dependencies: [flake8==3.9.2] 40 | 41 | - repo: https://github.com/pre-commit/mirrors-prettier 42 | rev: v2.4.0 43 | hooks: 44 | - id: prettier 45 | args: [--print-width=80, --prose-wrap=always] 46 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mobile robotics course by girafe-ai team 2 | 3 | Course is currently thought at MIPT's MSAI program and in MADE mail.ru 4 | 5 | ## Install ROS for this course 6 | 7 | In our course we will use only Python and no hardware, so setup needed is a 8 | computing part (without sensors and actuators). 9 | 10 | Thus we have following options: 11 | 12 | 1. Docker machine (the most simple and convenient, recommended) 13 | 1. Classical virtual machine (recommended as backup to Docker) 14 | 1. Cloud virtual machine with GUI (experimental) 15 | 1. Installation on host machine (not recommended) 16 | 17 | ### Docker 18 | 19 | Suppose you have Docker installed, if no - follow 20 | [official instructions](https://docs.docker.com/get-docker/) 21 | 22 | We will use prebuilt 23 | [osrf/ros:noetic-desktop-full](https://hub.docker.com/layers/ros/osrf/ros/noetic-desktop-full/images/sha256-9cb69b409a9a93c8bfa4faacf9b27bf705ce182021cb26c2a9667bb5c5513a67?context=explore) 24 | Docker image. FYI 25 | [Dockerfile source](<(https://github.com/osrf/docker_images/blob/master/ros/noetic/ubuntu/focal/desktop-full/Dockerfile)>). 26 | 27 | 1. Pull Docker container (~1Gb)
`docker pull osrf/ros:noetic-desktop-full` 28 | 29 | ### Ubuntu 30 | 31 | 1. Allow localhost to acess display:
`xhost +local:docker` 32 | 1. Run Docker with display forwarding
33 | `docker run -it --rm --privileged --net=host -e DISPLAY=$IP:0 -v /tmp/.X11-unix:/tmp/.X11-unix osrf/ros:noetic-desktop-full` 34 | 35 | #### MacOS 36 | 37 | 1. Install Xquartz:
`brew install xquartz`
or manually with `.dmg` from 38 | [official site](https://www.xquartz.org/) 39 | 1. Run Xquartz, go to _Preferences > Security > Allow connections from network 40 | clients_, close Xquartz. 41 | 1. Allow localhost to acess display:
`xhost +127.0.0.1` 42 | 1. Run Docker with display forwarding
43 | `docker run -it --rm --privileged --net=host -e DISPLAY=$IP:0 -v /tmp/.X11-unix:/tmp/.X11-unix osrf/ros:noetic-desktop-full` 44 | 45 | If you have troubles follow 46 | [full instruction](https://desertbot.io/blog/ros-turtlesim-beginners-guide-mac). 47 | 48 | #### Windows 49 | 50 | 1. Instal X Server.
51 | [Download here](https://sourceforge.net/projects/vcxsrv/), 52 | [more detailed instruction here](https://dev.to/darksmile92/run-gui-app-in-linux-docker-container-on-windows-host-4kde). 53 | 1. Run Docker with display forwarding
54 | `docker run -it -e DISPLAY=host.docker.internal:0 osrf/ros:noetic-desktop-full` 55 | 56 | Tested on Windows 10 + WSL 2. 57 | 58 | You could setup host VSCode to be able to edit files in running Docker image: 59 | [instruction](https://www.cloudsavvyit.com/12837/how-to-edit-code-in-docker-containers-with-visual-studio-code/). 60 | 61 | [Offifial Docker installation instruction](http://wiki.ros.org/docker/Tutorials/Docker) - 62 | very concise and missing details. 63 | 64 | ## Virtual machine 65 | 66 | Install [Virtualbox](https://www.virtualbox.org/wiki/Downloads) or 67 | [Parallels Desktop](https://www.parallels.com/products/desktop/) (recommeded for 68 | MacOS, not free). 69 | 70 | Then create and launch Ubuntu instance. 71 | 72 | Then follow regular installation on Ubuntu host (see [Host section](#host)) 73 | 74 | ## Cloud virtual machine with GUI 75 | 76 | [Follow instructions](https://dev.to/easyawslearn/how-to-setup-gui-on-amazon-ec2-ubuntu-server-4mgn) 77 | for AWS. 78 | 79 | Then follow regular installation on Ubuntu host (see [Host section](#host)) 80 | 81 | This is tested only once on Windows (RDP is available for MacSO tooo). If you 82 | prefer that way - please report your experience. 83 | 84 | ## Host 85 | 86 | Follow 87 | [installation guide for ROS on Ubuntu](http://wiki.ros.org/noetic/Installation/Ubuntu) 88 | (you need to install `ros-noetic-desktop-full`) 89 | 90 | This option is not recommended. 91 | 92 | Really available only for Linux machines. Warning - it may influence your 93 | machine behaviour, so do that if you know what you do or you are on virtual 94 | machine. 95 | 96 | ## Gratitude and reference 97 | 98 | Initial materials provided by [Oleg Shipitko](https://www.oleg-shipitko.com/).\ 99 | Currently course is thaught by Vladislav Goncharenko 100 | -------------------------------------------------------------------------------- /homeworks/hw1_kalman_filter.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Home assignment 1: Kalman filter" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "Imagine a robot. Robot state is described with the following parameters\n", 15 | "\n", 16 | "1. $x$, $y$ - robot coordinates,\n", 17 | "2. $V_x$, $V_y$ - velocities.\n", 18 | "\n", 19 | "We can only measure the coordinates of the robot, which should be reflected in the $ H $ matrix.\n", 20 | "\n", 21 | "In this homework assignment:\n", 22 | "- Fill in the matrices for the Kalman filter correctly.\n", 23 | "- For all three experiments, visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time.\n", 24 | "- For all three experiments, visualize the dependence of each component of the gain matrix ($ K $) over time.\n", 25 | " - What does the dynamics of changes in its components say?\n", 26 | "- How much does the velocity uncertainty decrease as a result of each experiment?" 27 | ] 28 | }, 29 | { 30 | "cell_type": "code", 31 | "execution_count": null, 32 | "metadata": { 33 | "ExecuteTime": { 34 | "end_time": "2021-10-06T18:43:23.441304Z", 35 | "start_time": "2021-10-06T18:43:23.228920Z" 36 | } 37 | }, 38 | "outputs": [], 39 | "source": [ 40 | "import numpy as np" 41 | ] 42 | }, 43 | { 44 | "cell_type": "code", 45 | "execution_count": null, 46 | "metadata": { 47 | "ExecuteTime": { 48 | "end_time": "2021-10-06T19:56:50.855904Z", 49 | "start_time": "2021-10-06T19:56:50.847424Z" 50 | } 51 | }, 52 | "outputs": [], 53 | "source": [ 54 | "def kalman_filter(x, E):\n", 55 | " \"\"\"Apply Kalman filter to sequence\n", 56 | "\n", 57 | " Args:\n", 58 | " x: initial state space configuration (location and velocity)\n", 59 | " E: initial covariance matrix\n", 60 | " \"\"\"\n", 61 | " k_log = []\n", 62 | "\n", 63 | " for measurement in measurements:\n", 64 | " # prediction\n", 65 | " x = (F @ x) + u\n", 66 | " E = F @ E @ F.T\n", 67 | "\n", 68 | " # measurement update\n", 69 | " Z = np.array([measurement])\n", 70 | " S = H @ E @ H.T + R\n", 71 | " K = E @ H.T @ np.linalg.inv(S)\n", 72 | " k_log.append(K)\n", 73 | " x = x + (K @ (Z.T - (H @ x)))\n", 74 | " E = (I - (K @ H)) @ E\n", 75 | "\n", 76 | " print(f\"x= \\n{x}\")\n", 77 | " print(f\"E= \\n{E}\")\n", 78 | "\n", 79 | " return k_log" 80 | ] 81 | }, 82 | { 83 | "cell_type": "markdown", 84 | "metadata": {}, 85 | "source": [ 86 | "You could edit `kalman_filter` function if you need more info about the process e.g. `x` values over time and so on\n", 87 | "\n", 88 | "_Hint:_ to define matrices below function [np.diag](https://numpy.org/doc/stable/reference/generated/numpy.diag.html) is very handy" 89 | ] 90 | }, 91 | { 92 | "cell_type": "code", 93 | "execution_count": null, 94 | "metadata": {}, 95 | "outputs": [], 96 | "source": [ 97 | "dt = 0.1\n", 98 | "\n", 99 | "# initial covariance matrix: 0. for positions x and y, 1000 for the two velocities\n", 100 | "E = \n", 101 | "\n", 102 | "# next state function: 4D\n", 103 | "F = \n", 104 | "\n", 105 | "# measurement function: reflect the fact that we observe x and y but not the two velocities\n", 106 | "H = \n", 107 | "\n", 108 | "# measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal\n", 109 | "R = \n", 110 | "\n", 111 | "# 4D identity matrix\n", 112 | "I = " 113 | ] 114 | }, 115 | { 116 | "cell_type": "markdown", 117 | "metadata": {}, 118 | "source": [ 119 | "## First experiment" 120 | ] 121 | }, 122 | { 123 | "cell_type": "code", 124 | "execution_count": null, 125 | "metadata": {}, 126 | "outputs": [], 127 | "source": [ 128 | "measurements = [[5.0, 10.0], [6.0, 8.0], [7.0, 6.0], [8.0, 4.0], [9.0, 2.0], [10.0, 0.0]]\n", 129 | "initial_xy = [4.0, 12.0]\n", 130 | "\n", 131 | "\n", 132 | "# initial robot state (location and velocity)\n", 133 | "x = np.array([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])\n", 134 | "# external motion applied to the robot\n", 135 | "u = np.array([[0.0], [0.1], [0.0], [0.0]])" 136 | ] 137 | }, 138 | { 139 | "cell_type": "code", 140 | "execution_count": null, 141 | "metadata": {}, 142 | "outputs": [], 143 | "source": [ 144 | "kalman_filter(x, E)" 145 | ] 146 | }, 147 | { 148 | "cell_type": "markdown", 149 | "metadata": {}, 150 | "source": [ 151 | "Visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time\n", 152 | "\n", 153 | "_(It's a good idea to write a function for this, so you could reuse it in the next experiment)_" 154 | ] 155 | }, 156 | { 157 | "cell_type": "code", 158 | "execution_count": null, 159 | "metadata": {}, 160 | "outputs": [], 161 | "source": [ 162 | "# YOUR CODE HERE" 163 | ] 164 | }, 165 | { 166 | "cell_type": "markdown", 167 | "metadata": {}, 168 | "source": [ 169 | "Visualize the components of the $ K $ matrix below" 170 | ] 171 | }, 172 | { 173 | "cell_type": "code", 174 | "execution_count": null, 175 | "metadata": {}, 176 | "outputs": [], 177 | "source": [ 178 | "# YOUR CODE HERE" 179 | ] 180 | }, 181 | { 182 | "cell_type": "markdown", 183 | "metadata": {}, 184 | "source": [ 185 | "## Second experiment" 186 | ] 187 | }, 188 | { 189 | "cell_type": "code", 190 | "execution_count": null, 191 | "metadata": {}, 192 | "outputs": [], 193 | "source": [ 194 | "measurements = [[1.0, 4.0], [6.0, 0.0], [11.0, -4.0], [16.0, -8.0]]\n", 195 | "initial_xy = [-4.0, 8.0]\n", 196 | "\n", 197 | "dt = 0.1\n", 198 | "\n", 199 | "# initial robot state (location and velocity)\n", 200 | "x = np.array([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])\n", 201 | "# external motion applied to the robot\n", 202 | "u = np.array([[0.0], [0.1], [0.0], [0.0]])" 203 | ] 204 | }, 205 | { 206 | "cell_type": "code", 207 | "execution_count": null, 208 | "metadata": {}, 209 | "outputs": [], 210 | "source": [ 211 | "kalman_filter(x, E)" 212 | ] 213 | }, 214 | { 215 | "cell_type": "markdown", 216 | "metadata": {}, 217 | "source": [ 218 | "Visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time" 219 | ] 220 | }, 221 | { 222 | "cell_type": "code", 223 | "execution_count": null, 224 | "metadata": {}, 225 | "outputs": [], 226 | "source": [ 227 | "# YOUR CODE HERE" 228 | ] 229 | }, 230 | { 231 | "cell_type": "markdown", 232 | "metadata": {}, 233 | "source": [ 234 | "Visualize the components of the $ K $ matrix below" 235 | ] 236 | }, 237 | { 238 | "cell_type": "code", 239 | "execution_count": null, 240 | "metadata": {}, 241 | "outputs": [], 242 | "source": [ 243 | "# YOUR CODE HERE" 244 | ] 245 | }, 246 | { 247 | "cell_type": "markdown", 248 | "metadata": {}, 249 | "source": [ 250 | "## Third Experiment" 251 | ] 252 | }, 253 | { 254 | "cell_type": "code", 255 | "execution_count": null, 256 | "metadata": {}, 257 | "outputs": [], 258 | "source": [ 259 | "measurements = [[1.0, 17.0], [1.0, 15.0], [1.0, 13.0], [1.0, 11.0]]\n", 260 | "initial_xy = [1.0, 19.0]\n", 261 | "\n", 262 | "dt = 0.1\n", 263 | "\n", 264 | "# initial robot state (location and velocity)\n", 265 | "x = np.array([[initial_xy[0]], [initial_xy[1]], [0.0], [0.0]])\n", 266 | "# external motion applied to the robot\n", 267 | "u = np.array([[0.0], [0.1], [0.0], [0.0]])" 268 | ] 269 | }, 270 | { 271 | "cell_type": "code", 272 | "execution_count": null, 273 | "metadata": {}, 274 | "outputs": [], 275 | "source": [ 276 | "kalman_filter(x, E)" 277 | ] 278 | }, 279 | { 280 | "cell_type": "markdown", 281 | "metadata": {}, 282 | "source": [ 283 | "Visualize the dependence of $ x $, $ y $, $ V_x $, $ V_y $ over time" 284 | ] 285 | }, 286 | { 287 | "cell_type": "code", 288 | "execution_count": null, 289 | "metadata": {}, 290 | "outputs": [], 291 | "source": [ 292 | "# YOUR CODE HERE" 293 | ] 294 | }, 295 | { 296 | "cell_type": "markdown", 297 | "metadata": {}, 298 | "source": [ 299 | "Visualize the components of the $ K $ matrix below" 300 | ] 301 | }, 302 | { 303 | "cell_type": "code", 304 | "execution_count": null, 305 | "metadata": {}, 306 | "outputs": [], 307 | "source": [ 308 | "# YOUR CODE HERE" 309 | ] 310 | }, 311 | { 312 | "cell_type": "markdown", 313 | "metadata": {}, 314 | "source": [ 315 | "## Conclusions\n", 316 | "\n", 317 | "Don't forget to put your thoughts on the experiments above.\n", 318 | "\n", 319 | "Questions to stimulate thoughts could be found in the beginning of the notebook =)" 320 | ] 321 | }, 322 | { 323 | "cell_type": "markdown", 324 | "metadata": {}, 325 | "source": [] 326 | } 327 | ], 328 | "metadata": { 329 | "kernelspec": { 330 | "display_name": "Python [conda env:ml-mipt]", 331 | "language": "python", 332 | "name": "conda-env-ml-mipt-py" 333 | }, 334 | "language_info": { 335 | "codemirror_mode": { 336 | "name": "ipython", 337 | "version": 3 338 | }, 339 | "file_extension": ".py", 340 | "mimetype": "text/x-python", 341 | "name": "python", 342 | "nbconvert_exporter": "python", 343 | "pygments_lexer": "ipython3", 344 | "version": "3.8.5" 345 | }, 346 | "toc": { 347 | "base_numbering": 1, 348 | "nav_menu": {}, 349 | "number_sections": true, 350 | "sideBar": true, 351 | "skip_h1_title": false, 352 | "title_cell": "Table of Contents", 353 | "title_sidebar": "Contents", 354 | "toc_cell": false, 355 | "toc_position": {}, 356 | "toc_section_display": true, 357 | "toc_window_display": true 358 | } 359 | }, 360 | "nbformat": 4, 361 | "nbformat_minor": 2 362 | } 363 | -------------------------------------------------------------------------------- /homeworks/hw2_particle_filter.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Home assignment 2: Particle filter" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "In this homework assignment, we will implement an algorithm for estimating the robot's pose known as a particle filter.\n", 15 | "\n", 16 | "A particle filter consists of the following steps:\n", 17 | "\n", 18 | "1. The movement of particles in accordance with the kinematic motion model. The movement is carried out using the probabilistic motion function, taking into account the randomness of the process (noise of the motion).\n", 19 | "2. Comparison of the obtained measurement with the expected one by applying a probabilistic measurement function that estimates the probability of the obtained measurement given a fixed position of the particle. As a result of this step, each particle is assigned a weight proportional to the likelihood of the measurement given the position of the particle.\n", 20 | "3. Resampling is a process in which the probability of a particle to be chosen for a new set is proportional to the weight (likelihood) of the particle.\n", 21 | "\n", 22 | "\n", 23 | "## Task 1\n", 24 | " \n", 25 | "Implement the motion model of the mobile robot $p(x_ {t + 1} | x_t, u_t)$ as part of the `Robot` class. The method must be named `move`. The `move` method takes as input the current position of the robot and a vector of control signals (the steering angle ($\\alpha$) and the distance the robot should move ($d$)). The `move` method must return an instance of the `Robot` class with a new state vector $(x, y, \\theta)$.\n", 26 | "\n", 27 | "For this homework assignment, we will assume that our robot has car-like kinematics. Such kinematics is described by the [bicycle model](https://nabinsharma.wordpress.com/2014/01/02/kinematics-of-a-robot-bicycle-model/) (you can also refer to Lecture 3. The tricycle (note the difference in the notation). You need to implement a bicycle model in the `move()` method as an approximation of car-like kinematics.\n", 28 | "\n", 29 | "**Important:** The coordinates $(x, y)$ of the robot's state vector set the position of the center of the rear wheel axle of the robot.\n", 30 | "\n", 31 | "**Important:** The `move` method must also simulate the noise of the control signals. For this, additive normal noise is applied to each component of the control signal vector. The steering angle noise is specified by the `Robot.steering_noise` class attribute. The movement noise is specified by the `Robot.distance_noise` class attribute. The `Robot.steering_noise` and `Robot.distance_noise` parameters set the standard deviation ($\\sigma$) of the normal distribution." 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": null, 37 | "metadata": {}, 38 | "outputs": [], 39 | "source": [ 40 | "from math import pi, \n", 41 | "import random" 42 | ] 43 | }, 44 | { 45 | "cell_type": "code", 46 | "execution_count": null, 47 | "metadata": {}, 48 | "outputs": [], 49 | "source": [ 50 | "# --------\n", 51 | "#\n", 52 | "# the \"world\" has 4 landmarks.\n", 53 | "# the robot's initial coordinates are somewhere in the square\n", 54 | "# represented by the landmarks.\n", 55 | "\n", 56 | "landmarks = [\n", 57 | " [100.0, 0.0],\n", 58 | " [0.0, 0.0],\n", 59 | " [0.0, 100.0],\n", 60 | " [100.0, 100.0],\n", 61 | "] # position of 4 landmarks\n", 62 | "world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel \"out of bounds\"" 63 | ] 64 | }, 65 | { 66 | "cell_type": "code", 67 | "execution_count": null, 68 | "metadata": {}, 69 | "outputs": [], 70 | "source": [ 71 | "class Robot:\n", 72 | "\n", 73 | " # --------\n", 74 | "\n", 75 | " # init:\n", 76 | " # creates robot and initializes location/orientation\n", 77 | " #\n", 78 | "\n", 79 | " def __init__(self, length=10.0):\n", 80 | " self.x = random.random() * world_size # initial x position\n", 81 | " self.y = random.random() * world_size # initial y position\n", 82 | " self.orientation = random.random() * 2.0 * pi # initial orientation\n", 83 | " self.length = length # length of robot\n", 84 | " self.bearing_noise = 0.0 # initialize bearing noise to zero\n", 85 | " self.distance_noise = 0.0 # initialize distance noise to zero\n", 86 | " self.steering_noise = 0.0 # initialize steering noise to zero\n", 87 | "\n", 88 | " def __repr__(self):\n", 89 | " return \"[x=%.6s y=%.6s theta=%.6s]\" % (\n", 90 | " str(self.x),\n", 91 | " str(self.y),\n", 92 | " str(self.orientation),\n", 93 | " )\n", 94 | "\n", 95 | " # --------\n", 96 | " # set:\n", 97 | " # sets a robot coordinate\n", 98 | " #\n", 99 | "\n", 100 | " def set(self, new_x, new_y, new_orientation):\n", 101 | "\n", 102 | " if new_orientation < 0 or new_orientation >= 2 * pi:\n", 103 | " raise ValueError(\"Orientation must be in [0..2pi]\")\n", 104 | " self.x = float(new_x)\n", 105 | " self.y = float(new_y)\n", 106 | " self.orientation = float(new_orientation)\n", 107 | "\n", 108 | " # --------\n", 109 | " # set_noise:\n", 110 | " # \tsets the noise parameters\n", 111 | " #\n", 112 | "\n", 113 | " def set_noise(self, new_b_noise, new_s_noise, new_d_noise):\n", 114 | " # makes it possible to change the noise parameters\n", 115 | " # this is often useful in particle filters\n", 116 | " self.bearing_noise = float(new_b_noise)\n", 117 | " self.steering_noise = float(new_s_noise)\n", 118 | " self.distance_noise = float(new_d_noise)\n", 119 | "\n", 120 | " ############# ONLY ADD/MODIFY CODE BELOW ###################\n", 121 | "\n", 122 | " # --------\n", 123 | " # move:\n", 124 | " # move along a section of a circular path according to motion parameters\n", 125 | " #\n", 126 | "\n", 127 | " def move(self, motion):\n", 128 | " return 0\n", 129 | " # make sure your move function returns an instance of Robot class\n", 130 | " # with the correct coordinates.\n", 131 | "\n", 132 | " ############## ONLY ADD/MODIFY CODE ABOVE ####################" 133 | ] 134 | }, 135 | { 136 | "cell_type": "code", 137 | "execution_count": null, 138 | "metadata": {}, 139 | "outputs": [], 140 | "source": [ 141 | "## --------\n", 142 | "## TEST CASE #1:\n", 143 | "##\n", 144 | "## The following code should print:\n", 145 | "## Robot: [x=0.0 y=0.0 theta=0.0]\n", 146 | "## Robot: [x=10.0 y=0.0 theta=0.0]\n", 147 | "## Robot: [x=19.861 y=1.4333 theta=0.2886]\n", 148 | "## Robot: [x=39.034 y=7.1270 theta=0.2886]\n", 149 | "##\n", 150 | "\n", 151 | "length = 20.0\n", 152 | "bearing_noise = 0.0\n", 153 | "steering_noise = 0.0\n", 154 | "distance_noise = 0.0\n", 155 | "\n", 156 | "myrobot = Robot(length)\n", 157 | "myrobot.set(0.0, 0.0, 0.0)\n", 158 | "myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n", 159 | "\n", 160 | "motions = [[0.0, 10.0], [pi / 6.0, 10], [0.0, 20.0]]\n", 161 | "\n", 162 | "T = len(motions)\n", 163 | "\n", 164 | "print(\"Robot: \", myrobot)\n", 165 | "for t in range(T):\n", 166 | " myrobot = myrobot.move(motions[t])\n", 167 | " print(\"Robot: \", myrobot)" 168 | ] 169 | }, 170 | { 171 | "cell_type": "code", 172 | "execution_count": null, 173 | "metadata": {}, 174 | "outputs": [], 175 | "source": [ 176 | "## --------\n", 177 | "## TEST CASE #2:\n", 178 | "##\n", 179 | "## The following code should print:\n", 180 | "## Robot: [x=0.0 y=0.0 theta=0.0]\n", 181 | "## Robot: [x=9.9828 y=0.5063 theta=0.1013]\n", 182 | "## Robot: [x=19.863 y=2.0201 theta=0.2027]\n", 183 | "## Robot: [x=29.539 y=4.5259 theta=0.3040]\n", 184 | "## Robot: [x=38.913 y=7.9979 theta=0.4054]\n", 185 | "## Robot: [x=47.887 y=12.400 theta=0.5067]\n", 186 | "## Robot: [x=56.369 y=17.688 theta=0.6081]\n", 187 | "## Robot: [x=64.273 y=23.807 theta=0.7094]\n", 188 | "## Robot: [x=71.517 y=30.695 theta=0.8108]\n", 189 | "## Robot: [x=78.027 y=38.280 theta=0.9121]\n", 190 | "## Robot: [x=83.736 y=46.485 theta=1.0135]\n", 191 | "\n", 192 | "length = 20.0\n", 193 | "bearing_noise = 0.0\n", 194 | "steering_noise = 0.0\n", 195 | "distance_noise = 0.0\n", 196 | "\n", 197 | "myrobot = Robot(length)\n", 198 | "myrobot.set(0.0, 0.0, 0.0)\n", 199 | "myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n", 200 | "\n", 201 | "motions = [[0.2, 10.0] for row in range(10)]\n", 202 | "\n", 203 | "T = len(motions)\n", 204 | "\n", 205 | "print(\"Robot: \", myrobot)\n", 206 | "for t in range(T):\n", 207 | " myrobot = myrobot.move(motions[t])\n", 208 | " print(\"Robot: \", myrobot)" 209 | ] 210 | }, 211 | { 212 | "cell_type": "markdown", 213 | "metadata": {}, 214 | "source": [ 215 | "## Task 2\n", 216 | " \n", 217 | "Implement a mobile robot measurement method. The method must be named `sense`. The `sense` method takes as input the current state of the robot (`self`) and returns $z$ - the current measurement consisting of four bearings to four landmarks located in space. Bearing is the angle at which the object is observed from the current position. The angle at which the robot observes each landmark is measured from the robot's current orientation $\\theta$. The counterclockwise direction is assumed to be positive.\n", 218 | "\n", 219 | "To calculate the bearing, you need the position of the landmarks in space. It is set by the global variable `landmarks`.\n", 220 | "\n", 221 | "**Important:** The `sense` method should also simulate the measurement noise. For this, additive normal noise is applied to each component of the measurement vector. The measurement noise is specified by the *Robot.bearing_noise* class attribute. This parameter specifies the standard deviation ($\\sigma$) of the normal distribution. Provide the ability to calculate a noisy measurement vector by passing an input argument `no_noise = True` to the function." 222 | ] 223 | }, 224 | { 225 | "cell_type": "markdown", 226 | "metadata": {}, 227 | "source": [ 228 | "Copy the `Robot` class to the cell below and add the `sense` method to it." 229 | ] 230 | }, 231 | { 232 | "cell_type": "code", 233 | "execution_count": null, 234 | "metadata": {}, 235 | "outputs": [], 236 | "source": [ 237 | "## --------\n", 238 | "## TEST CASE #1:\n", 239 | "##\n", 240 | "## 1) The following code should print the list:\n", 241 | "## [6.004885648174475, 3.7295952571373605, 1.9295669970654687, 0.8519663271732721]\n", 242 | "\n", 243 | "length = 20.0\n", 244 | "bearing_noise = 0.0\n", 245 | "steering_noise = 0.0\n", 246 | "distance_noise = 0.0\n", 247 | "\n", 248 | "myrobot = Robot(length)\n", 249 | "myrobot.set(30.0, 20.0, 0.0)\n", 250 | "myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n", 251 | "\n", 252 | "print(\"Robot: \", myrobot)\n", 253 | "print(\"Measurements: \", myrobot.sense())" 254 | ] 255 | }, 256 | { 257 | "cell_type": "code", 258 | "execution_count": null, 259 | "metadata": {}, 260 | "outputs": [], 261 | "source": [ 262 | "## --------\n", 263 | "## TEST CASE #2:\n", 264 | "##\n", 265 | "## 2) The following code should print the list^\n", 266 | "## [5.376567117456516, 3.101276726419402, 1.3012484663475101, 0.22364779645531352]\n", 267 | "\n", 268 | "length = 20.0\n", 269 | "bearing_noise = 0.0\n", 270 | "steering_noise = 0.0\n", 271 | "distance_noise = 0.0\n", 272 | "\n", 273 | "myrobot = Robot(length)\n", 274 | "myrobot.set(30.0, 20.0, pi / 5.0)\n", 275 | "myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n", 276 | "\n", 277 | "print(\"Robot: \", myrobot)\n", 278 | "print(\"Measurements: \", myrobot.sense())" 279 | ] 280 | }, 281 | { 282 | "cell_type": "markdown", 283 | "metadata": {}, 284 | "source": [ 285 | "## Task 3\n", 286 | "\n", 287 | "Implement the mobile robot observation model $p(z_t | x_t, M)$. The method must be named `measurement_prob`. The `measurement_prob` method takes a measurement vector $z$ as input and returns the likelihood of the measurement. Likelihood is calculated as the product of four (by the number of landmarks) normal distributions of measurement errors. Each normal distribution shows the bearing probability i.e. the normal distribution of the error for each bearing has the mathematical expectation in the true (expected) bearing value and the variance given by the `Robot.bearing_noise` parameter.\n", 288 | "\n", 289 | "**Important:** Remember to normalize the angles when calculating bearing errors. The error must be in the range $- \\pi ... + \\pi$.\n", 290 | "\n", 291 | "**Important:** To get the true (expected) values of the measurements, you can use the `sense` method with the `no_noise = True` flag.\n", 292 | "\n", 293 | "Copy the `Robot` class into the cell below and add the `measurement_prob` method to it." 294 | ] 295 | }, 296 | { 297 | "cell_type": "code", 298 | "execution_count": null, 299 | "metadata": {}, 300 | "outputs": [], 301 | "source": [ 302 | "# YOUR CODE HERE" 303 | ] 304 | }, 305 | { 306 | "cell_type": "markdown", 307 | "metadata": {}, 308 | "source": [ 309 | "## Task 4\n", 310 | "\n", 311 | "Run the particle filter based on the `Robot` class you have implemented. Add a step-by-step visualization of the particle filter for the second test case.\n", 312 | "\n", 313 | "The visualization should reflect:\n", 314 | "1. Map with marked positions of landmarks\n", 315 | "2. Particles - it is enough to reflect only $(x, y)$\n", 316 | "3. The final estimated position of the robot at each moment in time" 317 | ] 318 | }, 319 | { 320 | "cell_type": "code", 321 | "execution_count": null, 322 | "metadata": {}, 323 | "outputs": [], 324 | "source": [ 325 | "max_steering_angle = (\n", 326 | " pi / 4.0\n", 327 | ") # You do not need to use this value, but keep in mind the limitations of a real car.\n", 328 | "bearing_noise = 0.1\n", 329 | "steering_noise = 0.1\n", 330 | "distance_noise = 5.0\n", 331 | "\n", 332 | "tolerance_xy = 15.0 # Tolerance for localization in the x and y directions.\n", 333 | "tolerance_orientation = 0.25 # Tolerance for orientation.\n", 334 | "\n", 335 | "\n", 336 | "# --------\n", 337 | "#\n", 338 | "# the \"world\" has 4 landmarks.\n", 339 | "# the robot's initial coordinates are somewhere in the square\n", 340 | "# represented by the landmarks.\n", 341 | "\n", 342 | "landmarks = [\n", 343 | " [100.0, 0.0],\n", 344 | " [0.0, 0.0],\n", 345 | " [0.0, 100.0],\n", 346 | " [100.0, 100.0],\n", 347 | "] # position of 4 landmarks\n", 348 | "world_size = 100.0 # world is NOT cyclic. Robot is allowed to travel \"out of bounds\"" 349 | ] 350 | }, 351 | { 352 | "cell_type": "code", 353 | "execution_count": null, 354 | "metadata": {}, 355 | "outputs": [], 356 | "source": [ 357 | "# Some utility functions\n", 358 | "\n", 359 | "def get_position(p):\n", 360 | " x = 0.0\n", 361 | " y = 0.0\n", 362 | " orientation = 0.0\n", 363 | " for i in range(len(p)):\n", 364 | " x += p[i].x\n", 365 | " y += p[i].y\n", 366 | " # orientation is tricky because it is cyclic. By normalizing\n", 367 | " # around the first particle we are somewhat more robust to\n", 368 | " # the 0=2pi problem\n", 369 | " orientation += (((p[i].orientation - p[0].orientation + pi) % (2.0 * pi)) \n", 370 | " + p[0].orientation - pi)\n", 371 | " return [x / len(p), y / len(p), orientation / len(p)]\n", 372 | "\n", 373 | "\n", 374 | "# The following code generates ground truth poses and measurements\n", 375 | "def generate_ground_truth(motions):\n", 376 | "\n", 377 | " myrobot = Robot()\n", 378 | " myrobot.set_noise(bearing_noise, steering_noise, distance_noise)\n", 379 | "\n", 380 | " Z = []\n", 381 | " T = len(motions)\n", 382 | "\n", 383 | " for t in range(T):\n", 384 | " myrobot = myrobot.move(motions[t])\n", 385 | " Z.append(myrobot.sense())\n", 386 | " #print 'Robot: ', myrobot\n", 387 | " return [myrobot, Z]\n", 388 | "\n", 389 | "\n", 390 | "# The following code prints the measurements associated\n", 391 | "# with generate_ground_truth\n", 392 | "def print_measurements(Z):\n", 393 | "\n", 394 | " T = len(Z)\n", 395 | "\n", 396 | " print 'measurements = [[%.8s, %.8s, %.8s, %.8s],' % \\\n", 397 | " (str(Z[0][0]), str(Z[0][1]), str(Z[0][2]), str(Z[0][3]))\n", 398 | " for t in range(1,T-1):\n", 399 | " print ' [%.8s, %.8s, %.8s, %.8s],' % \\\n", 400 | " (str(Z[t][0]), str(Z[t][1]), str(Z[t][2]), str(Z[t][3]))\n", 401 | " print ' [%.8s, %.8s, %.8s, %.8s]]' % \\\n", 402 | " (str(Z[T-1][0]), str(Z[T-1][1]), str(Z[T-1][2]), str(Z[T-1][3]))\n", 403 | "\n", 404 | "\n", 405 | "# The following code checks to see if your particle filter\n", 406 | "# localizes the robot to within the desired tolerances\n", 407 | "# of the true position. The tolerances are defined at the top.\n", 408 | "def check_output(final_robot, estimated_position):\n", 409 | "\n", 410 | " error_x = abs(final_robot.x - estimated_position[0])\n", 411 | " error_y = abs(final_robot.y - estimated_position[1])\n", 412 | " error_orientation = abs(final_robot.orientation - estimated_position[2])\n", 413 | " error_orientation = (error_orientation + pi) % (2.0 * pi) - pi\n", 414 | " correct = error_x < tolerance_xy and error_y < tolerance_xy \\\n", 415 | " and error_orientation < tolerance_orientation\n", 416 | " return correct" 417 | ] 418 | }, 419 | { 420 | "cell_type": "code", 421 | "execution_count": null, 422 | "metadata": {}, 423 | "outputs": [], 424 | "source": [ 425 | "def particle_filter(motions, measurements, N=500): # We will use 500 particles\n", 426 | " # --------\n", 427 | " #\n", 428 | " # Create particles (models of the Robot)\n", 429 | " #\n", 430 | "\n", 431 | " particles = []\n", 432 | " for i in range(N):\n", 433 | " robot = Robot()\n", 434 | " robot.set_noise(bearing_noise, steering_noise, distance_noise)\n", 435 | " particles.append(robot)\n", 436 | "\n", 437 | " # --------\n", 438 | " #\n", 439 | " # Update particles\n", 440 | " #\n", 441 | "\n", 442 | " for t in range(len(motions)):\n", 443 | "\n", 444 | " # motion update (prediction)\n", 445 | " particles_after_motion = []\n", 446 | " for i in range(N):\n", 447 | " particles_after_motion.append(particles[i].move(motions[t]))\n", 448 | " particles = particles_after_motion\n", 449 | "\n", 450 | " # measurement update (correction)\n", 451 | " weights = []\n", 452 | " for i in range(N):\n", 453 | " weights.append(particles[i].measurement_prob(measurements[t]))\n", 454 | "\n", 455 | " # resampling\n", 456 | " particles_resampled = []\n", 457 | " index = int(random.random() * N)\n", 458 | " beta = 0.0\n", 459 | " mw = max(weights)\n", 460 | " for i in range(N):\n", 461 | " beta += random.random() * 2.0 * mw\n", 462 | " while beta > weights[index]:\n", 463 | " beta -= weights[index]\n", 464 | " index = (index + 1) % N\n", 465 | " particles_resampled.append(particles[index])\n", 466 | " particles = particles_resampled\n", 467 | "\n", 468 | " return get_position(particles)" 469 | ] 470 | }, 471 | { 472 | "cell_type": "code", 473 | "execution_count": null, 474 | "metadata": {}, 475 | "outputs": [], 476 | "source": [ 477 | "## --------\n", 478 | "## TEST CASE #1:\n", 479 | "## \n", 480 | "##1) Calling the particle_filter function with the following\n", 481 | "## motions and measurements should return a [x,y,orientation]\n", 482 | "## vector near [x=93.476 y=75.186 orient=5.2664], that is, the\n", 483 | "## robot's true location.\n", 484 | "##\n", 485 | "motions = [[2. * pi / 10, 20.] for row in range(8)]\n", 486 | "measurements = [[4.746936, 3.859782, 3.045217, 2.045506],\n", 487 | " [3.510067, 2.916300, 2.146394, 1.598332],\n", 488 | " [2.972469, 2.407489, 1.588474, 1.611094],\n", 489 | " [1.906178, 1.193329, 0.619356, 0.807930],\n", 490 | " [1.352825, 0.662233, 0.144927, 0.799090],\n", 491 | " [0.856150, 0.214590, 5.651497, 1.062401],\n", 492 | " [0.194460, 5.660382, 4.761072, 2.471682],\n", 493 | " [5.717342, 4.736780, 3.909599, 2.342536]]\n", 494 | "\n", 495 | "print particle_filter(motions, measurements)" 496 | ] 497 | }, 498 | { 499 | "cell_type": "code", 500 | "execution_count": null, 501 | "metadata": {}, 502 | "outputs": [], 503 | "source": [ 504 | "## --------\n", 505 | "## TEST CASE #1:\n", 506 | "##\n", 507 | "## 2) You can generate your own test cases by generating\n", 508 | "## measurements using the generate_ground_truth function.\n", 509 | "## It will print the robot's last location when calling it.\n", 510 | "##\n", 511 | "\n", 512 | "number_of_iterations = 6\n", 513 | "motions = [[2.0 * pi / 20, 12.0] for row in range(number_of_iterations)]\n", 514 | "\n", 515 | "x = generate_ground_truth(motions)\n", 516 | "final_robot = x[0]\n", 517 | "measurements = x[1]\n", 518 | "estimated_position = particle_filter(motions, measurements)\n", 519 | "print_measurements(measurements)\n", 520 | "print(\"Ground truth: \", final_robot)\n", 521 | "print(\"Particle filter: \", estimated_position)\n", 522 | "print(\"Code check: \", check_output(final_robot, estimated_position))" 523 | ] 524 | } 525 | ], 526 | "metadata": { 527 | "kernelspec": { 528 | "display_name": "Python 3", 529 | "language": "python", 530 | "name": "python3" 531 | }, 532 | "language_info": { 533 | "codemirror_mode": { 534 | "name": "ipython", 535 | "version": 3 536 | }, 537 | "file_extension": ".py", 538 | "mimetype": "text/x-python", 539 | "name": "python", 540 | "nbconvert_exporter": "python", 541 | "pygments_lexer": "ipython3", 542 | "version": "3.8.3" 543 | }, 544 | "toc": { 545 | "base_numbering": 1, 546 | "nav_menu": {}, 547 | "number_sections": true, 548 | "sideBar": true, 549 | "skip_h1_title": false, 550 | "title_cell": "Table of Contents", 551 | "title_sidebar": "Contents", 552 | "toc_cell": false, 553 | "toc_position": {}, 554 | "toc_section_display": true, 555 | "toc_window_display": true 556 | } 557 | }, 558 | "nbformat": 4, 559 | "nbformat_minor": 2 560 | } 561 | -------------------------------------------------------------------------------- /homeworks/hw3/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic-ros-base 2 | 3 | 4 | RUN mkdir -p homework/homework3/cpp \ 5 | mkdir -p homework/homework3/python 6 | 7 | # homework folder creation and PID homework repo downloading 8 | RUN cd homework/homework3/cpp && \ 9 | git clone https://github.com/udacity/CarND-PID-Control-Project.git && \ 10 | cd CarND-PID-Control-Project && sudo apt-get update && \ 11 | sudo apt-get install git libuv1-dev libssl-dev gcc g++ cmake make unzip wget -y && \ 12 | git clone https://github.com/uWebSockets/uWebSockets && \ 13 | cd uWebSockets && \ 14 | git checkout e94b6e1 && \ 15 | mkdir build && \ 16 | cd build && \ 17 | cmake .. && \ 18 | make && \ 19 | sudo make install && \ 20 | cd .. && \ 21 | cd .. && \ 22 | sudo ln -s /usr/lib64/libuWS.so /usr/lib/libuWS.so && \ 23 | sudo rm -r uWebSockets 24 | 25 | # udacity simulator installation 26 | RUN cd homework/homework3 && \ 27 | wget https://github.com/udacity/self-driving-car-sim/releases/download/v1.45/term2_sim_linux.zip && \ 28 | unzip term2_sim_linux.zip && rm term2_sim_linux.zip && cd term2_sim_linux && \ 29 | sudo chmod +x term2_sim.x86_64 && rm term2_sim.x86 30 | 31 | # zsh and oh-my-zsh installation 32 | RUN sudo apt install --yes zsh &&\ 33 | set -uex; \ 34 | wget https://raw.githubusercontent.com/robbyrussell/oh-my-zsh/master/tools/install.sh; \ 35 | sh ./install.sh; \ 36 | rm ./install.sh 37 | 38 | # vs code installation 39 | RUN sudo apt install --yes apt-utils libasound2 curl; \ 40 | echo "deb [arch=amd64] http://packages.microsoft.com/repos/vscode stable main" | sudo \ 41 | tee /etc/apt/sources.list.d/vs-code.list; \ 42 | curl https://packages.microsoft.com/keys/microsoft.asc | gpg --dearmor > microsoft.gpg; \ 43 | sudo mv microsoft.gpg /etc/apt/trusted.gpg.d/microsoft.gpg; \ 44 | sudo apt update; \ 45 | sudo apt install --yes code 46 | 47 | ADD ./control.py homework/homework3/python/control.py 48 | ADD ./pid.py homework/homework3/python/pid.py 49 | 50 | ADD ./entrypoint.sh /entrypoint.sh 51 | RUN chmod +x /entrypoint.sh 52 | ENTRYPOINT ["/entrypoint.sh"] 53 | -------------------------------------------------------------------------------- /homeworks/hw3/README.md: -------------------------------------------------------------------------------- 1 | # Homework 3 2 | 3 | In this homework assignment, we will implement a PID controller to control the 4 | steering wheel of an autonomous vehicle. We will test the implemented regulator 5 | on a simulator of an autonomous car, designed for Udacity Self-Driving Car 6 | Nanodegree https://github.com/udacity/self-driving-car-sim. 7 | 8 | We will use a prepared docker container for homework (of course you can install 9 | everything into your local OS or use virtual machine if you prefer to). In the 10 | directory where the archive is unpacked, build the docker image: 11 | 12 | `docker build --tag homework` 13 | 14 | After building the image, you can start the container with the command: 15 | 16 | `sudo docker run -e DISPLAY=unix$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix -it homework` 17 | 18 | **Warning:** If you get an error, starting graphical applications from the 19 | container, run the following command on you host OS: xhost + local 20 | 21 | To run an additional terminal in a container, you can find out the 22 | `CONTAINER_ID` by running the `docker ps` command and finding the required 23 | container in the `CONTAINER_ID` output. After that, you need to run the command: 24 | 25 | `docker exec -it zsh` 26 | 27 | **Warning:** With this method of launching an additional terminal, the launch of 28 | graphical applications will not be available from it. 29 | 30 | In the container, you need to go to the homework/homework3/ directory: 31 | 32 | `cd homework/homework3/` 33 | 34 | The homework directory contains three folders: 35 | 36 | - cpp - directory containing files to implement PID regulator in C++ 37 | - cpp - directory containing files to implement PID regulator in Python 38 | - term2_sim_linux/ - directory with a simulator of an autonomous vehicle 39 | 40 | To run the simulator, run the executable file: 41 | 42 | `/homework/homework3/term2_sim_linux/term2_sim.x86_64` 43 | 44 | Select the "Project 4: PID Controller" tab and press Select. 45 | 46 | There are two programming languages which can be used for this assignment (С++ 47 | or Python). Depending on the language you prefer proceed to the corresponding 48 | section. 49 | 50 | ## Python 51 | 52 | **Note:** Python 3.6 and higher is required 53 | 54 | `cd /homework/homework3/python` 55 | 56 | The homework directory contains two files: 57 | 58 | - `control.py` - main file for realizing steering control; 59 | - `pid.py` - realization of PID regulator. 60 | 61 | Realize the PID regulator in order to control the steering of autonomous car. 62 | All the instructions are given inside `.py` files. Note that you should launch 63 | control.py script in another terminal (not the one you used to launch 64 | simulator). 65 | 66 | `code --user-data-dir = / vscode` 67 | 68 | ## С++ 69 | 70 | In another terminal (not the one you used to launch simulator) of the container, 71 | you need to run a car control program with an implemented PID controller. To do 72 | this, go to the source code folder: 73 | 74 | `cd /homework/homework3/cpp/CarND-PID-Control-Project` 75 | 76 | Create a build directory and build the project: 77 | 78 | ```bash 79 | mkdir build && cd build 80 | cmake .. && make 81 | ``` 82 | 83 | Start the regulator (**Warning** when starting the regulator, the simulator must 84 | be running): 85 | 86 | `./pid` 87 | 88 | After starting the regulator, the vehicle should drive in a straight line. This 89 | is because the regulator code for steering control has not yet been implemented. 90 | You need to implement a PID controller in C ++. To do this, go to the directory: 91 | 92 | `cd /homework/homework3/cpp/CarND-PID-Control-Project/src` 93 | 94 | In this directory, you need to modify 2 files: 95 | 96 | - `PID.cpp` - PID regulator implementation 97 | - `main.cpp` - the main file with the main function, which implements the 98 | connection to the simulator and the call of the PID regulator 99 | 100 | In PID.cpp, you need to implement 3 functions: 101 | 102 | - void PID :: Init (double Kp*, double Ki*, double Kd\_) - initialization of the 103 | regulator, as well as its internal state 104 | - void PID :: UpdateError (double cte) - a function that updates the internal 105 | state of the regulator (proportional, integral and differential), depending on 106 | the error of the vehicle's lateral displacement from the desired trajectory 107 | (cross-track error, cte) 108 | - double PID :: TotalError () - calculation of the resulting influence of the 109 | regulator (according to the formula of the PID regulator from the lecture), 110 | which will be fed to the steering, as the angle of steering wheel rotation 111 | 112 | In `main.cpp`, you need to modify the main function, in which you need to add 113 | the initialization of the PID controller, as well as its use to calculate the 114 | steer_value - control signal for the angle of rotation of the wheels depending 115 | on the current error of the lateral displacement from the trajectory 116 | (cross-track error, cte). 117 | 118 | After making the necessary modifications, rebuild the cmake .. && make project 119 | from the /homework/homework3/cpp/CarND-PID-Control-Project/build folder. Run the 120 | simulator and regulator to see the updated car controls. Select such 121 | coefficients of the PID controller (Kp, Ki, Kd) at which the deviation from the 122 | trajectory will be minimal. 123 | 124 | **Warning!** For the convenience of programming, the Visual Studio Code 125 | development environment is installed in the container. Launching Visual Studio 126 | Code only works from a terminal that allows graphical applications to run. The 127 | Visual Studio Code environment can be launched with the command: 128 | 129 | `code --user-data-dir = / vscode` 130 | -------------------------------------------------------------------------------- /homeworks/hw3/control.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import asyncio 4 | import json 5 | 6 | import websockets 7 | from pid import PID 8 | 9 | 10 | # TODO: Initialize the pid variable. 11 | steering_pid = PID(0.053, 0.0, 0.0) 12 | 13 | # Checks if the SocketIO event has JSON data. 14 | # If there is data the JSON object in string format will be returned, 15 | # else the empty string "" will be returned. 16 | def getData(message): 17 | try: 18 | start = message.find("[") 19 | end = message.rfind("]") 20 | return message[start : end + 1] 21 | except: 22 | return "" 23 | 24 | 25 | async def handleTelemetry(websocket, msgJson): 26 | cte = msgJson[1]["cte"] 27 | speed = msgJson[1]["speed"] 28 | angle = msgJson[1]["steering_angle"] 29 | 30 | print("CTE: ", cte, ", speed: ", speed, ", angle: ", angle) 31 | 32 | # TODO: Calculate steering value here, remember the steering value is 33 | # [-1, 1]. 34 | # NOTE: Feel free to play around with the throttle and speed. 35 | # Maybe use another PID controller to control the speed! 36 | 37 | response = {} 38 | 39 | response["steering_angle"] = steer_value 40 | 41 | # TODO: Play around with throttle value 42 | response["throttle"] = 0.3 43 | 44 | msg = '42["steer",' + json.dumps(response) + "]" 45 | 46 | await websocket.send(msg) 47 | 48 | 49 | async def echo(websocket, path): 50 | async for message in websocket: 51 | if len(message) < 3 or message[0] != "4" or message[1] != "2": 52 | return 53 | 54 | s = getData(message) 55 | msgJson = json.loads(s) 56 | 57 | event = msgJson[0] 58 | if event == "telemetry": 59 | await handleTelemetry(websocket, msgJson) 60 | else: 61 | msg = '42["manual",{}]' 62 | await websocket.send(msg) 63 | 64 | 65 | def main(): 66 | start_server = websockets.serve(echo, "localhost", 4567) 67 | 68 | asyncio.get_event_loop().run_until_complete(start_server) 69 | asyncio.get_event_loop().run_forever() 70 | 71 | 72 | if __name__ == "__main__": 73 | main() 74 | -------------------------------------------------------------------------------- /homeworks/hw3/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | exec zsh 5 | exec "$@" 6 | -------------------------------------------------------------------------------- /homeworks/hw3/pid.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | 4 | class PID: 5 | 6 | # TODO: Complete the PID class. You may add any additional desired functions 7 | 8 | def __init__(self, Kp, Ki=0.0, Kd=0.0): 9 | # TODO: Initialize PID coefficients (and errors, if needed) 10 | pass 11 | 12 | def UpdateError(self, cte): 13 | # TODO: Update PID errors based on cte 14 | pass 15 | 16 | def TotalError(self): 17 | # TODO: Calculate and return the total error 18 | return 0.0 19 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.black] 2 | line-length = 90 3 | target-version = ["py38"] 4 | 5 | [tool.isort] 6 | profile="black" 7 | line_length = 90 8 | lines_after_imports = 2 9 | 10 | [tool.nbqa.config] 11 | black = "pyproject.toml" 12 | isort = "pyproject.toml" 13 | flake8 = "setup.cfg" 14 | 15 | [tool.nbqa.addopts] 16 | flake8 = ["--extend-ignore=E402"] 17 | 18 | [tool.nbqa.mutate] 19 | black = 1 20 | isort = 1 21 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 100 3 | ignore = E203, E501, W503, B950 4 | max-complexity = 12 5 | select = B, C, E, F, W, B9 6 | -------------------------------------------------------------------------------- /week01_ros_intro/lect_01_introduction_to_robotics_sensors.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week01_ros_intro/lect_01_introduction_to_robotics_sensors.pdf -------------------------------------------------------------------------------- /week01_ros_intro/sem_01_ros_intro.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week01_ros_intro/sem_01_ros_intro.pdf -------------------------------------------------------------------------------- /week02_localization/lect_02_localization.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week02_localization/lect_02_localization.pdf -------------------------------------------------------------------------------- /week02_localization/sem_02_file_system_first_package_communication_types.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week02_localization/sem_02_file_system_first_package_communication_types.pdf -------------------------------------------------------------------------------- /week02_localization/turtle.sh: -------------------------------------------------------------------------------- 1 | # get docker image (~1 Gb) 2 | docker pull osrf/ros:noetic-desktop-full 3 | 4 | 5 | # 1. For Linux 6 | # start image with graphical interface forwarded to host 7 | docker run -it -e DISPLAY=unix$DISPLAY -v /tmp/.X11-unix:/tmp/.X11-unix osrf/ros:noetic-desktop-full 8 | # same as above but destroys container image after shutting down 9 | docker run -it --rm --privileged --net=host -e DISPLAY=$IP:0 -v /tmp/.X11-unix:/tmp/.X11-unix osrf/ros:noetic-desktop-full 10 | 11 | # 2. For MacOS 12 | docker run -it -e DISPLAY=host.docker.internal:0 -v /tmp/.X11-unix:/tmp/.X11-unix osrf/ros:noetic-desktop-full 13 | export LIBGL_ALWAYS_INDIRECT=1 # to enable rendering on host machine, not virtual 14 | 15 | # setup ROS environment for particular shell 16 | env | grep ROS # 17 | 18 | echo $SHELL # /bin/bash (or smth else) 19 | source /opt/ros/noetic/setup.bash 20 | 21 | env | grep ROS # 22 | 23 | 24 | # Start ROS 25 | roscore 26 | # PRESS Ctrl+Z to pass roscore to background 27 | # alternative command: `roscore &` 28 | # to pass process to background from the start 29 | 30 | # check roscore is in background 31 | ps 32 | 33 | # roscore is unique process 34 | roscore # 35 | 36 | 37 | # Check logging 38 | ls -la /root/.ros/log// 39 | 40 | # Alternative way to check latest log 41 | ls -la ~/.ros/log/latest/ 42 | 43 | # Look into log file 44 | less ~/.ros/log/latest/master.log 45 | 46 | 47 | # Multiple terminals for one docker container 48 | # OPEN new terminal window 49 | docker ps # find running docker id 50 | docker exec -it bash 51 | source /opt/ros/noetic/setup.bash # DON'T FORGET!!! 52 | 53 | 54 | # Start turtlesim 55 | rosrun turtlesim turtlesim_node 56 | 57 | 58 | # Launch rqt GUI 59 | rqt_graph 60 | 61 | 62 | # Moving turtle 63 | rosrun turtlesim turtle_teleop_key 64 | 65 | 66 | # CLI instruments 67 | rostopic list 68 | rosnode list 69 | rostopic info /turtle1/cmd_vel 70 | rosmsg info geometry_msgs/Twist 71 | rostopic echo /turtle1/cmd_vel 72 | # now press keys to see what happens 73 | 74 | 75 | # Make turtle make autonomous actions 76 | rosrun turtlesim draw_square 77 | rostopic echo /turtle1/pose 78 | rostopic info /turtle1/pose 79 | rqt_graph 80 | -------------------------------------------------------------------------------- /week03_motion_models/first_node.sh: -------------------------------------------------------------------------------- 1 | # Start in your virtual machine or docker container with ros 2 | sudo apt install build-essential ros-melodic-desktop-full 3 | source /opt/ros/melodic/setup.bash 4 | 5 | # Create dir for project 6 | mkdir first_workspace 7 | cd first_workspace 8 | mkdir src 9 | 10 | # First invocation of this command creates infrastructure for your ROS project 11 | catkin_make # juicy output 12 | 13 | # Add your workspace infrastructure to environment and note difference 14 | env | grep ROS 15 | source devel/setup.bash 16 | env | grep ROS 17 | 18 | # Create package in workspace 19 | cd src 20 | catkin_create_pkg first_package rospy std_msgs # juicy output 21 | # inspect contents 22 | ls first_package 23 | less first_package/package.xml 24 | 25 | # Open file wiht some editor e.g. VSCode 26 | code . --user-data-dir=/vscode 27 | 28 | # Add code files 29 | mv /signal_generator_node.py first_package/src 30 | mv /signal_filter_node.py first_package/src 31 | 32 | # make executable 33 | chmod +x first_package/src/signal_generator_node.py 34 | chmod +x first_package/src/signal_filter_node.py 35 | 36 | # Check package is building well 37 | cd .. 38 | catkin_make 39 | 40 | # Launch ROS and new nodes 41 | roscore 42 | # Ctrl + Z (or another terminal) 43 | rosrun first_package signal_generator_node.py 44 | rosrun first_package signal_filter_node.py 45 | 46 | # Analyze package 47 | rostopic list 48 | rostopic info /signal 49 | rostopic hz /signal 50 | 51 | # Visualize topics 52 | rqt_graph 53 | 54 | # Visualize topic contents on plot 55 | rqt_plot 56 | -------------------------------------------------------------------------------- /week03_motion_models/lect_03_kinematics_probabilistic_motion_models.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week03_motion_models/lect_03_kinematics_probabilistic_motion_models.pdf -------------------------------------------------------------------------------- /week03_motion_models/sem_03_services_actions_parameter_server_roslaunch.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week03_motion_models/sem_03_services_actions_parameter_server_roslaunch.pdf -------------------------------------------------------------------------------- /week03_motion_models/signal_filter_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from collections import deque 4 | 5 | import rospy 6 | from std_msgs.msg import Float32 7 | 8 | 9 | class SignalFilter: 10 | def __init__(self): 11 | # Initializing node with the "signal_filter" name in this process 12 | rospy.init_node("signal_filter") 13 | # Creating subscriber for the signal and publisher for filtered one 14 | self.signal_sub = rospy.Subscriber("signal", Float32, self.signal_callback) 15 | self.signal_pub = rospy.Publisher("filtered_signal", Float32, queue_size=10) 16 | 17 | # Buffer to store last 5 signal values 18 | self.signal_window = deque([], 5) 19 | 20 | def signal_callback(self, signal): 21 | # Logging recieved data 22 | rospy.loginfo("Recieved {}".format(signal.data)) 23 | 24 | # Filtering signal with the moving average and 25 | # publishing filtered signal 26 | self.signal_window.append(signal.data) 27 | filtered_signal = sum(self.signal_window) / len(self.signal_window) 28 | 29 | self.signal_pub.publish(filtered_signal) 30 | 31 | def spin(self): 32 | # Pass control to ROS 33 | try: 34 | rospy.spin() 35 | except rospy.ROSInterruptException: 36 | rospy.logerr("Ctrl+C was pressed!") 37 | 38 | 39 | if __name__ == "__main__": 40 | SignalFilter().spin() 41 | -------------------------------------------------------------------------------- /week03_motion_models/signal_generator_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import rospy 5 | from std_msgs.msg import Float32 6 | 7 | 8 | class SignalGenerator: 9 | def __init__(self): 10 | # Creaytin publisher for the signal 11 | self.signal_pub = rospy.Publisher("signal", Float32, queue_size=10) 12 | 13 | def generate_and_publish_signal(self): 14 | # Generation and publishing sine signal 15 | # dependent on current system time 16 | seconds = rospy.get_time() 17 | new_signal_value = np.sin(seconds) + 0.2 * np.random.randn() 18 | 19 | self.signal_pub.publish(new_signal_value) 20 | 21 | rospy.loginfo("Published {}".format(new_signal_value)) 22 | 23 | def launch_signal_generator(self): 24 | # Initializing node with the "signal_generator" name 25 | rospy.init_node("signal_generator") 26 | 27 | # Setting node (publishing) rate 28 | rate = rospy.Rate(10) 29 | while not rospy.is_shutdown(): 30 | self.generate_and_publish_signal() 31 | # Using sleep to keep the desired rate 32 | rate.sleep() 33 | 34 | def spin(self): 35 | try: 36 | self.launch_signal_generator() 37 | except rospy.ROSInterruptException: 38 | rospy.logerr("Ctrl+C was pressed!") 39 | 40 | 41 | if __name__ == "__main__": 42 | SignalGenerator().spin() 43 | -------------------------------------------------------------------------------- /week04_observation_models/GetWindowMedian.srv: -------------------------------------------------------------------------------- 1 | --- 2 | bool success 3 | float32 median 4 | -------------------------------------------------------------------------------- /week04_observation_models/Signal.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 signal 3 | -------------------------------------------------------------------------------- /week04_observation_models/lect_04_probabilistic_observation_models.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week04_observation_models/lect_04_probabilistic_observation_models.pdf -------------------------------------------------------------------------------- /week04_observation_models/second_node.sh: -------------------------------------------------------------------------------- 1 | # Start in your virtual machine or docker container with ros 2 | sudo apt install build-essential ros-melodic-desktop-full 3 | source /opt/ros/melodic/setup.bash 4 | 5 | # Create dir for project. Just repeat of previous seminar 6 | mkdir second_workspace 7 | cd second_workspace 8 | mkdir src 9 | catkin_make 10 | source devel/setup.bash 11 | cd src 12 | catkin_create_pkg second_package rospy std_msgs 13 | # Add code files 14 | mv /signal_generator_node.py second_package/src 15 | mv /signal_filter_node.py second_package/src 16 | chmod +x second_package/src/signal_generator_node.py 17 | chmod +x second_package/src/signal_filter_node.py 18 | 19 | # NEW: create custom messages 20 | mkdir second_package/msg 21 | mv /Signal.msg second_package/msg/ 22 | 23 | # Add to package.xml: 24 | # message_generation 25 | # message_runtime 26 | 27 | # Add to package CMakeLists.txt: 28 | # find_package(catkin REQUIRED COMPONENTS 29 | # roscpp 30 | # rospy 31 | # std_msgs 32 | # message_generation 33 | # ) 34 | 35 | # And to the same file below: 36 | # catkin_package( 37 | # ... 38 | # CATKIN_DEPENDS message_runtime ... 39 | # ... 40 | # ) 41 | 42 | # Uncomment directive: 43 | # add_message_files( 44 | # FILES 45 | # signal.msg 46 | # ) 47 | 48 | # Uncomment generating messages: 49 | # generate_messages( 50 | # DEPENDENCIES 51 | # std_msgs 52 | # ) 53 | 54 | # Check package is building well 55 | cd .. 56 | catkin_make 57 | 58 | # Look at our beautiful message! 59 | rosmsg show second_package/Signal 60 | 61 | # Launch ROS and new nodes 62 | roscore 63 | # Ctrl + Z (or another terminal) 64 | rosrun second_package signal_generator_node.py 65 | # Ctrl + Z (or another terminal) 66 | rosrun second_package signal_filter_node.py 67 | 68 | rostopic info /signal 69 | 70 | 71 | # Setup own service 72 | # Copy service description to appropriate dir 73 | mkdir src/second_package/srv 74 | cp /GetWindowMedian.srv src/second_package/srv 75 | # Compile new service to ROS 76 | catkin_make 77 | 78 | # Update source files to expose service (add handle_get_median) 79 | # Restart nodes in terminal 80 | 81 | rossrv show second_package/GetWindowMedian 82 | rosservice list 83 | rosservice call /get_median "{}" 84 | -------------------------------------------------------------------------------- /week04_observation_models/sem_04_names_time_debugging_visualization.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week04_observation_models/sem_04_names_time_debugging_visualization.pdf -------------------------------------------------------------------------------- /week04_observation_models/signal_filter_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from collections import deque 4 | 5 | import numpy as np 6 | import rospy 7 | from second_package.msg import Signal 8 | from second_package.srv import GetWindowMedian, GetWindowMedianResponse 9 | 10 | 11 | class SignalFilter: 12 | def __init__(self): 13 | # Initializing node with the "signal_filter" name in this process 14 | rospy.init_node("signal_filter") 15 | # Creating subscriber for the signal and publisher for filtered one 16 | self.signal_sub = rospy.Subscriber("signal", Signal, self.signal_callback) 17 | self.signal_pub = rospy.Publisher("filtered_signal", Signal, queue_size=10) 18 | 19 | self.median_srv = rospy.Service( 20 | "get_median", GetWindowMedian, self.handle_get_median 21 | ) 22 | 23 | # Buffer to store last 5 signal values 24 | self.signal_window = deque([], 5) 25 | 26 | def signal_callback(self, signal): 27 | # Logging recieved data 28 | rospy.loginfo("Recieved {}\nAt {}".format(signal.signal, signal.header.stamp)) 29 | 30 | # Filtering signal with the moving average and 31 | # publishing filtered signal 32 | self.signal_window.append(signal.signal) 33 | # TODO: use Butterworth filter 34 | filtered_signal_value = sum(self.signal_window) / len(self.signal_window) 35 | filtered_signal = Signal() 36 | filtered_signal.signal = filtered_signal_value 37 | filtered_signal.header.stamp = rospy.Time.now() 38 | 39 | self.signal_pub.publish(filtered_signal) 40 | 41 | def handle_get_median(self, request): 42 | median = np.median(self.signal_window) 43 | response = GetWindowMedianResponse() 44 | response.median = median 45 | response.success = True 46 | return response 47 | 48 | def spin(self): 49 | # Pass control to ROS 50 | try: 51 | rospy.spin() 52 | except rospy.ROSInterruptException: 53 | rospy.logerr("Ctrl+C was pressed!") 54 | 55 | 56 | if __name__ == "__main__": 57 | SignalFilter().spin() 58 | -------------------------------------------------------------------------------- /week04_observation_models/signal_generator_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import rospy 5 | from second_package.msg import Signal 6 | 7 | 8 | class SignalGenerator: 9 | def __init__(self): 10 | # Creaytin publisher for the signal 11 | self.signal_pub = rospy.Publisher("signal", Signal, queue_size=10) 12 | 13 | def generate_and_publish_signal(self): 14 | # Generation and publishing sine signal 15 | # dependent on current system time 16 | # WARNING: not safe operation 17 | seconds = rospy.get_time() 18 | new_signal_value = np.sin(seconds) + 0.2 * np.random.randn() 19 | new_signal = Signal() 20 | new_signal.signal = new_signal_value 21 | new_signal.header.stamp = rospy.Time.now() 22 | 23 | self.signal_pub.publish(new_signal) 24 | 25 | rospy.loginfo("Published {}".format(new_signal_value)) 26 | 27 | def launch_signal_generator(self): 28 | # Initializing node with the "signal_generator" name 29 | rospy.init_node("signal_generator") 30 | 31 | # Setting node (publishing) rate 32 | rate = rospy.Rate(10) 33 | while not rospy.is_shutdown(): 34 | self.generate_and_publish_signal() 35 | # Using sleep to keep the desired rate 36 | rate.sleep() 37 | 38 | def spin(self): 39 | try: 40 | self.launch_signal_generator() 41 | except rospy.ROSInterruptException: 42 | rospy.logerr("Ctrl+C was pressed!") 43 | 44 | 45 | if __name__ == "__main__": 46 | SignalGenerator().spin() 47 | -------------------------------------------------------------------------------- /week05_mapping_turtlebot_simulation/lect_05_mapping.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week05_mapping_turtlebot_simulation/lect_05_mapping.pdf -------------------------------------------------------------------------------- /week05_mapping_turtlebot_simulation/signal_filter_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from collections import deque 4 | 5 | import numpy as np 6 | import rospy 7 | from second_package.msg import Signal 8 | from second_package.srv import GetWindowMedian, GetWindowMedianResponse 9 | 10 | 11 | class SignalFilter: 12 | def __init__(self): 13 | # Initializing node with the "signal_filter" name in this process 14 | rospy.init_node("signal_filter") 15 | # Creating subscriber for the signal and publisher for filtered one 16 | self.signal_sub = rospy.Subscriber("signal", Signal, self.signal_callback) 17 | self.signal_pub = rospy.Publisher("filtered_signal", Signal, queue_size=10) 18 | 19 | self.median_srv = rospy.Service( 20 | "get_median", GetWindowMedian, self.handle_get_median 21 | ) 22 | 23 | self.window_size = rospy.get_param("/window_size", 5) 24 | rospy.loginfo("Setup node with window_size {}".format(self.window_size)) 25 | 26 | # Buffer to store last 5 signal values 27 | self.signal_window = deque([], self.window_size) 28 | 29 | def signal_callback(self, signal): 30 | # Logging recieved data 31 | rospy.loginfo("Recieved {}\nAt {}".format(signal.signal, signal.header.stamp)) 32 | 33 | # Filtering signal with the moving average and 34 | # publishing filtered signal 35 | self.signal_window.append(signal.signal) 36 | # TODO: use Butterworth filter 37 | filtered_signal_value = sum(self.signal_window) / len(self.signal_window) 38 | filtered_signal = Signal() 39 | filtered_signal.signal = filtered_signal_value 40 | filtered_signal.header.stamp = rospy.Time.now() 41 | 42 | self.signal_pub.publish(filtered_signal) 43 | 44 | def handle_get_median(self, request): 45 | median = np.median(self.signal_window) 46 | response = GetWindowMedianResponse() 47 | response.median = median 48 | response.success = True 49 | return response 50 | 51 | def spin(self): 52 | # Pass control to ROS 53 | try: 54 | rospy.spin() 55 | except rospy.ROSInterruptException: 56 | rospy.logerr("Ctrl+C was pressed!") 57 | 58 | 59 | if __name__ == "__main__": 60 | SignalFilter().spin() 61 | -------------------------------------------------------------------------------- /week05_mapping_turtlebot_simulation/signal_pipeline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /week05_mapping_turtlebot_simulation/turtle_action.sh: -------------------------------------------------------------------------------- 1 | # Based on tutorial page http://wiki.ros.org/actionlib_tutorials/Tutorials/Calling%20Action%20Server%20without%20Action%20Client 2 | 3 | # Start ROS 4 | roscore 5 | # and turtle 6 | rosrun turtlesim turtlesim_node 7 | # and readymade actioin server 8 | rosrun turtle_actionlib shape_server 9 | 10 | # Check everything has created 11 | rostopic list 12 | rqt_graph 13 | 14 | # Send task to action server 15 | rostopic pub /turtle_shape/goal turtle_actionlib/ShapeActionGoal 16 | # here press to autofill all the info and set: 17 | # edges: 3 18 | # radius: 1.0 19 | 20 | # At another terminal!! 21 | # Look at result 22 | rostopic echo /turtle_shape/result 23 | rostopic echo /turtle_shape/feedback 24 | # hit goal one more time 25 | rostopic pub /turtle_shape/goal .... 26 | 27 | 28 | # Update signal_filter_node.py to use paramter 29 | rosparam set /window_size 10 30 | rosparam get /window_size 10 31 | rosrun second_package signal_generator_node.py 32 | # Ctrl + Z (or another terminal) 33 | rosrun second_package signal_filter_node.py 34 | 35 | # look at changed result 36 | rqt_plot 37 | 38 | 39 | # Launch file createion 40 | mkdir src/second_package/launch 41 | mv /signal_pipeline.launch src/second_package/launch 42 | 43 | # Start process with launch file 44 | roslaunch second_package signal_pipeline.launch 45 | 46 | # Check everything created 47 | rostopic echo /filtered_signal 48 | rqt_graph 49 | -------------------------------------------------------------------------------- /week05_mapping_turtlebot_simulation/turtlebot_simulation.sh: -------------------------------------------------------------------------------- 1 | source /opt/ros/melodic/setup.bash 2 | 3 | sudo apt update 4 | sudo apt install ros-melodic-turtlebot3 5 | cd first_workspace/src 6 | # change branch to your ROS version instead of `melodic-devel` 7 | git clone -b melodic-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git 8 | cd .. 9 | catkin_make 10 | source devel/setup.bash 11 | 12 | 13 | # First terminal: 14 | # TB3_MODEL: burger, waffle, waffle_pi - choose your 15 | export TURTLEBOT3_MODEL=burger 16 | roslaunch turtlebot3_fake turtlebot3_fake.launch 17 | 18 | # Second terminal: 19 | export TURTLEBOT3_MODEL=burger 20 | roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch 21 | 22 | # Third terminal: 23 | rqt 24 | # Go to Visualization -> TF Tree 25 | 26 | 27 | # First terminal: 28 | export TURTLEBOT3_MODEL=waffle_pi 29 | roslaunch turtlebot3_gazebo turtlebot3_house.launch 30 | # Second terminal: 31 | export TURTLEBOT3_MODEL=waffle_pi 32 | roslaunch turtlebot3_gazebo turtlebot3_simulation.launch 33 | # Third terminal: 34 | export TURTLEBOT3_MODEL=waffle_pi 35 | roslaunch turtlebot3_gazebo turtlebot3_gazebo_rviz.launch 36 | -------------------------------------------------------------------------------- /week06_planning_control/lect_06_path planning.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week06_planning_control/lect_06_path planning.pdf -------------------------------------------------------------------------------- /week06_planning_control/lect_07_control algorithms.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/girafe-ai/robotics/85fdb522eb3699c8c962a7f57bbb7bd6c4864f5d/week06_planning_control/lect_07_control algorithms.pdf --------------------------------------------------------------------------------