[ { "path": ".catkin_workspace", "content": "# This file currently only serves to mark the location of a catkin workspace for tool integration\n" }, { "path": ".gitattributes", "content": "# LaTeX\n*.tex text eol=lf\n*.cls text eol=lf\n*.sty text eol=lf\n*.bst text eol=lf\n*.bib text eol=lf\n\n#sources\n*.c text eol=lf\n*.cc text eol=lf\n*.cxx text eol=lf\n*.cpp text eol=lf\n*.c++ text eol=lf\n*.hpp text eol=lf\n*.h text eol=lf\n*.h++ text eol=lf\n*.hh text eol=lf\n\n# Compiled Object files\n*.slo binary\n*.lo binary\n*.o binary\n*.obj binary\n\n# Precompiled Headers\n*.gch binary\n*.pch binary\n\n# Compiled Dynamic libraries\n*.so binary\n*.dylib binary\n*.dll binary\n\n# Compiled Static libraries\n*.lai binary\n*.la binary\n*.a binary\n*.lib binary\n\n# Executables\n*.exe binary\n*.out binary\n*.app binary\n\n\n#common settings that generally should always be used with your language specific settings\n\n# Auto detect text files and perform LF normalization\n# http://davidlaing.com/2012/09/19/customise-your-gitattributes-to-become-a-git-ninja/\n* text=auto\n\n#\n# The above will handle all files NOT found below\n#\n\n# Documents\n*.doc\t diff=astextplain\n*.DOC\t diff=astextplain\n*.docx diff=astextplain\n*.DOCX diff=astextplain\n*.dot diff=astextplain\n*.DOT diff=astextplain\n*.pdf diff=astextplain\n*.PDF\t diff=astextplain\n*.rtf\t diff=astextplain\n*.RTF\t diff=astextplain\n*.md text eol=lf\n*.adoc text\n*.textile text\n*.mustache text\n*.csv text eol=lf\n*.tab text eol=lf\n*.tsv text eol=lf\n*.sql text eol=lf\n\n# Graphics\n*.png binary\n*.jpg binary\n*.jpeg binary\n*.gif binary\n*.tif binary\n*.tiff binary\n*.ico binary\n*.svg binary\n*.eps binary\n# Basic .gitattributes for a MATLAB repo.\n# This template includes Simulink and MuPAD extensions, in addition\n# to the MATLAB extensions.\n\n# Source files\n# ============\n*.m text eol=lf\n*.mu text eol=lf\n\n# Caution: *.m also matches Mathematica packages.\n\n# Binary files\n# ============\n*.p binary\n*.mex* binary\n*.fig binary\n*.mat binary\n*.mdl binary\n*.slx binary\n*.mdlp binary\n*.slxp binary\n*.sldd binary\n*.mltbx binary\n*.mlappinstall binary\n*.mlpkginstall binary\n*.mn binary\n# Basic .gitattributes for a python repo.\n\n# Source files\n# ============\n*.pxd\t\ttext eol=lf\n*.py \t\ttext eol=lf\n*.py3 \t\ttext eol=lf\n*.pyw \t\ttext eol=lf\n*.pyx \t\ttext eol=lf\n\n# Binary files\n# ============\n*.db\t\tbinary\n*.p \t\tbinary\n*.pkl \t\tbinary\n*.pyc \t\tbinary\n*.pyd\t\tbinary\n*.pyo \t\tbinary\n\n# Note: .db, .p, and .pkl files are associated\n# with the python modules ``pickle``, ``dbm.*``,\n# ``shelve``, ``marshal``, ``anydbm``, & ``bsddb``\n# (among others).# Auto detect text files and perform LF normalization\n# http://davidlaing.com/2012/09/19/customise-your-gitattributes-to-become-a-git-ninja/\n* text=auto\n\n# Custom for Visual Studio\n*.sln text eol=crlf\n*.csproj text eol=crlf\n*.vbproj text eol=crlf\n*.fsproj text eol=crlf\n*.dbproj text eol=crlf\n\n*.vcxproj text eol=crlf\n*.vcxitems text eol=crlf\n*.props text eol=crlf\n*.filters text eol=crlf\n\n" }, { "path": ".gitignore", "content": ".vscode/\n\n# Created by https://www.toptal.com/developers/gitignore/api/c++,jupyternotebooks,python,linux,windows,macos,vscode,vim,ros,matlab,libreoffice,microsoftoffice\n# Edit at https://www.toptal.com/developers/gitignore?templates=c++,jupyternotebooks,python,linux,windows,macos,vscode,vim,ros,matlab,libreoffice,microsoftoffice\n\n### C++ ###\n# Prerequisites\n*.d\n\n# Compiled Object files\n*.slo\n*.lo\n*.o\n*.obj\n\n# Precompiled Headers\n*.gch\n*.pch\n\n# Linker files\n*.ilk\n\n# Debugger Files\n*.pdb\n\n# Compiled Dynamic libraries\n*.so\n*.dylib\n*.dll\n\n# Fortran module files\n*.mod\n*.smod\n\n# Compiled Static libraries\n*.lai\n*.la\n*.a\n*.lib\n\n# Executables\n*.exe\n*.out\n*.app\n\n### JupyterNotebooks ###\n# gitignore template for Jupyter Notebooks\n# website: http://jupyter.org/\n\n.ipynb_checkpoints\n*/.ipynb_checkpoints/*\n\n# IPython\nprofile_default/\nipython_config.py\n\n# Remove previous ipynb_checkpoints\n# git rm -r .ipynb_checkpoints/\n\n### LibreOffice ###\n# LibreOffice locks\n.~lock.*#\n\n### Linux ###\n*~\n\n# temporary files which can be created if a process still has a handle open of a deleted file\n.fuse_hidden*\n\n# KDE directory preferences\n.directory\n\n# Linux trash folder which might appear on any partition or disk\n.Trash-*\n\n# .nfs files are created when an open file is removed but is still being accessed\n.nfs*\n\n### macOS ###\n# General\n.DS_Store\n.AppleDouble\n.LSOverride\n\n# Icon must end with two \\r\nIcon\n\n\n# Thumbnails\n._*\n\n# Files that might appear in the root of a volume\n.DocumentRevisions-V100\n.fseventsd\n.Spotlight-V100\n.TemporaryItems\n.Trashes\n.VolumeIcon.icns\n.com.apple.timemachine.donotpresent\n\n# Directories potentially created on remote AFP share\n.AppleDB\n.AppleDesktop\nNetwork Trash Folder\nTemporary Items\n.apdisk\n\n### MATLAB ###\n# Windows default autosave extension\n*.asv\n\n# OSX / *nix default autosave extension\n*.m~\n\n# Compiled MEX binaries (all platforms)\n*.mex*\n\n# Packaged app and toolbox files\n*.mlappinstall\n*.mltbx\n\n# Generated helpsearch folders\nhelpsearch*/\n\n# Simulink code generation folders\nslprj/\nsccprj/\n\n# Matlab code generation folders\ncodegen/\n\n# Simulink autosave extension\n*.autosave\n\n# Simulink cache files\n*.slxc\n\n# Octave session info\noctave-workspace\n\n### MicrosoftOffice ###\n*.tmp\n\n# Word temporary\n~$*.doc*\n\n# Word Auto Backup File\nBackup of *.doc*\n\n# Excel temporary\n~$*.xls*\n\n# Excel Backup File\n*.xlk\n\n# PowerPoint temporary\n~$*.ppt*\n\n# Visio autosave temporary files\n*.~vsd*\n\n### Python ###\n# Byte-compiled / optimized / DLL files\n__pycache__/\n*.py[cod]\n*$py.class\n\n# C extensions\n\n# Distribution / packaging\n.Python\nbuild/\ndevelop-eggs/\ndist/\ndownloads/\neggs/\n.eggs/\nlib/\nlib64/\nparts/\nsdist/\nvar/\nwheels/\npip-wheel-metadata/\nshare/python-wheels/\n*.egg-info/\n.installed.cfg\n*.egg\nMANIFEST\n\n# PyInstaller\n# Usually these files are written by a python script from a template\n# before PyInstaller builds the exe, so as to inject date/other infos into it.\n*.manifest\n*.spec\n\n# Installer logs\npip-log.txt\npip-delete-this-directory.txt\n\n# Unit test / coverage reports\nhtmlcov/\n.tox/\n.nox/\n.coverage\n.coverage.*\n.cache\nnosetests.xml\ncoverage.xml\n*.cover\n*.py,cover\n.hypothesis/\n.pytest_cache/\npytestdebug.log\n\n# Translations\n*.mo\n*.pot\n\n# Django stuff:\n*.log\nlocal_settings.py\ndb.sqlite3\ndb.sqlite3-journal\n\n# Flask stuff:\ninstance/\n.webassets-cache\n\n# Scrapy stuff:\n.scrapy\n\n# Sphinx documentation\ndocs/_build/\ndoc/_build/\n\n# PyBuilder\ntarget/\n\n# Jupyter Notebook\n\n# IPython\n\n# pyenv\n.python-version\n\n# pipenv\n# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control.\n# However, in case of collaboration, if having platform-specific dependencies or dependencies\n# having no cross-platform support, pipenv may install dependencies that don't work, or not\n# install all needed dependencies.\n#Pipfile.lock\n\n# PEP 582; used by e.g. github.com/David-OConnor/pyflow\n__pypackages__/\n\n# Celery stuff\ncelerybeat-schedule\ncelerybeat.pid\n\n# SageMath parsed files\n*.sage.py\n\n# Environments\n.env\n.venv\nenv/\nvenv/\nENV/\nenv.bak/\nvenv.bak/\npythonenv*\n\n# Spyder project settings\n.spyderproject\n.spyproject\n\n# Rope project settings\n.ropeproject\n\n# mkdocs documentation\n/site\n\n# mypy\n.mypy_cache/\n.dmypy.json\ndmypy.json\n\n# Pyre type checker\n.pyre/\n\n# pytype static type analyzer\n.pytype/\n\n# profiling data\n.prof\n\n### ROS ###\n.catkin_tools/\ndevel/\nlogs/\nbin/\nmsg_gen/\nsrv_gen/\nmsg/*Action.msg\nmsg/*ActionFeedback.msg\nmsg/*ActionGoal.msg\nmsg/*ActionResult.msg\nmsg/*Feedback.msg\nmsg/*Goal.msg\nmsg/*Result.msg\nmsg/_*.py\nbuild_isolated/\ndevel_isolated/\n\n# Generated by dynamic reconfigure\n*.cfgc\n/cfg/cpp/\n/cfg/*.py\n\n# Ignore generated docs\n*.dox\n*.wikidoc\n\n# eclipse stuff\n.project\n.cproject\n\n# qcreator stuff\nCMakeLists.txt.user\n\nsrv/_*.py\n*.pcd\n*.pyc\nqtcreator-*\n*.user\n\n/planning/cfg\n/planning/docs\n/planning/src\n\n\n# Emacs\n.#*\n\n# Catkin custom files\nCATKIN_IGNORE\n\n### Vim ###\n# Swap\n[._]*.s[a-v][a-z]\n!*.svg # comment out if you don't need vector files\n[._]*.sw[a-p]\n[._]s[a-rt-v][a-z]\n[._]ss[a-gi-z]\n[._]sw[a-p]\n\n# Session\nSession.vim\nSessionx.vim\n\n# Temporary\n.netrwhist\n# Auto-generated tag files\ntags\n# Persistent undo\n[._]*.un~\n\n### ROS2 ###\ninstall/\nlog/\nbuild/\n\n# Ignore generated docs\n*.dox\n*.wikidoc\n\n# eclipse stuff\n.project\n.cproject\n\n# qcreator stuff\nCMakeLists.txt.user\n\nsrv/_*.py\n*.pcd\n*.pyc\nqtcreator-*\n*.user\n\n*~\n\n# Emacs\n.#*\n\n# Colcon custom files\nCOLCON_IGNORE\nAMENT_IGNORE\n\n\n### vscode ###\n.vscode/*\n!.vscode/settings.json\n!.vscode/tasks.json\n!.vscode/launch.json\n!.vscode/extensions.json\n*.code-workspace\n\n### Windows ###\n# Windows thumbnail cache files\nThumbs.db\nThumbs.db:encryptable\nehthumbs.db\nehthumbs_vista.db\n\n# Dump file\n*.stackdump\n\n# Folder config file\n[Dd]esktop.ini\n\n# Recycle Bin used on file shares\n$RECYCLE.BIN/\n\n# Windows Installer files\n*.cab\n*.msi\n*.msix\n*.msm\n*.msp\n\n# Windows shortcuts\n*.lnk\n\n# End of https://www.toptal.com/developers/gitignore/api/c++,jupyternotebooks,python,linux,windows,macos,vscode,vim,ros,matlab,libreoffice,microsoftoffice\n" }, { "path": ".gitmodules", "content": "[submodule \"libraries/eigen\"]\n\tpath = libraries/eigen\n\turl = https://github.com/fdcl-gwu/eigen.git\n" }, { "path": ".travis.yml", "content": "language: python\npython:\n - \"3.9\"\n\n# command to install dependencies\ninstall:\n - pip install -r requirements.txt\n\n# command to run tests\nscript: \n - python -m unittest\n" }, { "path": "Dockerfile", "content": "FROM ros:iron-ros-core-jammy\nRUN apt update\nRUN apt -y install python3-pip tmux\nRUN apt -y install python3-colcon-common-extensions\nRUN apt -y install ros-iron-gazebo-ros-pkgs ros-iron-gazebo-ros2-control ros-iron-xacro\nRUN python3 -m pip install pip --upgrade\nRUN python3 -m pip install numpy pandas matplotlib\nRUN python3 -m pip install PyQt5\n\nRUN adduser --disabled-password sim\nUSER sim\n\nWORKDIR /home\n" }, { "path": "License", "content": "MIT License\n\nCopyright (c) 2020 Flight Dynamics and Control Lab\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\nof this software and associated documentation files (the \"Software\"), to deal\nin the Software without restriction, including without limitation the rights\nto use, copy, modify, merge, publish, distribute, sublicense, and/or sell\ncopies of the Software, and to permit persons to whom the Software is\nfurnished to do so, subject to the following conditions:\n\nThe above copyright notice and this permission notice shall be included in all\ncopies or substantial portions of the Software.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\nIMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\nFITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE\nAUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\nLIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,\nOUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE\nSOFTWARE." }, { "path": "README.md", "content": "|![TravisCI](https://img.shields.io/badge/travis%20ci-%232B2F33.svg?style=for-the-badge&logo=travis&logoColor=white) | ![Docker](https://img.shields.io/badge/docker-%230db7ed.svg?style=for-the-badge&logo=docker&logoColor=white)|\n|-|-|\n|[![Build Status](https://app.travis-ci.com/fdcl-gwu/uav_simulator.svg?branch=main)](https://app.travis-ci.com/fdcl-gwu/uav_simulator) | [tag:ros2-iron](https://hub.docker.com/layers/kanishgama/uav_simulator/ros2-iron/images/sha256-a18af779d63b68b9d6c3b7c522274f629c28e5de9da088285ce2ea7f12ee2d3c?context=repo) |\n\n# Python - Gazebo Simulation Environment for a UAV with Geometric Control\n\nThis repository includes Python codes for the position control a UAV in a Gazebo simulation environment, using [geometric controllers](https://github.com/fdcl-gwu/uav_geometric_control).\n\n## Features\n* Developed using Python\n* Uses a geometric controller that works great with aggressive maneuvers\n* Uses Gazebo as the physics engine\n* Has a nice GUI for controlling the UAV\n* Estimator, controller, and trajectory generators are in their own ROS nodes. If you need to test your own estimator, controller, or a trajectory, you only need to modify the respective node.\n\n![Landing](images/trajectory.gif)\n\n### Why Python?\n* Python makes developing/debugging easier and shorter (no compiling)\n* Can easily find open-source modules or libraries for different tasks\n\n## Which controller is used for the UAV control?\n* A geometric controller with decoupled-yaw attitude control is used\n* The controller is published in:\n ```sh\n @InProceedings{Gamagedara2019b,\n title={Geometric controls of a quadrotor uav with decoupled yaw control},\n author={Gamagedara, Kanishke and Bisheban, Mahdis and Kaufman, Evan and Lee, Taeyoung},\n booktitle={2019 American Control Conference (ACC)},\n pages={3285--3290},\n year={2019},\n organization={IEEE}\n }\n ```\n* Implementation of the same controller in C++ and Matlab can be found at [https://github.com/fdcl-gwu/uav_geometric_control](https://github.com/fdcl-gwu/uav_geometric_control)\n\n\n\n## Which estimator is used for the state estimation?\n* The estimator defined in the following paper is implemented here (except for the sensor bias estimation terms):\n ```sh\n @InProceedings{Gamagedara2019a,\n author = {Kanishke Gamagedara and Taeyoung Lee and Murray R. Snyder},\n title = {Real-time Kinematics {GPS} Based Telemetry System for Airborne Measurements of Ship Air Wake},\n booktitle = {{AIAA} Scitech 2019 Forum},\n year = {2019},\n month = {jan},\n publisher = {American Institute of Aeronautics and Astronautics},\n doi = {10.2514/6.2019-2377}\n }\n ```\n* Matlab implementation of the above controller can be found at [https://github.com/fdcl-gwu/dkf-comparison](https://github.com/fdcl-gwu/dkf-comparison).\n* Note that the Matlab implementation has a delayed Kalman filter that has not been implemented here. Only the non-delayed parts inside `DelayedKalmanFilter.m` is utilized here.\n\n\n\n## Setting-up\n\n### Releases\n\nThe current `main` branch has been tested to work on Ubuntu 22.04, running ROS2-Iron. Check [releases](https://github.com/fdcl-gwu/uav_simulator/releases) for different OS/ROS versions.\n1. [v1.0](https://github.com/fdcl-gwu/uav_simulator/tree/v1.0): Ubuntu 18.04 with ROS-Melodic\n1. [v2.0](https://github.com/fdcl-gwu/uav_simulator/tree/v2.0): Ubuntu 20.04 with ROS-Noetic\n1. [v3.0](https://github.com/fdcl-gwu/uav_simulator/tree/v3.0): Ubuntu 22.04 with ROS2-Iron\n\n:bangbang: If you are trying to use an older release, please checkout that release, and use setup instructions there.\nEach release has different instructions.\n\n:bangbang: If you are running this on a virtual machine, please make sure that Gazebo can run at real-time speed.\nIt is known that this simulation exhibits unintended behavior if the \"real-time factor\" of the Gazebo simulation is not closer to 1.0 (See [issue #3](https://github.com/fdcl-gwu/uav_simulator/issues/3)).\n\n### Setting-up the repository\n1. Clone the repository.\n ```sh\n git clone https://github.com/fdcl-gwu/uav_simulator.git\n ```\n1. Update the submodules.\n ```sh\n cd uav_simulator\n git submodule update --init --recursive\n ```\n\n### Dependencies\n\nYou have to options here:\n1. Installing everything locally\n1. Running a docker container\n\nInstalling everything locally is probably the most straight-forward way, but you have to instal dependencies manually, or may have to deal with package version changes. \nDocker solves this by streamlining all the dependencies, up-to the OS.\nFor example, if you are on Ubuntu 22.04 and want to test the ROS-Melodic version, docker will be the only way.\n\nIf you want to install everything locally, follow [Local Install](#local-install).\nIf you want to run a docker container instead, skip to [Docker Setup](#docker-setup).\n\n#### Local Install\n1. [ROS2](http://wiki.ros.org/): this repository has been developed using ROS2 Iron, on Ubuntu 22.04. If you are on a different version of Ubunto or ROS, please check the previous releases before installing dependencies. We recommend installing the ROS2 full version.\n\n1. Python modules: these libraries must be installed in the system\n 1. NumPy\n 1. Pandas\n 1. Matplotlib\n ```sh\n python3 -m pip install numpy pandas matplotlib\n ```\n\nNow, skip to [Setting-up the plugins and Gazebo](#setting-up-the-plugins-and-gazebo).\n\n#### Docker Setup\n\nThe instructions here assume you are on Ubuntu.\nThis has not been tested on other OS versions.\n\n1. Install docker following [official instructions](https://docs.docker.com/engine/install/ubuntu/).\n1. If you are not already there, `cd uav_simulator`\n1. Enable xhost (required for Gazebo and GUI): `xhost +`\n1. Build the docker image: `docker build -t uav_simulator .` (see following paragraph if you just want to pull the already built image instead)\n1. Run a container: `bash docker_run.sh`\n\nThe last command will start a docker container, install all the dependencies, and mount the local directory there.\nThe first time you run the build command will take a while as it installs all the libraries.\n\nYou can skip the build command altogether by pulling the built docker from the Docker Hub with the following command.\nThis is NOT required if you are building it locally using the build command.\n```sh\ndocker pull kanishgama/uav_simulator:ros2-iron\nbash docker_run.sh\n```\n\nAfter that, you only need to run the `bash docker_run.sh` every time you need to run the simulation.\nSince this mounts the local repository inside the docker, you just need to change the code in your local repository, and it will be automatically update inside the docker.\n\nFor running the code, simply follow [Setting-up the plugins and Gazebo](#setting-up-the-plugins-and-gazebo), and onwards.\n\n\n### Setting-up the plugins and Gazebo\nYou only need to do the followings once (unless you change the Gazebo plugins)\n1. Make the plugging.\n ```sh\n # From uav_simulator\n colcon build\n ```\n1. Source the relevant directories (**NOTE**: you need to do this on every new terminal).\n ```sh\n # From uav_simulator\n source install/local_setup.bash\n ```\n\n### Running the simulation environment \n1. In the current terminal window, launch the Gazebo environment:\n ```sh\n # From uav_simulator\n ros2 launch uav_gazebo uav_gazebo.launch.py \n ```\n1. Once the Gazebo is launched, run the UAV code from a different terminal (if you already don't know, you may find [**tmux**](https://github.com/tmux/tmux/wiki) a life-saver):\n ```sh\n # From uav_simulator\n ros2 launch fdcl_uav fdcl_uav_launch.py\n ```\n\n Everytime you change the Python code, run the following commands\n ```sh\n # From uav_simulator\n colcon build --packages-select fdcl_uav\n ros2 launch fdcl_uav fdcl_uav_launch.py\n ```\n The code has been tested with Python3.10.12, which comes default with Ubuntu 22.04.\n\n\n![Terminal](images/running.gif)\n\n### Tips\n1. Every time you change the simulation environment, you have to kill the program, `colcon build` and re-run it. \n1. If you do not make any changes to the simulation environment, you only need to kill the Python program. \n\n\n## Control Guide\n* Simply click on the buttons on the GUI to control the UAV.\n* You can easily switch between each trajectory mode simply clicking on the radio buttons.\n* Stay mode simply commands the UAV to stay at the current position.\n* When take-off, stay, and circle trajectories end, the UAV switches to the \"manual\" mode.\n* When the UAV is in manual, you can use following keys (these are not case sensitive):\n * `WASD`: to move in horizontal plane\n * `P`: increase altitude\n * `L`: decrease altitude\n * `Q`: yaw rotation in anti-clockwise direction\n * `E`: yaw rotation in clockwise direction\n* At any point of the flight, you can use following keys (these are not case sensitive):\n * `M`: kill motors\n * `0-5`: set the flight mode without clicking on the GUI\n* Please not that the GUI must be in focus for any of the above keys to work.\n* If you want to change the above keyboard shortcuts, you can do so by editing `on_key_press` function in `gui.py`.\n\n## Running Unit-Tests\n* Make sure you are in the main directory.\n* Run `python -m unittest`.\n* Unit tests have only been tested on Python 3.9.\n* Currently, unit test only covers the `matrix_utils.py` module. \n\n" }, { "path": "docker_release.sh", "content": "docker build --tag -t uav_simulator .\ndocker tag uav_simulator kanishgama/uav_simulator:\ndocker push kanishgama/uav_simulator:" }, { "path": "docker_run.sh", "content": "echo \"Starting docker image ..\"\n\nif [ \"$(docker ps -a -q -f name=uav-simulator-container)\" ]; \nthen\n echo \"Previously created container exists, starting it ..\"\n docker start --interactive uav-simulator-container\nelse\n echo \"No previously created container, creating a new one ..\"\n\n docker run \\\n --name uav-simulator-container \\\n --mount source=\"$(pwd)\",target=/home/uav_simulator,type=bind \\\n --net host \\\n -v /tmp/.X11-unix:/tmp/.X11-unix \\\n -u sim \\\n -e DISPLAY=unix$DISPLAY \\\n --privileged \\\n -it uav_simulator bash\nfi\n\necho \"Docker run complete\"" }, { "path": "scripts/data_logs/.gitignore", "content": "*.txt" }, { "path": "scripts/plot_utils.py", "content": "import itertools\nimport matplotlib.pyplot as plt\nimport numpy as np\nimport pandas as pd\nimport pdb\n\n# plt.style.use('seaborn')\n\ndef plot_data():\n file_name = get_file_name()\n data = Data(file_name)\n\n t = data.t / 1000.0\n plot_31_2(t, data.x, t, data.xd, title='x', legend=['Estimated', 'Desired'])\n plot_33_2(t, data.R, t, data.Rd, title='R', legend=['Estimated', 'Desired'])\n\n print('Press Q to close each figure.')\n plt.show()\n\n\ndef get_file_name():\n with open('data_logs/last_log.txt', 'r') as f:\n return f.readline()\n\n\ndef plot_31_1(x, y, x_label='', y_label=['1', '2', '3'], title=''):\n\n fig, ax = plt.subplots(3, 1)\n fig.suptitle(r'${{{}}}$'.format(title), fontsize=12)\n\n for i in range(3):\n ax[i].plot(x[:], y[i][:], 'r')\n ax[i].set_ylabel(r'${{{}}}$'.format(y_label[i]))\n ax[i].grid('on')\n\n\ndef plot_31_2(x1, y1, x2, y2, x_label='', y_label=['', '', ''], title='',\n legend=['', '']):\n\n fig, ax = plt.subplots(3, 1)\n fig.suptitle(r'${{{}}}$'.format(title), fontsize=12)\n\n for i in range(3):\n ax[i].plot(x1[:], y1[i][:], 'r', label=legend[0])\n ax[i].plot(x2[:], y2[i][:], 'g', label=legend[1])\n ax[i].set_ylabel(r'${{{}}}$'.format(y_label[i]))\n ax[i].set_xlabel(r'${{{}}}$'.format(x_label))\n ax[i].grid('on')\n\n if not legend[0] == '':\n ax[0].legend()\n\n\ndef plot_33_1(x, y, x_label='', title=''):\n\n fig, ax = plt.subplots(3, 3)\n fig.suptitle(r'${{{}}}$'.format(title), fontsize=12)\n\n for i, j in list(itertools.product(range(0, 3), range(0, 3))):\n ax[i, j].plot(x[:], y[i, j, :], 'r')\n ax[i, j].set_ylim((-1.2, 1.2))\n ax[i, j].grid('on')\n\n\ndef plot_33_2(x1, y1, x2, y2, x_label='', title='', legend=['', '']):\n \n fig, ax = plt.subplots(3, 3)\n fig.suptitle(r'${{{}}}$'.format(title), fontsize=12)\n \n for i, j in list(itertools.product(range(0, 3), range(0, 3))):\n ax[i, j].plot(x1[:], y1[i, j, :], 'r', label=legend[0])\n ax[i, j].plot(x2[:], y2[i, j, :], 'g', label=legend[1])\n ax[i, j].set_ylim((-1.2, 1.2))\n ax[i, j].grid('on')\n \n if not legend[0] == '':\n ax[0, 0].legend()\n\n\nclass Data:\n def __init__(self, file_name):\n self.data = pd.read_csv(file_name, header=0, skipinitialspace=True)\n \n self.t_system = self.data['time']\n self.t = self.data['t']\n N = len(self.t)\n\n self.x = self.get_data_3x1('x')\n self.v = self.get_data_3x1('v')\n self.a = self.get_data_3x1('a')\n self.W = self.get_data_3x1('W')\n\n self.xd = self.get_data_3x1('xd')\n self.vd = self.get_data_3x1('xd_dot')\n self.b1d = self.get_data_3x1('b1d')\n\n self.R = self.rot_mat_from_df('R', N)\n self.Rd = self.rot_mat_from_df('Rd', N)\n\n def get_data_3x1(self, var_name):\n return [self.data['{}_0'.format(var_name)],\n self.data['{}_1'.format(var_name)],\n self.data['{}_2'.format(var_name)]]\n\n\n def rot_mat_from_df(self, mat, N):\n R = np.zeros((3, 3, N))\n\n for i in range(0, N):\n for j, k in list(itertools.product(range(0, 3), range(0, 3))):\n R[j, k, i] = self.data['{}_{}{}'.format(mat, j, k)][i]\n\n return R\n\nif __name__ == '__main__':\n plot_data()\n" }, { "path": "scripts/thread_log.py", "content": "from rover import rover\n\nimport datetime\nimport numpy as np\nimport rospy\n\n\ndef thread_log():\n print('LOG: thread starting ..')\n\n freq = 50.0\n t0 = datetime.datetime.now()\n t = datetime.datetime.now()\n t_pre = datetime.datetime.now()\n avg_number = 100\n\n header_written = False\n file_open = False\n file_name = rover.t0.strftime('data_logs/log_%Y%m%d_%H%M%S.txt')\n\n rate = rospy.Rate(freq)\n\n while not rospy.is_shutdown() and rover.on:\n t = datetime.datetime.now()\n dt = (t - t_pre).total_seconds()\n if dt < 1e-6:\n continue\n\n freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number\n t_pre = t\n rover.freq_log = freq\n \n dt_millis = t - t0\n t_millis = int(dt_millis.seconds * 1e3 + dt_millis.microseconds / 1e3)\n\n if rover.save_on:\n if not header_written:\n header_written = True\n write_header(file_name)\n else:\n if not file_open:\n f = open(file_name, 'a')\n file_open = True\n write_date(f, t_millis)\n\n rate.sleep()\n\n if file_open:\n f.close()\n\n\n print('LOG: thread closed!')\n\n\ndef write_header(file_name):\n # NOTE: header order and the data order must be of the same order.\n with open(file_name, 'w') as f:\n f.write('time,')\n f.write('t,')\n \n f.write(string_vector('x'))\n f.write(string_vector('v'))\n f.write(string_vector('a'))\n f.write(string_vector('W'))\n f.write(string_3x3('R'))\n\n f.write(string_vector('xd'))\n f.write(string_vector('xd_dot'))\n f.write(string_vector('b1d'))\n f.write(string_vector('Wd'))\n f.write(string_3x3('Rd'))\n\n f.write('\\n')\n\n with open('data_logs/last_log.txt', 'w') as f:\n f.write(file_name)\n\n\ndef write_date(f, t_millis):\n # NOTE: header order and the data order must be of the same order.\n write_scalar(f, datetime.datetime.now().strftime('%H%M%S.%f'))\n write_scalar(f, t_millis)\n\n write_vector(f, rover.x)\n write_vector(f, rover.v)\n write_vector(f, rover.a)\n write_vector(f, rover.W)\n write_3x3(f, rover.R)\n\n write_vector(f, rover.control.xd)\n write_vector(f, rover.control.xd_dot)\n write_vector(f, rover.control.b1d)\n write_vector(f, rover.control.Wd)\n write_3x3(f, rover.control.Rd)\n \n f.write('\\n')\n\n\ndef string_vector(name, length=3):\n out = ''\n for i in range(length):\n out += '{}_{},'.format(name, i)\n return out\n\n\ndef string_3x3(name):\n out = ''\n for i in range(3):\n for j in range(3):\n out += '{}_{}{},'.format(name, i, j)\n return out\n\n\ndef write_scalar(f, data):\n f.write('{},'.format(data))\n\n\ndef write_vector(f, data, length=3):\n line = ''\n for i in range(length):\n line += '{},'.format(data[i])\n f.write(line)\n\n\ndef write_3x3(f, data):\n for i in range(3):\n for j in range(3):\n f.write('{},'.format(data[i, j]))\n" }, { "path": "src/fdcl_uav/fdcl_uav/__init__.py", "content": "" }, { "path": "src/fdcl_uav/fdcl_uav/control.py", "content": "from .matrix_utils import hat, vee, deriv_unit_vector, saturate\nfrom .integral_utils import IntegralError, IntegralErrorVec3\n\nimport datetime\nimport numpy as np\n\nimport rclpy\nfrom rclpy.node import Node\n\nfrom geometry_msgs.msg import Vector3, WrenchStamped\nfrom std_msgs.msg import Int32, Bool\n\nfrom uav_gazebo.msg import DesiredData, ErrorData, ModeData, StateData\n\n\nclass ControlNode(Node):\n \"\"\"Controller for the UAV trajectory tracking.\n\n This class detemines the control outputs for a quadrotor UAV given it's\n current state and the desired states.\n\n Attributes:\n t0: (datetime object) time at epoch\n t: (float) current time since epoch [s]\n t_prev: (float) time since epoch in the previous loop [s]\n dt: (float) time difference between two calls to the controller [s]\n\n x: (3x1 numpy array) current position of the UAV [m]\n v: (3x1 numpy array) current velocity of the UAV [m/s]\n a: (3x1 numpy array) current acceleration of the UAV [m/s^s]\n R: (3x3 numpy array) current attitude of the UAV in SO(3)\n W: (3x1 numpy array) current angular velocity of the UAV [rad/s]\n\n xd: (3x1 numpy array) desired position of the UAV [m]\n xd_dot: (3x1 numpy array) desired velocity of the UAV [m/s]\n xd_2dot: (3x1 numpy array) desired accleration of the UAV [m/s^2]\n xd_3dot: (3x1 numpy array) desired third derivative of the UAV \n position [m/s^3]\n xd_4dot: (3x1 numpy array) desired fourth derivative of the UAV \n position [m/s^4]\n\n b1d: (3x1 numpy array) desired direction of the first body axis\n b1d_dot: (3x1 numpy array) first time derivative of the desired \n direction of the first body axis\n b1d_2dot: (3x1 numpy array) second time desired direction of the first \n body axis\n\n Wd: (3x1 numpy array) desired body angular velocity [rad/s]\n Wd_dot: (3x1 numpy array) desired body angular acceleration [rad/s^2]\n\n Rd: (3x3 numpy array) desired attitude in SO(3)\n\n b3d: (3x1 numpy array) desired direction of the third body axis\n b3d_dot: (3x1 numpy array) first time derivative of the desired \n direction of the third body axis\n b3d_2dot: (3x1 numpy array) second time desired direction of the third \n body axis\n\n b1c: (3x1 numpy array) calculated direction of the first body axis\n wc3: (float) angular velocity around third body axis [rad/s]\n wc3_dot: (float) angular acceleration around third body axis [rad/s^s]\n\n use_integral: (bool) flag to enable/disable integral control \n\n e1 = (3x1 numpy array) direction of the e1 axis\n e2 = (3x1 numpy array) direction of the e2 axis\n e3 = (3x1 numpy array) direction of the e3 axis\n\n m: (float) mass of the rover [kg]\n g: (float) gravitational acceleration [m/s^2]\n c_tf: (float) torsional moment generated by the propellers\n l: (float) length of the rover arm [m]\n J: (3x3 numpy array) inertia matrix of the rover\n\n kR: (3x3 numpy array) attitude gains\n kW: (3x3 numpy array) angular rate gains\n\n ky: (float) yaw gain for decoupled-yaw controller\n kwy: (float) yaw angular rate gain for decoupled-yaw controller\n\n kX: (3x3 numpy array) position gains\n kV: (3x3 numpy array) velocity gains\n\n kIR: (float) attitude integral gain\n ki: (float) position integral gain\n kI: (float) ttitude integral gain for roll and pitch\n kyI: (float) attitude integral gain for yaw\n kIX: (float) position integral gains\n c1: (float) parameters for decoupled-yaw integral control\n c2: (float) parameters for decoupled-yaw integral control\n c3: (float) parameters for decoupled-yaw integral control\n\n fM: (4x1 numpy array) force-moment vector\n f_total: (float) calculated forces required by each moter\n\n fM_to_forces: (4x4 numpy array) force to force-moment conversion\n matrix\n fM_to_forces_inv: (4x4 numpy array) force-moment to force conversion\n matrix\n\n eIX: (IntegralErrorVec3) position integral error\n eIR: (IntegralErrorVec3) attitude integral error\n eI1: (IntegralError) attitude integral error for roll axis\n eI2: (IntegralError) attitude integral error for pitch axis\n eIy: (IntegralError) attitude integral error for yaw axis\n eIX: (IntegralError) position integral error\n\n sat_sigma\n \"\"\"\n\n def __init__(self):\n Node.__init__(self, 'control')\n\n self.t0 = self.get_clock().now()\n self.t = 0.0\n self.t_pre = 0.0\n self.dt = 1e-9\n\n # Current state\n self.x = np.zeros(3)\n self.v = np.zeros(3)\n self.a = np.zeros(3)\n self.R = np.identity(3)\n self.W = np.zeros(3)\n\n # Desired states\n self.xd = np.zeros(3)\n self.xd_dot = np.zeros(3)\n self.xd_2dot = np.zeros(3)\n self.xd_3dot = np.zeros(3)\n self.xd_4dot = np.zeros(3)\n\n self.b1d = np.zeros(3)\n self.b1d[0] = 1.0\n self.b1d_dot = np.zeros(3)\n self.b1d_2dot = np.zeros(3)\n\n self.Wd = np.zeros(3)\n self.Wd_dot = np.zeros(3)\n\n self.Rd = np.identity(3)\n\n self.b3d = np.zeros(3)\n self.b3d_dot = np.zeros(3)\n self.b3d_2dot = np.zeros(3)\n\n self.b1c = np.zeros(3)\n self.wc3 = 0.0\n self.wc3_dot = 0.0\n\n # Flag to enable/disable integral control \n self.use_integral = False\n\n self.e1 = np.zeros(3)\n self.e1[0] = 1.0\n self.e2 = np.zeros(3)\n self.e2[1] = 1.0\n self.e3 = np.zeros(3)\n self.e3[2] = 1.0\n\n self.m = 1.95 # Mass of the rover (kg)\n self.g = 9.81 # Gravitational acceleration (m/s^2)\n self.c_tf = 0.0135 # Torsional moment generated by the propellers\n self.l = 0.23 # Length of the rover arm (m)\n self.J = np.diag([0.02, 0.02, 0.04]) # Inertia matrix of the rover\n\n # Attitude gains\n self.kR = np.diag([1.6, 1.6, 0.6]) # Attitude gains\n self.kW = np.diag([0.40, 0.40, 0.10]) # Angular rate gains\n\n self.ky = self.kR[2, 2] # Yaw gain for decoupled-yaw controller\n self.kwy = self.kR[2, 2] # Yaw angular rate gain for decoupled-yaw \n # controller\n\n # Position gains\n self.kX = np.diag([16.0, 16.0, 20.0]) # Position gains\n self.kV = np.diag([12.0, 12.0, 12.0]) # Velocity gains\n\n # Integral gains\n self.kIR = 0.015 # Attitude integral gain\n self.ki = 0.01 # Position integral gain\n self.kI = 0.01 # Attitude integral gain for roll and pitch\n self.kyI = 0.02 # Attitude integral gain for yaw\n self.kIX = 4.0 # Position integral gains\n self.c1 = 1.0 # Parameters for decoupled-yaw integral control\n self.c2 = 1.0 # Parameters for decoupled-yaw integral control\n self.c3 = 1.0 # Parameters for decoupled-yaw integral control\n\n self.fM = np.zeros((4, 1)) # Force-moment vector\n self.f_total = 0.0 # Calculated forces required by each moter\n\n fM_to_forces = np.array([\n [1.0, 1.0, 1.0, 1.0],\n [0.0, -self.l, 0.0, self.l],\n [self.l, 0.0, -self.l, 0.0],\n [-self.c_tf, self.c_tf, -self.c_tf, self.c_tf]\n ])\n self.fM_to_forces_inv = np.linalg.inv(fM_to_forces) # Force to \n # force-moment conversion matrix\n\n # Control errors\n self.ex = np.zeros(3)\n self.ev = np.zeros(3)\n self.eR = np.zeros(3)\n self.eW = np.zeros(3)\n \n # Integral errors\n self.eIX = IntegralErrorVec3() # Position integral error\n self.eIR = IntegralErrorVec3() # Attitude integral error\n self.eI1 = IntegralError() # Attitude integral error for roll axis\n self.eI2 = IntegralError() # Attitude integral error for pitch axis\n self.eIy = IntegralError() # Attitude integral error for yaw axis\n\n self.sat_sigma = 1.8\n\n self.is_landed = True\n self.mode = 0\n self.motor_on = False\n\n self.init_subscribers()\n self.init_publishers()\n\n\n def control_run(self):\n \"\"\"Run the controller to get the force-moments required to achieve the \n the desired states from the current state.\n\n Args:\n state: (x, v, a, R, W) current states of the UAV\n desired: (xd, xd_dot, xd_2dot, xd_3dot, xd_4dot, b1d, b1d_dot,\n b1d_2dot, is_landed) desired states of the UAV\n\n Return:\n fM: (4x1 numpy array) force-moments vector\n \"\"\"\n\n # If the UAV is landed, do not run the controller and produce zero\n # force-moments.\n if self.is_landed:\n self.fM = np.zeros(4)\n else: \n self.position_control()\n self.attitude_control()\n\n if (not self.motor_on) or (self.mode < 2):\n force = Vector3(x=0.0, y=0.0, z=0.0)\n torque = Vector3(x=0.0, y=0.0, z=0.0) \n else:\n # FDCL uses NED, Gazebo uses NWU\n force = Vector3(x=0.0, y=0.0, z=self.fM[0])\n torque = Vector3(x=self.fM[1], y=-self.fM[2], z=-self.fM[3])\n\n \n fM_message = WrenchStamped()\n fM_message.wrench.force = force\n fM_message.wrench.torque = torque\n\n self.pub_fM.publish(fM_message)\n\n self.publish_desired()\n self.publish_errors()\n\n\n def position_control(self):\n \"\"\"Position controller to determine desired attitude and angular rates\n to achieve the deisred states.\n\n This uses the controller defined in \"Control of Complex Maneuvers\n for a Quadrotor UAV using Geometric Methods on SE(3)\"\n URL: https://arxiv.org/pdf/1003.2005.pdf\n \"\"\"\n\n m = self.m\n g = self.g\n e3 = self.e3\n\n kX = self.kX\n kV = self.kV\n kIX = self.kIX\n\n R = self.R\n R_T = self.R.T\n\n x = self.x\n v = self.v\n W = self.W\n\n b1d = self.b1d\n b1d_dot = self.b1d_dot\n b1d_2dot = self.b1d_2dot\n \n xd = self.xd\n xd_dot = self.xd_dot\n xd_2dot = self.xd_2dot\n xd_3dot = self.xd_3dot\n xd_4dot = self.xd_4dot\n\n self.update_current_time()\n self.dt = self.t - self.t_pre\n\n # Translational error functions\n eX = x - xd # position error - eq (11)\n eV = v - xd_dot # velocity error - eq (12)\n\n # Position integral terms\n if self.use_integral:\n self.eIX.integrate(self.c1 * eX + eV, self.dt) # eq (13)\n self.eIX.error = saturate(self.eIX.error, \\\n -self.sat_sigma, self.sat_sigma)\n else:\n self.eIX.set_zero()\n\n # Force 'f' along negative b3-axis - eq (14)\n # This term equals to R.e3\n A = - kX @ eX \\\n - kV @ eV \\\n - kIX * self.eIX.error \\\n - m * g * e3 \\\n + m * xd_2dot\n\n hatW = hat(W)\n\n b3 = R @ e3\n b3_dot = R @ hatW @ e3 # eq (22)\n f_total = -A @ b3\n\n # Intermediate terms for rotational errors\n ea = g * e3 \\\n - f_total / m * b3 \\\n - xd_2dot\n A_dot = - kX @ eV \\\n - kV @ ea \\\n + m * xd_3dot\n\n fdot = - A_dot @ b3 \\\n - A @ b3_dot\n eb = - fdot / m * b3 \\\n - f_total / m * b3_dot \\\n - xd_3dot\n A_2dot = - kX @ ea \\\n - kV @ eb \\\n + m * xd_4dot\n\n b3c, b3c_dot, b3c_2dot = deriv_unit_vector(-A, -A_dot, -A_2dot)\n\n hat_b1d = hat(b1d)\n hat_b1d_dot = hat(b1d_dot)\n\n A2 = -hat_b1d @ b3c\n A2_dot = - hat_b1d_dot @ b3c - hat_b1d @ b3c_dot\n A2_2dot = - hat(b1d_2dot) @ b3c \\\n - 2.0 * hat_b1d_dot @ b3c_dot \\\n - hat_b1d @ b3c_2dot\n\n b2c, b2c_dot, b2c_2dot = deriv_unit_vector(A2, A2_dot, A2_2dot)\n\n hat_b2c = hat(b2c)\n hat_b2c_dot = hat(b2c_dot)\n\n b1c = hat_b2c @ b3c\n b1c_dot = hat_b2c_dot @ b3c + hat_b2c @ b3c_dot\n b1c_2dot = hat(b2c_2dot) @ b3c \\\n + 2.0 * hat_b2c_dot @ b3c_dot \\\n + hat_b2c @ b3c_2dot\n\n Rd = np.vstack((b1c, b2c, b3c)).T\n Rd_dot = np.vstack((b1c_dot, b2c_dot, b3c_dot)).T\n Rd_2dot = np.vstack((b1c_2dot, b2c_2dot, b3c_2dot)).T\n\n Rd_T = Rd.T\n Wd = vee(Rd_T @ Rd_dot)\n\n hat_Wd = hat(Wd)\n Wd_dot = vee(Rd_T @ Rd_2dot - hat_Wd @ hat_Wd)\n\n self.f_total = f_total\n self.Rd = Rd\n self.Wd = Wd\n self.Wd_dot = Wd_dot\n\n # Roll / pitch\n self.b3d = b3c\n self.b3d_dot = b3c_dot\n self.b3d_2dot = b3c_2dot\n\n # Yaw\n self.b1c = b1c\n self.wc3 = e3 @ (R_T @ Rd @ Wd)\n self.wc3_dot = e3 @ (R_T @ Rd @ Wd_dot) \\\n - e3 @ (hatW @ R_T @ Rd @ Wd)\n \n self.ex = eX\n self.ev = eV\n\n\n def attitude_control(self):\n \"\"\"Deacouple-yaw attitude controller to achieve the desired attitude and\n angular rates.\n \n This uses the controller defined in \"Geometric Controls of a Quadrotor\n with a Decoupled Yaw Control\" \n URL: https://doi.org/10.23919/ACC.2019.8815189\n \"\"\"\n\n R = self.R\n R_T = self.R.T\n\n Rd = self.Rd\n Rd_T = self.Rd.T\n\n b3d = self.b3d\n b3d_dot = self.b3d_dot\n b3d_2dot = self.b3d_2dot\n\n W = self.W\n Wd = self.Wd\n\n J = self.J\n \n b1 = R @ self.e1\n b2 = R @ self.e2\n b3 = R @ self.e3\n\n hat_b3 = hat(b3)\n\n # Roll/pitch angular velocity vector\n W_12 = W[0] * b1 + W[1] * b2\n b3_dot = hat(W_12) @ b3 # eq (26)\n\n hat_b3d = hat(b3d)\n W_12d = hat_b3d @ b3d_dot\n W_12d_dot = hat_b3d @ b3d_2dot\n\n eb = hat_b3d @ b3 # eq (27)\n ew = W_12 + hat_b3 @ hat_b3 @ W_12d # eq (28)\n\n # Yaw\n ey = -b2 @ self.b1c\n ewy = W[2] - self.wc3\n\n # Attitude integral terms\n eI = ew + self.c2 * eb\n\n self.eI1.integrate(eI @ b1, self.dt) # b1 axis - eq (29)\n self.eI2.integrate(eI @ b2, self.dt) # b2 axis - eq (30)\n self.eIy.integrate(ewy + self.c3 * ey, self.dt)\n\n # Control moment for the roll/pitch dynamics - eq (31)\n tau = -self.kR[0, 0] * eb \\\n - self.kW[0, 0] * ew \\\n - J[0, 0] * b3.T @ W_12d * b3_dot \\\n - J[0, 0] * hat_b3 @ hat_b3 @ W_12d_dot\n if self.use_integral:\n tau += -self.kI * self.eI1.error * b1 \\\n - self.kI * self.eI2.error * b2\n\n # Control moment around b1 axis - roll - eq (24)\n M1 = b1.T @ tau + J[2, 2] * W[2] * W[1]\n\n # Control moment around b2 axis - pitch - eq (24)\n M2 = b2.T @ tau - J[2, 2] * W[2] * W[0]\n\n # Control moment around b3 axis - yaw - eq (52)\n M3 = - self.ky * ey \\\n - self.kwy * ewy \\\n + self.J[2, 2] * self.wc3_dot\n \n if self.use_integral:\n M3 += - self.kyI * self.eIy.error\n\n M = np.array([M1, M2, M3])\n\n self.fM[0] = self.f_total\n for i in range(3):\n self.fM[i + 1] = M[i]\n \n f_motor = self.fM_to_forces_inv @ self.fM\n\n # For saving:\n RdtR = Rd_T @ R\n self.eR = 0.5 * vee(RdtR - RdtR.T)\n self.eIR.error = np.array([self.eI1.error, self.eI2.error, \\\n self.eIy.error])\n self.eW = W - R_T @ Rd @ Wd\n\n # self.get_logger().info('eR ' + str(self.eR))\n\n\n def set_integral_errors_to_zero(self):\n \"\"\"Set all integrals to zero.\"\"\"\n self.eIX.set_zero()\n self.eIR.set_zero()\n self.eI1.set_zero()\n self.eI2.set_zero()\n self.eIy.set_zero()\n self.eIX.set_zero()\n\n\n def update_current_time(self):\n \"\"\"Update the current time since epoch.\"\"\"\n self.t_pre = self.t\n\n t_now = self.get_clock().now()\n self.t = float((t_now - self.t0).nanoseconds) * 1e-9\n \n\n\n def init_publishers(self):\n self.pub_desired = self.create_publisher(DesiredData, '/uav/desired', 1)\n self.pub_errors = self.create_publisher(ErrorData, '/uav/errors', 1)\n self.pub_fM = self.create_publisher(WrenchStamped, '/uav/fm', 1)\n\n timer_period = 0.005\n self.timer = self.create_timer(timer_period, self.control_run)\n \n\n def init_subscribers(self):\n self.sub_trajectory = self.create_subscription( \\\n StateData,\n '/uav/states', \n self.states_callback, \n 1)\n \n self.sub_trajectory = self.create_subscription( \\\n DesiredData,\n '/uav/trajectory', \n self.trajectory_callback, \n 1)\n \n self.sub_mode = self.create_subscription( \\\n ModeData,\n '/uav/mode',\n self.mode_callback,\n 1)\n \n\n self.sub_motors_on = self.create_subscription( \\\n Bool,\n '/uav/motors_on',\n self.motors_on_callback,\n 1)\n \n\n def mode_callback(self, msg):\n if self.mode == msg.mode:\n return\n \n self.mode = msg.mode\n self.get_logger().info('Mode switched to {}'.format(self.mode))\n\n \n def states_callback(self, msg):\n\n for i in range(3):\n self.x[i] = msg.position[i]\n self.v[i] = msg.velocity[i]\n self.a[i] = msg.acceleration[i]\n self.W[i] = msg.angular_velocity[i]\n\n for j in range(3):\n self.R[i, j] = msg.attitude[3*i + j]\n\n\n def trajectory_callback(self, msg):\n \"\"\"Callback function for the trajectory subscriber.\n\n Args:\n msg: (DesiredData) Trajectory data message\n \"\"\"\n\n for i in range(3):\n self.xd[i] = msg.position[i]\n self.xd_dot[i] = msg.velocity[i]\n self.xd_2dot[i] = msg.acceleration[i]\n self.xd_3dot[i] = msg.jerk[i]\n self.xd_4dot[i] = msg.snap[i]\n\n self.b1d[i] = msg.b1[i]\n self.b1d_dot[i] = msg.b1_dot[i]\n self.b1d_2dot[i] = msg.b1_2dot[i]\n\n self.is_landed = msg.is_landed\n\n \n def motors_on_callback(self, msg):\n self.motor_on = msg.data\n\n if self.motor_on:\n self.get_logger().info('Turning motors on')\n else:\n self.get_logger().info('Turning motors off')\n\n \n def publish_desired(self):\n \"\"\"Publish the desired states.\"\"\"\n\n msg = DesiredData()\n\n for i in range(3):\n msg.position[i] = self.xd[i]\n msg.velocity[i] = self.xd_dot[i]\n msg.acceleration[i] = self.xd_2dot[i]\n msg.jerk[i] = self.xd_3dot[i]\n msg.snap[i] = self.xd_4dot[i]\n\n msg.b1[i] = self.b1d[i]\n msg.b1_dot[i] = self.b1d_dot[i]\n msg.b1_2dot[i] = self.b1d_2dot[i]\n\n msg.angular_velocity[i] = self.Wd[i]\n msg.angular_acceleration[i] = self.Wd_dot[i]\n # msg.angular_jerk = self.Wd_2dot\n\n msg.b1c[i] = self.b1c[i]\n\n msg.b3[i] = self.b3d[i]\n msg.b3_dot[i] = self.b3d_dot[i]\n msg.b3_2dot[i] = self.b3d_2dot[i]\n\n for j in range(3):\n msg.attitude[3*i + j] = self.Rd[i, j]\n\n msg.wc3 = self.wc3\n msg.wc3_dot = self.wc3_dot\n \n msg.is_landed = self.is_landed\n\n self.pub_desired.publish(msg)\n\n\n def publish_errors(self):\n\n msg = ErrorData()\n\n for i in range(3):\n msg.position[i] = self.ex[i]\n msg.velocity[i] = self.ev[i]\n\n msg.attitude[i] = self.eR[i]\n msg.angular_velocity[i] = self.eW[i]\n\n msg.position_integral[i] = self.eIX.error[i]\n msg.attitude_integral[i] = self.eIR.error[i]\n \n\n self.pub_errors.publish(msg)\n\n\ndef main(args=None):\n print(\"Starting control node\")\n\n rclpy.init(args=args)\n\n control = ControlNode()\n\n try:\n rclpy.spin(control)\n except KeyboardInterrupt:\n pass\n\n # Destroy the node explicitly\n # (optional - otherwise it will be done automatically\n # when the garbage collector destroys the node object)\n control.destroy_node()\n \n print(\"Terminating control node\")\n" }, { "path": "src/fdcl_uav/fdcl_uav/estimator.py", "content": "from .matrix_utils import hat, vee, expm_SO3, q_to_R\n\nimport rclpy\nfrom rclpy.node import Node\n\nimport datetime\nimport numpy as np\n\nimport rclpy\nfrom rclpy.node import Node\n\nfrom nav_msgs.msg import Odometry\nfrom sensor_msgs.msg import Imu\n\nfrom uav_gazebo.msg import StateData\n\n\nclass EstimatorNode(Node):\n \"\"\"Estimates the states of the UAV.\n\n This uses the estimator defined in \"Real-time Kinematics GPS Based \n Telemetry System for Airborne Measurements of Ship Air Wake\", but without\n the bias estimation terms.\n DOI: 10.2514/6.2019-2377\n\n x (3x1 numpy array) current position of the UAV [m]\n x: (3x1 numpy array) current position of the UAV [m]\n v: (3x1 numpy array) current velocity of the UAV [m/s]\n a: (3x1 numpy array) current acceleration of the UAV [m/s^s]\n b_a: (float) accelerometer bias in e3 direction [m/s^2]\n R: (3x3 numpy array) current attitude of the UAV in SO(3)\n W: (3x1 numpy array) current angular velocity of the UAV [rad/s]\n \n Q: (7x7 numpy array) variances of w_k\n P: (10x10 numpy array) covariances of the states\n\n t0: (datetime object) time at epoch\n t: (float) current time since epoch [s]\n t_prev: (float) time since epoch in the previous loop [s]\n\n W_pre: (3x1 numpy array) angular velocity of the previous loop [rad/s]\n a_imu_pre: (3x1 numpy array) acceleration of the previous loop [m/s^2]\n R_pre: (3x3 numpy array) attitude in the previous loop in SO(3)\n b_a_pre: (3x1 numpy array) accelerometer bias in the previous loop [m/s^2]\n\n g: (float) gravitational acceleration [m/s^2]\n ge3: (3x1 numpy array) gravitational acceleration direction [m/s^2]\n\n R_bi: (3x3 numpy array) transformation from IMU frame to the body frame\n R_ib: (3x3 numpy array) transformation from IMU frame to the body frame\n\n e3 : (3x1 numpy array) direction of the e3 axis\n eye3: (3x3 numpy array) 3x3 identity matrix\n eye10: (10x10 numpy array) 10x10 identity matrix\n\n zero3: (3x3 numpy array) 3x3 zero matrix\n \"\"\"\n\n def __init__(self):\n Node.__init__(self, 'estimator')\n\n self.x = np.zeros(3)\n self.v = np.zeros(3)\n self.a = np.zeros(3)\n self.b_a = 0.0\n self.R = np.eye(3)\n self.W = np.zeros(3)\n \n # Variances of w_k\n self.Q = np.diag([\n 0.001, 0.001, 0.001, # acceleration\n 0.025, 0.025, 0.025, # angular velocity\n 0.0001 # acclerometer z bias\n ])\n\n # Initial covariances of x\n self.P = np.diag([\n 1.0, 1.0, 1.0, # position\n 1.0, 1.0, 1.0, # velocity\n 0.01, 0.01, 0.01, # attitude\n 1.0 # accelerometer z bias\n ])\n\n self.t0 = self.get_clock().now()\n self.t = 0.0\n self.t_pre = 0.0\n\n self.W_pre = np.zeros(3)\n self.a_imu_pre = np.zeros(3)\n self.W_imu_pre = np.zeros(3)\n self.R_pre = np.eye(3)\n self.b_a_pre = 0.0\n\n self.g = 9.81\n self.ge3 = np.array([0.0, 0.0, self.g])\n\n # Transformation from IMU frame to the body frame.\n self.R_bi = np.array([\n [1.0, 0.0, 0.0],\n [0.0, -1.0, 0.0],\n [0.0, 0.0, -1.0]\n ])\n self.R_ib = self.R_bi.T\n\n self.e3 = np.array([0.0, 0.0, 1.0])\n self.eye3 = np.eye(3)\n self.eye10 = np.eye(10)\n\n self.zero3 = np.zeros((3, 3))\n\n self.V_R_imu = np.diag([0.01, 0.01, 0.01])\n self.V_x_gps = np.diag([0.01, 0.01, 0.01])\n self.V_v_gps = np.diag([0.01, 0.01, 0.01])\n\n # Gazebo uses ENU frame, but NED frame is used in FDCL.\n # Note that ENU and the NED here refer to their direction order.\n # ENU: E - axis 1, N - axis 2, U - axis 3\n # NED: N - axis 1, E - axis 2, D - axis 3\n self.R_fg = np.array([\n [1.0, 0.0, 0.0],\n [0.0, -1.0, 0.0],\n [0.0, 0.0, -1.0]\n ])\n\n self.init_subscribers()\n self.init_publishers()\n\n\n def prediction(self, a_imu, W_imu):\n \"\"\"Prediction step of the estimator.\n\n Args:\n a_imu: (3x1 numpy array) acceleration measured by the IMU [m/s^2]\n W_imu: (3x1 numpy array) angular rate measured by the IMU [rad/s]\n \"\"\"\n\n h = self.get_dt()\n\n self.R_pre = np.copy(self.R)\n self.W_pre = np.copy(self.W)\n self.b_a_pre = self.b_a * 1.0\n\n self.W = self.R_bi @ W_imu\n self.R = self.R @ expm_SO3(h / 2.0 * (self.W + self.W_pre))\n\n # This assumes IMU provide acceleration without g\n self.a = self.R @ self.R_bi @ a_imu + self.b_a * self.e3\n a_pre = self.R_pre @ self.R_bi @ self.a_imu_pre + self.b_a_pre * self.e3\n\n self.x = self.x + h * self.v + h**2 / 2.0 * a_pre\n self.v = self.v + h / 2.0 * (self.a + a_pre)\n\n # Calculate A(t_{k-1})\n A = np.zeros((10, 10))\n A[0:3, 3:6] = self.eye3\n A[3:6, 6:9] = - self.R_pre @ hat(self.R_bi @ self.a_imu_pre)\n A[3:6, 9] = self.e3\n A[6:9, 6:9] = -hat(self.R_bi @ W_imu)\n\n # Calculate F(t_{k-1})\n F = np.zeros((10, 7))\n F[3:6, 0:3] = self.R_pre @ self.R_bi\n F[6:9, 3:6] = self.R_bi\n F[9, 6] = 1.0\n\n # Calculate \\Psi using A(t)\n psi = self.eye10 + h / 2.0 * A\n\n A = self.eye10 + h * A @ psi\n F = h * psi @ F\n\n self.P = A @ self.P @ A.T + F @ self.Q @ F.T\n\n self.a_imu_pre = a_imu\n self.W_imu_pre = W_imu\n\n # rover.x = self.x\n # rover.v = self.v\n # rover.a = self.a\n # rover.R = self.R\n # rover.W = self.W\n\n\n def imu_correction(self, R_imu, V_R_imu):\n \"\"\"IMU correction step of the estimator.\n\n Args:\n R_imu: (3x3 numpy array) attitude measured by the IMU in SO(3)\n V_R_imu: (3x3 numpy array) attitude measurement covariance\n \"\"\"\n\n imu_R = self.R.T @ R_imu @ self.R_ib\n del_z = 0.5 * vee(imu_R - imu_R.T)\n\n H = np.block([self.zero3, self.zero3, self.eye3, np.zeros((3, 1))])\n H_T = H.T\n\n G = self.R_bi\n G_T = G.T\n\n V = V_R_imu\n\n S = H @ self.P @ H_T + G @ V @ G_T\n K = self.P @ H_T @ np.linalg.inv(S)\n\n X = K @ del_z\n\n eta = X[6:9]\n self.R = self.R @ expm_SO3(eta)\n\n I_KH = self.eye10 - K @ H\n self.P = I_KH @ self.P @ I_KH.T + K @ G @ V @ G_T @ K.T\n\n\n def gps_correction(self, x_gps, v_gps, V_x_gps, V_v_gps):\n \"\"\"GPS correction step of the estimator.\n\n Args:\n x_gps: (3x1 numpy array) position measured by the GPS [m]\n v_gps: (3x1 numpy array) velocity measured by the GPS [m]\n V_x_gps: (3x1 numpy array) position measurement covariance\n V_v_gps: (3x1 numpy array) velocity measurement covariance\n \"\"\"\n\n del_z = np.hstack((x_gps - self.x, v_gps - self.v))\n\n H = np.block([\n [self.eye3, self.zero3, self.zero3, np.zeros((3, 1))],\n [self.zero3, self.eye3, self.zero3, np.zeros((3, 1))]\n ])\n H_T = H.T\n\n V = np.block([\n [V_x_gps, self.zero3],\n [self.zero3, V_v_gps]\n ])\n\n S = H @ self.P @ H_T + V\n K = self.P @ H_T @ np.linalg.inv(S)\n\n X = K @ del_z\n\n dx = X[0:3]\n dv = X[3:6]\n db_a = X[9]\n\n self.x = self.x + dx\n self.v = self.v + dv\n self.b_a = self.b_a + db_a\n\n I_KH = self.eye10 - K @ H\n self.P = I_KH @ self.P @ I_KH.T + K @ V @ K.T\n\n \n def get_dt(self):\n \"\"\"Get the time difference between two loops.\n\n Return:\n dt: (float) time difference between two loops\n \"\"\"\n\n self.t_pre = self.t * 1.0\n t_now = self.get_clock().now()\n self.t = float((t_now - self.t0).nanoseconds) * 1e-9\n\n return self.t - self.t_pre\n\n\n def get_states(self):\n \"\"\"Return the current states of the estimator.\n\n Return:\n x: (3x1 numpy array) current position of the UAV [m]\n v: (3x1 numpy array) current velocity of the UAV [m/s]\n a: (3x1 numpy array) current acceleration of the UAV [m/s^s]\n R: (3x3 numpy array) current attitude of the UAV in SO(3)\n W: (3x1 numpy array) current angular velocity of the UAV [rad/s]\n \"\"\"\n return (self.x, self.v, self.a, self.R, self.W)\n \n\n def init_subscribers(self):\n self.sub_imu = self.create_subscription( \\\n Imu,\n '/uav/imu',\n self.imu_callback,\n 1)\n \n self.sub_gps = self.create_subscription( \\\n Odometry,\n '/uav/gps',\n self.gps_callback,\n 1)\n \n \n def imu_callback(self, msg):\n \"\"\"Callback function for the IMU subscriber.\n\n Args:\n msg: (Imu) IMU message\n \"\"\"\n\n q_gazebo = msg.orientation\n a_gazebo = msg.linear_acceleration\n W_gazebo = msg.angular_velocity\n\n q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w])\n\n R_gi = q_to_R(q) # IMU to Gazebo frame\n R_fi = self.R_fg @ R_gi # IMU to FDCL frame (NED frame)\n\n # FDCL-UAV expects IMU accelerations without gravity.\n a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z])\n a_imu = R_gi.T @ (R_gi @ a_i - self.ge3)\n\n W_imu = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z])\n\n # TODO: get covariances from the message\n self.prediction(a_imu, W_imu)\n self.imu_correction(R_fi, self.V_R_imu)\n\n self.publish_states()\n\n\n\n def gps_callback(self, msg):\n \"\"\"Callback function for the GPS subscriber.\n\n Args:\n msg: (GPS) GPS message\n \"\"\"\n\n x_gazebo = msg.pose.pose.position\n v_gazebo = msg.twist.twist.linear\n\n # Gazebo uses ENU frame, but NED frame is used in FDCL.\n x_gps = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z])\n v_gps = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z])\n\n self.gps_correction(x_gps, v_gps, self.V_x_gps, self.V_v_gps)\n\n self.publish_states()\n \n\n\n def init_publishers(self):\n self.pub_state = self.create_publisher(StateData, '/uav/states', 1)\n\n \n def publish_states(self):\n states = StateData()\n for i in range(3):\n states.position[i] = self.x[i]\n states.velocity[i] = self.v[i]\n states.acceleration[i] = self.a[i]\n states.angular_velocity[i] = self.W[i]\n\n for j in range(3):\n states.attitude[3*i + j] = self.R[i, j]\n\n self.pub_state.publish(states)\n\n\n\ndef main(args=None):\n print(\"Starting estimator node\")\n\n rclpy.init(args=args)\n\n estimator = EstimatorNode()\n\n try:\n rclpy.spin(estimator)\n except KeyboardInterrupt:\n pass\n\n # Destroy the node explicitly\n # (optional - otherwise it will be done automatically\n # when the garbage collector destroys the node object)\n estimator.destroy_node()\n\n print(\"Terminating estimator node\")" }, { "path": "src/fdcl_uav/fdcl_uav/freq_utils.py", "content": "\n\nclass Freq():\n def __init__(self, name, freq):\n self.name = name\n self.t_prev = 0.0\n self.num_avg = 50\n self.freq_avg = freq\n self.dt = 0\n\n def update(self, t_now):\n dt = t_now - self.t_prev\n\n if dt <= 0.0:\n return\n \n self.dt = dt\n freq = 1.0 / dt\n self.freq_avg = (freq + (self.num_avg - 1) * self.freq_avg) / self.num_avg\n\n self.t_prev = t_now\n\n\n def get_freq(self):\n return self.freq_avg\n \n\n def get_freq_str(self):\n return f'{self.name}: {self.freq_avg:5.1f} Hz'" }, { "path": "src/fdcl_uav/fdcl_uav/gui.py", "content": "from .matrix_utils import q_to_R, R_to_RPY\nfrom .freq_utils import Freq\n\nimport numpy as np\nimport os\n\nfrom PyQt5.QtCore import Qt, QTimer\nfrom PyQt5.QtWidgets import QApplication, QLabel, QWidget, QVBoxLayout, \\\n QHBoxLayout, QPushButton, QRadioButton, QMainWindow\n\nimport rclpy\nfrom rclpy.node import Node\n\nfrom geometry_msgs.msg import WrenchStamped\nfrom nav_msgs.msg import Odometry\nfrom sensor_msgs.msg import Imu\nfrom std_msgs.msg import Bool\n\nfrom uav_gazebo.msg import DesiredData, ErrorData, ModeData, StateData\n\n\nclass GuiNode(Node, QMainWindow):\n def __init__(self):\n Node.__init__(self, 'gui')\n QMainWindow.__init__(self)\n\n self.t0 = self.get_clock().now()\n\n self.motor_on = False\n\n # States\n self.freq_estimator = Freq(\"EST\", 100)\n self.x = np.zeros(3)\n self.v = np.zeros(3)\n self.a = np.zeros(3)\n self.R = np.identity(3)\n self.W = np.zeros(3)\n\n # Desired states\n self.xd = np.zeros(3)\n self.vd = np.zeros(3)\n self.b1d = np.zeros(3)\n self.b1d[0] = 1.0\n self.Rd = np.identity(3)\n self.Wd = np.zeros(3)\n\n # Errors\n self.ex = np.zeros(3)\n self.ev = np.zeros(3)\n self.eR = np.identity(3)\n self.eW = np.zeros(3)\n self.eIX = np.zeros(3)\n self.eIR = np.zeros(3)\n\n # Sensor measurements\n self.freq_imu = Freq(\"IMU\", 100)\n self.R_imu = np.identity(3)\n self.a_imu = np.zeros([3, 1])\n self.W_imu = np.zeros([3, 1])\n\n self.freq_gps = Freq(\"GPS\", 10)\n self.x_gps = np.zeros([3, 1])\n self.v_gps = np.zeros([3, 1])\n\n # Motor forces\n self.freq_control = Freq(\"CTRL\", 100)\n self.fM = np.zeros(4)\n\n # Manual mode\n self.x_offset = np.zeros(3)\n self.x_offset_delta = 0.1\n self.yaw_offset = 0.0\n self.yaw_offset_delta = 0.02\n\n self.mode = 0\n\n self.g = 9.81\n self.ge3 = np.array([0.0, 0.0, self.g])\n\n self.RAD2DEG = 180.0 / np.pi\n\n self.init_gui()\n self.init_subscribers()\n self.init_publishers()\n\n self.timer = QTimer()\n self.timer.timeout.connect(self.update)\n self.timer.start(100)\n\n\n def init_gui(self):\n window = QWidget()\n window.setWindowTitle(\"FDCL UAV Simulator\")\n\n main_layout = QVBoxLayout()\n main_layout.setSpacing(10)\n\n # Status box\n self.label_time = QLabel(\"0.0\")\n self.label_time.setMinimumWidth(80)\n self.label_freq_imu = QLabel(\"0.0\")\n self.label_freq_gps = QLabel(\"0.0\")\n self.label_freq_control = QLabel(\"0.0\")\n self.label_freq_estimator = QLabel(\"0.0\")\n\n thread_status_box = QHBoxLayout()\n thread_status_box.setAlignment(Qt.AlignLeft)\n thread_status_box.addWidget(self.label_time)\n thread_status_box.addWidget(self.label_freq_imu)\n thread_status_box.addWidget(self.label_freq_gps)\n thread_status_box.addWidget(self.label_freq_control)\n thread_status_box.addWidget(self.label_freq_estimator)\n\n # Control box\n self.button_on = QPushButton(\"Shutdown\")\n self.button_on.clicked.connect(self.on_btn_close_clicked)\n\n self.button_motor_on = QPushButton(\"Turn on motors\")\n self.button_motor_on.clicked.connect(self.on_btn_motor_on_clicked)\n\n controls_box = QHBoxLayout()\n controls_box.setAlignment(Qt.AlignLeft)\n controls_box.addWidget(self.button_on)\n controls_box.addWidget(self.button_motor_on)\n\n # Flight mode box\n label_flight_mode = QLabel(\"Mode: \")\n self.radio_button_modes = \\\n [ \\\n QRadioButton(\"Idle\"), \\\n QRadioButton(\"Warmup\"), \\\n QRadioButton(\"Take-off\"), \\\n QRadioButton(\"Stay\"), \\\n QRadioButton(\"Circle\"), \\\n QRadioButton(\"Land\") \\\n ]\n num_flight_modes = len(self.radio_button_modes)\n \n flight_mode_box = QHBoxLayout()\n flight_mode_box.setAlignment(Qt.AlignLeft)\n flight_mode_box.addWidget(label_flight_mode)\n for i in range(num_flight_modes):\n flight_mode_box.addWidget(self.radio_button_modes[i])\n self.radio_button_modes[i].toggled.connect(self.on_flight_mode_changed)\n\n # State box\n state_box = QHBoxLayout()\n state_box.setAlignment(Qt.AlignLeft)\n state_box.addSpacing(5)\n [state_box, self.label_x] = self.init_vbox(state_box, \"x\", 4)\n [state_box, self.label_v] = self.init_vbox(state_box, \"v\", 4)\n [state_box, self.label_a] = self.init_vbox(state_box, \"a\", 4)\n [state_box, self.label_R] = self.init_attitude_vbox(state_box, \"R\")\n [state_box, self.label_W] = self.init_vbox(state_box, \"W\", 4)\n\n # Desired box\n desired_box = QHBoxLayout()\n desired_box.setAlignment(Qt.AlignLeft)\n desired_box.addSpacing(5)\n [desired_box, self.label_xd] = self.init_vbox(desired_box, \"xd\", 4)\n [desired_box, self.label_vd] = self.init_vbox(desired_box, \"vd\", 4)\n [desired_box, self.label_b1d] = self.init_vbox(desired_box, \"b1d\", 4)\n [desired_box, self.label_Rd] = self.init_attitude_vbox(desired_box, \"Rd\")\n [desired_box, self.label_Wd] = self.init_vbox(desired_box, \"Wd\", 4)\n\n # Sensor box\n sensor_box = QHBoxLayout()\n sensor_box.setAlignment(Qt.AlignLeft)\n sensor_box.addSpacing(5)\n [sensor_box, self.label_imu_ypr] = self.init_vbox(sensor_box, \"IMU RPY\", 4)\n [sensor_box, self.label_imu_a] = self.init_vbox(sensor_box, \"IMU a\", 4)\n [sensor_box, self.label_imu_W] = self.init_vbox(sensor_box, \"IMU W\", 4)\n [sensor_box, self.label_gps_x] = self.init_vbox(sensor_box, \"GPS x\", 4)\n [sensor_box, self.label_gps_v] = self.init_vbox(sensor_box, \"GPS v\", 4)\n\n # Error box\n error_box = QHBoxLayout()\n error_box.setAlignment(Qt.AlignLeft)\n error_box.addSpacing(5)\n [error_box, self.label_ex] = self.init_vbox(error_box, \"ex\", 4)\n [error_box, self.label_ev] = self.init_vbox(error_box, \"ev\", 4)\n [error_box, self.label_fM] = self.init_vbox(error_box, \"f-M\", 5)\n # [error_box, self.label_f] = self.init_vbox(error_box, \"f\", 5)\n # [error_box, self.label_thr] = self.init_vbox(error_box, \"Throttle\", 5)\n\n # Left box\n left_box = QVBoxLayout()\n left_box.setAlignment(Qt.AlignTop)\n left_box.addSpacing(5)\n left_box.addLayout(state_box)\n main_layout.addSpacing(10)\n left_box.addLayout(desired_box)\n\n # Right box\n right_box = QVBoxLayout()\n right_box.setAlignment(Qt.AlignTop)\n right_box.addSpacing(5)\n right_box.addLayout(sensor_box)\n main_layout.addSpacing(10)\n right_box.addLayout(error_box)\n\n # Data box\n data_box = QHBoxLayout()\n data_box.setAlignment(Qt.AlignLeft)\n data_box.addLayout(left_box)\n data_box.addSpacing(5)\n data_box.addLayout(right_box)\n\n main_layout.setAlignment(Qt.AlignTop)\n main_layout.addLayout(thread_status_box)\n main_layout.addSpacing(5)\n main_layout.addLayout(controls_box)\n main_layout.addSpacing(5)\n main_layout.addLayout(flight_mode_box)\n main_layout.addSpacing(5)\n main_layout.addLayout(data_box)\n\n window.setLayout(main_layout)\n self.setCentralWidget(window)\n window.keyPressEvent = self.on_key_press\n\n def init_subscribers(self):\n\n self.sub_states = self.create_subscription( \\\n StateData,\n '/uav/states',\n self.states_callback,\n 1)\n \n self.sub_desired = self.create_subscription( \\\n DesiredData,\n '/uav/desired',\n self.desired_callback,\n 1)\n \n self.sub_errors = self.create_subscription( \\\n ErrorData,\n '/uav/errors',\n self.errors_callback,\n 1)\n\n self.sub_fM = self.create_subscription( \\\n WrenchStamped,\n '/uav/fm',\n self.fM_callback,\n 1)\n \n self.sub_imu = self.create_subscription( \\\n Imu,\n '/uav/imu',\n self.imu_callback,\n 1)\n \n self.sub_gps = self.create_subscription( \\\n Odometry,\n '/uav/gps',\n self.gps_callback,\n 1)\n\n\n def init_publishers(self):\n self.pub_mode = self.create_publisher(ModeData, '/uav/mode', 1) \n self.pub_motors_on = self.create_publisher(Bool, '/uav/motors_on', 1) \n \n \n def update(self):\n t_sec = self.get_t_now()\n\n self.label_time.setText(f't: {t_sec:5.1f} s')\n self.label_freq_imu.setText(self.freq_imu.get_freq_str())\n self.label_freq_gps.setText(self.freq_gps.get_freq_str())\n self.label_freq_control.setText(self.freq_control.get_freq_str())\n self.label_freq_estimator.setText(self.freq_estimator.get_freq_str())\n\n self.update_vbox(self.label_x, self.x)\n self.update_vbox(self.label_v, self.v)\n self.update_vbox(self.label_a, self.a)\n self.update_attitude_vbox(self.label_R, self.R)\n self.update_vbox(self.label_W, self.W)\n\n self.update_vbox(self.label_xd, self.xd)\n self.update_vbox(self.label_vd, self.vd)\n self.update_vbox(self.label_b1d, self.b1d)\n self.update_attitude_vbox(self.label_Rd, self.Rd)\n self.update_vbox(self.label_Wd, self.Wd)\n\n imu_ypr = R_to_RPY(self.R_imu) * self.RAD2DEG\n self.update_vbox(self.label_imu_ypr, imu_ypr)\n\n self.update_vbox(self.label_imu_a, self.a_imu)\n self.update_vbox(self.label_imu_W, self.W_imu)\n self.update_vbox(self.label_gps_x, self.x_gps)\n self.update_vbox(self.label_gps_v, self.v_gps)\n\n self.update_vbox(self.label_ex, self.ex)\n self.update_vbox(self.label_ev, self.ev)\n\n self.update_vbox(self.label_fM, self.fM, data_size=4)\n # self.update_vbox(self.label_f, self.f)\n # self.update_vbox(self.label_thr, self.thr)\n\n msg = ModeData()\n msg.mode = self.mode\n msg.x_offset = self.x_offset\n msg.yaw_offset = self.yaw_offset\n self.pub_mode.publish(msg)\n\n\n def init_vbox(self, box, name, num):\n vbox = QVBoxLayout()\n vbox.setAlignment(Qt.AlignTop)\n\n labels = []\n for _ in range(num):\n label_i = QLabel(\"{:5.2}\".format(0.0))\n\n labels.append(label_i)\n vbox.addWidget(label_i)\n\n labels[0].setText(name)\n box.addLayout(vbox)\n\n return [box, labels]\n\n \n def init_attitude_vbox(self, box, name):\n vbox_array = [QVBoxLayout(), QVBoxLayout(), QVBoxLayout()]\n labels = []\n\n for vbox_index in range(len(vbox_array)):\n vbox = vbox_array[vbox_index]\n vbox.setAlignment(Qt.AlignTop)\n\n for _ in range(4):\n label_i = QLabel(\"{:6.4}\".format(0.0))\n \n labels.append(label_i)\n vbox.addWidget(label_i)\n\n box.addLayout(vbox)\n\n labels[0].setText(name)\n labels[4].setText(\"\")\n labels[8].setText(\"\")\n\n return [box, labels]\n \n\n def update_vbox(self, labels, data, data_size=3):\n for i in range(data_size):\n try:\n labels[i + 1].setText(\"{:>5.2f}\".format(data[i]))\n except TypeError:\n labels[i + 1].setText(\"{:>5.2f}\".format(-1))\n\n \n def update_attitude_vbox(self, labels, data):\n for i in range(3):\n for j in range(1, 4):\n labels[i * 4 + j].setText(\"{:>5.2f}\".format(data[i, j - 1]))\n\n\n def on_btn_close_clicked(self):\n self.get_logger().info(\"Shutdown button clicked\")\n\n self.get_logger().info('Turning motors off')\n self.motor_on = False\n\n msg = Bool()\n msg.data = self.motor_on\n self.pub_motors_on.publish(msg)\n \n self.destroy_node()\n self.close()\n rclpy.shutdown()\n\n \n def on_btn_motor_on_clicked(self):\n\n if self.motor_on:\n self.get_logger().info('Turning motors off')\n self.motor_on = False\n self.button_motor_on.setText(\"Turn on motors\")\n else:\n self.get_logger().info('Turning motors on')\n self.motor_on = True\n self.button_motor_on.setText(\"Turn off motors\")\n \n msg = Bool()\n msg.data = self.motor_on\n self.pub_motors_on.publish(msg)\n\n\n def on_flight_mode_changed(self):\n for i in range(len(self.radio_button_modes)):\n if self.radio_button_modes[i].isChecked():\n self.get_logger().info('Mode switched to {}'.format(i))\n\n self.mode = i\n \n msg = ModeData()\n msg.mode = self.mode\n self.pub_mode.publish(msg)\n\n self.x_offset = np.zeros(3)\n self.yaw_offset = 0.0\n break\n\n\n def on_key_press(self, event):\n self.get_logger().info('Key presed {}'.format(event.key()))\n if event.key() == Qt.Key_M:\n self.on_btn_motor_on_clicked()\n elif event.key() == Qt.Key_QuoteLeft:\n self.radio_button_modes[0].setChecked(True)\n elif event.key() == Qt.Key_1:\n self.radio_button_modes[1].setChecked(True)\n elif event.key() == Qt.Key_2:\n self.radio_button_modes[2].setChecked(True)\n elif event.key() == Qt.Key_3:\n self.radio_button_modes[3].setChecked(True)\n elif event.key() == Qt.Key_4:\n self.radio_button_modes[4].setChecked(True)\n elif event.key() == Qt.Key_5:\n self.radio_button_modes[5].setChecked(True)\n elif event.key() == Qt.Key_6:\n self.radio_button_modes[6].setChecked(True)\n elif event.key() == Qt.Key_W:\n self.x_offset[0] += self.x_offset_delta\n elif event.key() == Qt.Key_S:\n self.x_offset[0] -= self.x_offset_delta\n elif event.key() == Qt.Key_D:\n self.x_offset[1] += self.x_offset_delta\n elif event.key() == Qt.Key_A:\n self.x_offset[1] -= self.x_offset_delta\n elif event.key() == Qt.Key_L:\n self.x_offset[2] += self.x_offset_delta\n elif event.key() == Qt.Key_P:\n self.x_offset[2] -= self.x_offset_delta\n elif event.key() == Qt.Key_E:\n self.yaw_offset += self.yaw_offset_delta\n elif event.key() == Qt.Key_Q:\n self.yaw_offset -= self.yaw_offset_delta\n\n\n def get_t_now(self):\n t_now = self.get_clock().now()\n t_sec = float((t_now - self.t0).nanoseconds) * 1e-9\n return t_sec\n\n\n def states_callback(self, msg):\n \"\"\"Callback function for the states subscriber.\n\n Args:\n msg: (StateData) State data message\n \"\"\"\n\n self.freq_estimator.update(self.get_t_now())\n \n for i in range(3):\n self.x[i] = msg.position[i]\n self.v[i] = msg.velocity[i]\n self.a[i] = msg.acceleration[i]\n self.W[i] = msg.angular_velocity[i]\n\n for j in range(3):\n self.R[i, j] = msg.attitude[3*i + j]\n\n\n def desired_callback(self, msg):\n \"\"\"Callback function for the desired subscriber.\n\n Args:\n msg: (DesiredData) Desired data message\n \"\"\"\n\n for i in range(3):\n self.xd[i] = msg.position[i]\n self.vd[i] = msg.velocity[i]\n self.b1d[i] = msg.b1[i]\n self.Wd[i] = msg.angular_velocity[i]\n\n for j in range(3):\n self.Rd[i, j] = msg.attitude[3*i + j]\n\n\n def errors_callback(self, msg):\n \"\"\"Callback function for the error subscriber.\n\n Args:\n msg: (ErrorData) Error data message\n \"\"\"\n\n for i in range(3):\n self.ex[i] = msg.position[i]\n self.ev[i] = msg.velocity[i]\n self.eR[i] = msg.attitude[i]\n self.eW[i] = msg.angular_velocity[i]\n self.eIX[i] = msg.position_integral[i]\n self.eIR[i] = msg.attitude_integral[i]\n\n \n def imu_callback(self, msg):\n \"\"\"Callback function for the IMU subscriber.\n\n Args:\n msg: (Imu) IMU message\n \"\"\"\n\n self.freq_imu.update(self.get_t_now())\n\n # Gazebo uses ENU frame, but NED frame is used in FDCL.\n # Note that ENU and the NED here refer to their direction order.\n # ENU: E - axis 1, N - axis 2, U - axis 3\n # NED: N - axis 1, E - axis 2, D - axis 3\n self.R_fg = np.array([\n [1.0, 0.0, 0.0],\n [0.0, -1.0, 0.0],\n [0.0, 0.0, -1.0]\n ])\n\n q_gazebo = msg.orientation\n a_gazebo = msg.linear_acceleration\n W_gazebo = msg.angular_velocity\n\n q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w])\n\n R_gi = q_to_R(q) # IMU to Gazebo frame\n R_fi = self.R_fg @ R_gi # IMU to FDCL frame (NED frame)\n\n self.R_imu = R_fi\n\n # FDCL-UAV expects IMU accelerations without gravity.\n a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z])\n self.a_imu = R_gi.T @ (R_gi @ a_i - self.ge3)\n\n self.W_imu = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z])\n\n\n def gps_callback(self, msg):\n \"\"\"Callback function for the GPS subscriber.\n\n Args:\n msg: (GPS) GPS message\n \"\"\"\n\n self.freq_gps.update(self.get_t_now())\n\n x_gazebo = msg.pose.pose.position\n v_gazebo = msg.twist.twist.linear\n\n # Gazebo uses ENU frame, but NED frame is used in FDCL.\n self.x_gps = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z])\n self.v_gps = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z])\n\n\n def fM_callback(self, msg):\n\n self.freq_control.update(self.get_t_now())\n\n self.fM[0] = msg.wrench.force.z\n self.fM[1] = msg.wrench.torque.x\n self.fM[2] = msg.wrench.torque.y\n self.fM[3] = msg.wrench.torque.z\n\n\ndef main(args=None):\n print(\"Starting gui node\")\n\n rclpy.init(args=args)\n\n app = QApplication([])\n app.processEvents()\n\n gui = GuiNode()\n gui.show()\n\n executor = rclpy.executors.SingleThreadedExecutor()\n executor.add_node(gui)\n\n while rclpy.ok():\n executor.spin_once()\n app.processEvents()\n \n app.quit()\n\n # Destroy the node explicitly\n # (optional - otherwise it will be done automatically\n # when the garbage collector destroys the node object)\n gui.destroy_node()\n \n # rclpy.shutdown()\n \n print(\"Terminating gui node\")" }, { "path": "src/fdcl_uav/fdcl_uav/integral_utils.py", "content": "import numpy as np\n\n\nclass IntegralErrorVec3:\n def __init__(self):\n self.error = np.zeros(3)\n self.integrand = np.zeros(3)\n\n\n def integrate(self, current_integrand, dt):\n self.error += (self.integrand + current_integrand) * dt / 2.0\n self.integrand = current_integrand\n\n def set_zero(self):\n self.error = np.zeros(3)\n self.integrand = np.zeros(3)\n\n\nclass IntegralError:\n def __init__(self):\n self.error = 0.0\n self.integrand = 0.0\n\n\n def integrate(self, current_integrand, dt):\n self.error += (self.integrand + current_integrand) * dt / 2.0\n self.integrand = current_integrand\n\n def set_zero(self):\n self.error = 0.0\n self.integrand = 0.0" }, { "path": "src/fdcl_uav/fdcl_uav/matrix_utils.py", "content": "import numpy as np\n# import pdb\n\n\ndef hat(x):\n \"\"\"Returns the hat map of a given 3x1 vector.\n\n Args:\n x: (3x1 numpy array) vector\n\n Returns:\n x_hat: (3x3 numpy array) hat of the input vector\n \"\"\"\n \n ensure_vector(x, 3)\n x_hat = np.array([\n [0.0, -x[2], x[1]],\n [x[2], 0.0, -x[0]],\n [-x[1], x[0], 0.0]\n ])\n \n return x_hat\n\n\ndef vee(x):\n \"\"\"Returns the vee map of a given 3x3 matrix.\n\n Args:\n x: (3x3 numpy array) hat of the input vector\n\n Returns:\n (3x1 numpy array) vee map of the input matrix\n \"\"\"\n ensure_skew(x, 3)\n return np.array([x[2,1], x[0,2], x[1,0]])\n\n\ndef q_to_R(q):\n \"\"\"Converts a quaternion of a rotation matrix in SO(3).\n\n Args:\n q: (4x1 numpy array) quaternion in [x, y, z, w] format\n\n Returns:\n R: (3x3 numpy array) rotation matrix corresponding to the quaternion\n \"\"\"\n\n # TODO: ensure quaternion instead of ensure vector\n ensure_vector(q, 4)\n\n R = np.identity(3)\n q13 = np.array([q[0], q[1], q[2]])\n q4 = q[3]\n\n hat_q = hat(q13)\n R += 2.0 * q4 * hat_q + 2.0 * hat_q @ hat_q\n\n return R\n\n\ndef R_to_RPY(R):\n \"\"\"Converts a rotation matrix to Euler angles.\n\n Args:\n R: (3x3 numpy array) rotation matrix\n\n Returns:\n (3x1 numpy array) Euler angles in [roll, pitch, yaw] format\n \"\"\"\n threshold = 0.999999999999999\n if np.abs(R[2, 0]) > threshold: \n sign_pitch = -np.sign(R[2, 0])\n pitch = sign_pitch * np.pi/2\n\n # Set yaw to 0\n yaw = 0\n\n roll = np.arctan2(R[0, 1] * sign_pitch, R[0, 2] * sign_pitch)\n\n return np.array((roll, pitch, yaw))\n \n else:\n pitch = np.arcsin(-R[2, 0])\n cos_pitch = np.cos(pitch)\n \n sin_roll = R[2, 1] / cos_pitch\n cos_roll = R[2, 2] / cos_pitch\n roll = np.pi - np.arctan2(sin_roll, cos_roll)\n\n sin_yaw = R[1, 0] / cos_pitch\n cos_yaw = R[0, 0] / cos_pitch\n yaw = np.arctan2(sin_yaw, cos_yaw)\n\n return np.array((roll,pitch,yaw))\n \n\ndef deriv_unit_vector(A, A_dot, A_2dot):\n \"\"\"Returns the unit vector and it's derivatives for a given vector.\n\n Args:\n A: (3x1 numpy array) vector\n A_dot: (3x1 numpy array) first derivative of the vector\n A_2dot: (3x1 numpy array) second derivative of the vector\n\n Returns:\n q: (3x1 numpy array) unit vector of A\n q_dot: (3x1 numpy array) first derivative of q\n q_2dot: (3x1 numpy array) second derivative of q\n \"\"\"\n\n ensure_vector(A, 3)\n ensure_vector(A_dot, 3)\n ensure_vector(A_2dot, 3)\n\n nA = np.linalg.norm(A)\n\n if abs(np.linalg.norm(nA)) < 1.0e-9:\n raise ZeroDivisionError('The 2-norm of A should not be zero')\n\n nA3 = nA * nA * nA\n nA5 = nA3 * nA * nA\n\n A_A_dot = A.dot(A_dot)\n\n q = A / nA\n q_dot = A_dot / nA - A.dot(A_A_dot) / nA3\n\n q_2dot = A_2dot / nA \\\n - A_dot.dot(2.0 * A_A_dot) / nA3 \\\n - A.dot(A_dot.dot(A_dot) + A.dot(A_2dot)) / nA3 \\\n + 3.0 * A.dot(A_A_dot).dot(A_A_dot) / nA5\n\n return (q, q_dot, q_2dot)\n\n\ndef saturate(x, x_min, x_max):\n \"\"\"Saturate input vector between two values.\n \n Args:\n x: (nx1 numpy ndarray) value\n x_min: (float) minimum value for x\n x_max: (float) maximum value for x\n \n Returns:\n (nx1 numpy ndarray) saturated x\n \"\"\"\n\n for i in range(len(x)):\n if x[i] > x_max:\n x[i] = x_max\n elif x[i] < x_min:\n x[i] = x_min\n \n return x\n\n\ndef expm_SO3(r):\n \"\"\"Returns the matrix exponential of a matrix in SO(3).\n\n Args:\n r: (3x1 numpy array) vector\n\n Returns:\n R: (3x3 numpy array) matrix exponential of r\n \"\"\"\n\n ensure_vector(r, 3)\n\n theta = np.linalg.norm(r)\n\n y = sinx_over_x(theta)\n y2 = sinx_over_x(theta / 2.0)\n\n hat_r = hat(r)\n R = np.eye(3) + y * hat_r + 0.5 * y2**2 * hat_r @ hat_r\n\n return R\n\n\ndef sinx_over_x(x):\n \"\"\"Calculate sin(x)/x, while dealing with the cases where denominator is \n zero.\n\n Args:\n x: (float) value\n\n Returns:\n y: (float) value of sin(x)/x\n \"\"\"\n \n eps = 1e-6\n if abs(x) < eps:\n y = - x**10 / 39916800.0 + x**8 / 362880.0 - x**6 / 5040.0 \\\n + x**4 / 120.0 - x**2 / 6.0 + 1.0\n else:\n y = np.sin(x) / x\n \n return y\n\n\ndef ensure_vector(x, n):\n \"\"\"Make sure the given input array x is a vector of size n.\n\n Args:\n x: (nx1 numpy array) vector\n n: (int) desired length of the vector\n\n Returns:\n True if the input array is satisfied with size constraint. Raises an\n exception otherwise.\n \"\"\"\n\n np.atleast_2d(x) # Make sure the array is atleast 2D.\n\n if not len(np.ravel(x)) == n:\n raise ValueError('Input array needs to be of length {}, but the size' \\\n 'detected is {}'.format(n, np.shape(x)))\n \n return True\n\n\ndef ensure_matrix(x, m, n):\n \"\"\"Make sure the given input array x is a matrix of size mxn.\n\n Args:\n x: (mxn numpy array) array\n m: (int) desired number of rows\n n: (int) desired number of columns\n\n Returns:\n True if the input array is satisfied with size constraint. Raises an\n exception otherwise.\n \"\"\"\n\n np.atleast_2d(x) # Make sure the array is atleast 2D.\n\n if not np.shape(x) == (m, n):\n raise ValueError('Input array needs to be of size {} x {}, but the ' \\\n 'size detected is {}'.format(m, n, np.shape(x)))\n \n return True\n\n\ndef ensure_skew(x, n):\n \"\"\"Make sure the given input array is a skew-symmetric matrix of size nxn.\n\n Args:\n x: (nxn numpy array) array\n m: (int) desired number of rows and columns\n\n Returns:\n True if the input array is a skew-symmetric matrix. Raises an\n exception otherwise.\n \"\"\"\n ensure_matrix(x, n, n)\n \n if not np.allclose(x.T, -x):\n raise ValueError('Input array must be a skew-symmetric matrix')\n \n return True\n\n\ndef ensure_SO3(x):\n \"\"\"Make sure the given input array is in SO(3).\n\n Args:\n x: (3x3 numpy array) matrix\n\n Returns:\n True if the input array is in SO(3). Raises an exception otherwise.\n \"\"\"\n ensure_matrix(x, 3, 3)\n \n if not np.array_equal(x.T @ x, np.eye(3)):\n raise ValueError('Input array does not satisfy R^TR = I')\n\n if not abs(np.linalg.det(x)) - 1.0 < 1e-9:\n raise ValueError('Input array does not det(R) = 1')\n \n return True" }, { "path": "src/fdcl_uav/fdcl_uav/trajectory.py", "content": "import datetime\nimport numpy as np\n\nimport rclpy\nfrom rclpy.node import Node\n\nfrom uav_gazebo.msg import DesiredData, ModeData, StateData\n\nclass TrajectoryNode(Node):\n\n def __init__(self):\n Node.__init__(self, 'trajectory')\n\n self.mode = 0\n self.is_mode_changed = False\n self.is_landed = False\n\n self.t0 = self.get_clock().now()\n self.t = 0.0\n self.t_traj = 0.0\n\n self.xd = np.zeros(3)\n self.xd_dot = np.zeros(3)\n self.xd_2dot = np.zeros(3)\n self.xd_3dot = np.zeros(3)\n self.xd_4dot = np.zeros(3)\n\n self.b1d = np.zeros(3)\n self.b1d[0] = 1.0\n self.b1d_dot = np.zeros(3)\n self.b1d_2dot = np.zeros(3)\n\n self.x = np.zeros(3)\n self.v = np.zeros(3)\n self.a = np.zeros(3)\n self.R = np.identity(3)\n self.W = np.zeros(3)\n\n self.x_init = np.zeros(3)\n self.v_init = np.zeros(3)\n self.a_init = np.zeros(3)\n self.R_init = np.identity(3)\n self.W_init = np.zeros(3)\n self.b1_init = np.zeros(3)\n self.theta_init = 0.0\n\n self.trajectory_started = False\n self.trajectory_complete = False\n \n # Manual mode\n self.manual_mode = False\n self.manual_mode_init = False\n self.x_offset = np.zeros(3)\n self.yaw_offset = 0.0\n\n # Take-off\n self.takeoff_end_height = -1.0 # (m)\n self.takeoff_velocity = -1.0 # (m/s)\n\n # Landing\n self.landing_velocity = 1.0 # (m/s)\n self.landing_motor_cutoff_height = -0.25 # (m)\n\n # Circle\n self.circle_center = np.zeros(3)\n self.circle_linear_v = 1.0 \n self.circle_W = 1.2\n self.circle_radius = 1.2\n\n self.waypoint_speed = 2.0 # (m/s)\n\n self.e1 = np.array([1.0, 0.0, 0.0])\n\n self.init_subscribers()\n self.init_publishers()\n\n\n def init_subscribers(self):\n\n self.sub_states = self.create_subscription( \\\n StateData,\n '/uav/states',\n self.states_callback,\n 1)\n \n self.sub_mode = self.create_subscription( \\\n ModeData,\n '/uav/mode',\n self.mode_callback,\n 1)\n \n\n def init_publishers(self):\n\n self.pub_trajectory = self.create_publisher( \\\n DesiredData,\n '/uav/trajectory',\n 1)\n\n\n def states_callback(self, msg):\n\n for i in range(3):\n self.x[i] = msg.position[i]\n self.v[i] = msg.velocity[i]\n self.a[i] = msg.acceleration[i]\n self.W[i] = msg.angular_velocity[i]\n\n for j in range(3):\n self.R[i, j] = msg.attitude[3*i + j]\n\n self.calculate_trajectory()\n self.publish_trajectory()\n\n \n def mode_callback(self, msg):\n \n self.x_offset = msg.x_offset\n self.yaw_offset = msg.yaw_offset\n\n if self.mode != msg.mode:\n self.mode = msg.mode\n self.is_mode_changed = True\n self.mark_traj_start()\n self.get_logger().info('Mode switched to {}'.format(self.mode))\n\n\n def publish_trajectory(self):\n\n msg = DesiredData()\n\n for i in range(3):\n msg.position[i] = self.xd[i]\n msg.velocity[i] = self.xd_dot[i]\n msg.acceleration[i] = self.xd_2dot[i]\n msg.jerk[i] = self.xd_3dot[i]\n msg.snap[i] = self.xd_4dot[i]\n\n msg.b1[i] = self.b1d[i]\n msg.b1_dot[i] = self.b1d_dot[i]\n msg.b1_2dot[i] = self.b1d_2dot[i]\n\n msg.is_landed = self.is_landed\n\n self.pub_trajectory.publish(msg)\n\n \n def calculate_trajectory(self):\n if self.manual_mode:\n self.manual()\n return\n \n if self.mode == 0 or self.mode == 1: # idle and warm-up\n self.set_desired_states_to_zero()\n self.mark_traj_start()\n elif self.mode == 2: # take-off\n self.takeoff()\n elif self.mode == 3: # stay\n self.stay()\n elif self.mode == 4: # circle\n self.circle()\n elif self.mode == 5: # land\n self.land()\n else:\n self.get_logger().info('Undefined mode {}'.format(self.mode))\n\n\n def mark_traj_start(self):\n self.trajectory_started = False\n self.trajectory_complete = False\n\n self.manual_mode = False\n self.manual_mode_init = False\n self.is_landed = False\n\n self.t = 0.0\n self.t_traj = 0.0\n self.t0 = self.get_clock().now()\n\n self.x_offset = np.zeros(3)\n self.yaw_offset = 0.0\n\n self.update_initial_state()\n\n\n def mark_traj_end(self, switch_to_manual=False):\n self.trajectory_complete = True\n\n if switch_to_manual:\n self.manual_mode = True\n\n\n def set_desired_states_to_zero(self):\n self.xd = np.zeros(3)\n self.xd_dot = np.zeros(3)\n self.xd_2dot = np.zeros(3)\n self.xd_3dot = np.zeros(3)\n self.xd_4dot = np.zeros(3)\n\n self.b1d = np.array([1.0, 0.0, 0.0])\n self.b1d_dot = np.zeros(3)\n self.b1d_2dot = np.zeros(3)\n\n \n def set_desired_states_to_current(self):\n self.xd = np.copy(self.x)\n self.xd_dot = np.copy(self.v)\n self.xd_2dot = np.zeros(3)\n self.xd_3dot = np.zeros(3)\n self.xd_4dot = np.zeros(3)\n\n self.b1d = self.get_current_b1()\n self.b1d_dot = np.zeros(3)\n self.b1d_2dot = np.zeros(3)\n\n\n def update_initial_state(self):\n self.x_init = np.copy(self.x)\n self.v_init = np.copy(self.v)\n self.a_init = np.copy(self.a)\n self.R_init = np.copy(self.R)\n self.W_init = np.copy(self.W)\n\n self.b1_init = self.get_current_b1()\n self.theta_init = np.arctan2(self.b1_init[1], self.b1_init[0])\n\n \n def get_current_b1(self):\n b1 = self.R @ self.e1\n theta = np.arctan2(b1[1], b1[0])\n return np.array([np.cos(theta), np.sin(theta), 0.0])\n\n\n def waypoint_reached(self, waypoint, current, radius):\n delta = waypoint - current\n \n if abs(np.linalg.norm(delta) < radius):\n return True\n else:\n return False\n\n\n def update_current_time(self):\n t_now = self.get_clock().now()\n self.t = float((t_now - self.t0).nanoseconds) * 1e-9\n\n\n def manual(self):\n if not self.manual_mode_init:\n self.set_desired_states_to_current()\n self.update_initial_state()\n\n self.manual_mode_init = True\n self.x_offset = np.zeros(3)\n self.yaw_offset = 0.0\n\n print('Switched to manual mode')\n \n self.xd = self.x_init + self.x_offset\n self.xd_dot = (self.xd - self.x) / 1.0\n\n theta = self.theta_init + self.yaw_offset\n self.b1d = np.array([np.cos(theta), np.sin(theta), 0.0])\n\n\n def takeoff(self):\n if not self.trajectory_started:\n self.set_desired_states_to_zero()\n\n # Take-off starts from the current horizontal position.\n self.xd = np.copy(self.x)\n\n self.xd_dot = np.zeros(3)\n self.xd_dot[2] = self.takeoff_velocity\n\n self.t_traj = abs((self.takeoff_end_height - self.x[2]) / self.takeoff_velocity)\n\n # Set the takeoff attitude to the current attitude.\n self.b1d = self.get_current_b1()\n\n self.trajectory_started = True\n\n takeoff_end_position = np.copy(self.x_init)\n takeoff_end_position[2] += self.takeoff_velocity * self.t_traj\n\n self.get_logger().info('Takeoff started')\n self.get_logger().info('Starting from ' + str(self.x))\n self.get_logger().info('Takeoff time ' + str(self.t_traj))\n self.get_logger().info('Reaching ' + str(takeoff_end_position))\n self.get_logger().info('Takeoff velocity ' + str(self.takeoff_velocity) + ' m/s')\n\n self.update_current_time()\n\n if self.t < self.t_traj:\n self.xd[2] = self.x_init[2] + self.takeoff_velocity * self.t\n self.xd_dot[2] = self.takeoff_velocity\n\n elif not self.waypoint_reached(self.xd, self.x, 0.04):\n self.xd[2] = self.takeoff_end_height\n xd_dot_gain = 0.1\n self.xd_dot = xd_dot_gain * (self.x - self.xd)\n\n else:\n self.xd[2] = self.takeoff_end_height\n self.xd_dot[2] = 0.0\n\n if not self.trajectory_complete:\n self.get_logger().info('Takeoff complete')\n self.get_logger().info('Switching to manual mode')\n\n self.mark_traj_end(True)\n \n\n def land(self):\n if not self.trajectory_started:\n self.set_desired_states_to_current()\n self.t_traj = (self.landing_motor_cutoff_height - self.x[2]) / \\\n self.landing_velocity\n\n # Set the landing attitude to the current attitude.\n self.b1d = self.get_current_b1()\n\n self.trajectory_started = True\n\n self.update_current_time()\n\n if self.t < self.t_traj:\n self.xd[2] = self.x_init[2] + self.landing_velocity * self.t\n self.xd_2dot[2] = self.landing_velocity\n else:\n if self.x[2] > self.landing_motor_cutoff_height:\n self.xd[2] = self.landing_motor_cutoff_height\n self.xd_dot[2] = 0.0\n\n if not self.trajectory_complete:\n self.get_logger().info('Landing complete')\n\n\n self.mark_traj_end(False)\n self.is_landed = True\n else:\n self.xd[2] = self.landing_motor_cutoff_height\n self.xd_dot[2] = self.landing_velocity\n\n \n def stay(self):\n if not self.trajectory_started:\n self.set_desired_states_to_current()\n self.trajectory_started = True\n \n self.mark_traj_end(True)\n\n\n def circle(self):\n if not self.trajectory_started:\n self.set_desired_states_to_current()\n self.trajectory_started = True\n\n self.circle_center = np.copy(self.x)\n self.circle_W = 2 * np.pi / 8\n\n num_circles = 2\n self.t_traj = self.circle_radius / self.circle_linear_v \\\n + num_circles * 2 * np.pi / self.circle_W\n\n self.update_current_time()\n\n if self.t < (self.circle_radius / self.circle_linear_v):\n self.xd[0] = self.circle_center[0] + self.circle_linear_v * self.t\n self.xd_dot[0] = self.circle_linear_v\n\n elif self.t < self.t_traj:\n circle_W = self.circle_W\n circle_radius = self.circle_radius\n\n t = self.t - circle_radius / self.circle_linear_v\n th = circle_W * t\n\n circle_W2 = circle_W * circle_W\n circle_W3 = circle_W2 * circle_W\n circle_W4 = circle_W3 * circle_W\n\n # axis 1\n self.xd[0] = circle_radius * np.cos(th) + self.circle_center[0]\n self.xd_dot[0] = - circle_radius * circle_W * np.sin(th)\n self.xd_2dot[0] = - circle_radius * circle_W2 * np.cos(th)\n self.xd_3dot[0] = circle_radius * circle_W3 * np.sin(th)\n self.xd_4dot[0] = circle_radius * circle_W4 * np.cos(th)\n\n # axis 2\n self.xd[1] = circle_radius * np.sin(th) + self.circle_center[1]\n self.xd_dot[1] = circle_radius * circle_W * np.cos(th)\n self.xd_2dot[1] = - circle_radius * circle_W2 * np.sin(th)\n self.xd_3dot[1] = - circle_radius * circle_W3 * np.cos(th)\n self.xd_4dot[1] = circle_radius * circle_W4 * np.sin(th)\n\n w_b1d = 2.0 * np.pi / 10.0\n th_b1d = w_b1d * t\n\n self.b1d = np.array([np.cos(th_b1d), np.sin(th_b1d), 0])\n self.b1d_dot = np.array([- w_b1d * np.sin(th_b1d), \\\n w_b1d * np.cos(th_b1d), 0.0])\n self.b1d_2dot = np.array([- w_b1d * w_b1d * np.cos(th_b1d),\n w_b1d * w_b1d * np.sin(th_b1d), 0.0])\n else:\n self.mark_traj_end(True)\n\n\ndef main(args=None):\n print(\"Starting trajectory node\")\n\n rclpy.init(args=args)\n\n trajectory = TrajectoryNode()\n\n try:\n rclpy.spin(trajectory)\n except KeyboardInterrupt:\n pass\n\n # Destroy the node explicitly\n # (optional - otherwise it will be done automatically\n # when the garbage collector destroys the node object)\n trajectory.destroy_node()\n\n print(\"Terminating trajectory node\")" }, { "path": "src/fdcl_uav/launch/fdcl_uav_launch.py", "content": "from launch import LaunchDescription\nfrom launch_ros.actions import Node\n\ndef generate_launch_description():\n return LaunchDescription([\n Node(\n package='fdcl_uav',\n namespace='estimator',\n executable='estimator',\n name='sim'\n ),\n Node(\n package='fdcl_uav',\n namespace='control',\n executable='control',\n name='sim'\n ),\n Node(\n package='fdcl_uav',\n namespace='trajectory',\n executable='trajectory',\n name='sim'\n ),\n Node(\n package='fdcl_uav',\n namespace='gui',\n executable='gui',\n name='sim'\n )\n ])" }, { "path": "src/fdcl_uav/package.xml", "content": "\n\n\n fdcl_uav\n 0.0.0\n FDCL UAV Simulator\n kani\n MIT\n\n rclpy\n std_msgs\n\n ros2launch\n\n \n ament_python\n \n\n" }, { "path": "src/fdcl_uav/resource/fdcl_uav", "content": "" }, { "path": "src/fdcl_uav/setup.cfg", "content": "[develop]\nscript_dir=$base/lib/fdcl_uav\n[install]\ninstall_scripts=$base/lib/fdcl_uav\n" }, { "path": "src/fdcl_uav/setup.py", "content": "from setuptools import find_packages, setup\nfrom glob import glob\nimport os\n\npackage_name = 'fdcl_uav'\n\nsetup(\n name=package_name,\n version='0.0.0',\n packages=find_packages(exclude=['test']),\n data_files=[\n ('share/ament_index/resource_index/packages',\n ['resource/' + package_name]),\n ('share/' + package_name, ['package.xml']),\n (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*')))\n ],\n install_requires=['setuptools'],\n zip_safe=True,\n maintainer='Kanishke Gamagedara',\n maintainer_email='kanishkegb@gwu.edu',\n description='FDCL UAV Simulator',\n license='MIT',\n tests_require=['pytest'],\n entry_points={\n 'console_scripts': [\n 'estimator = fdcl_uav.estimator:main',\n 'control = fdcl_uav.control:main',\n 'trajectory = fdcl_uav.trajectory:main',\n 'gui = fdcl_uav.gui:main',\n ],\n },\n)\n" }, { "path": "src/uav_gazebo/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.22.1)\nproject(uav_gazebo)\n\nset(CMAKE_CXX_STANDARD 14)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(rosidl_default_generators REQUIRED)\nfind_package(std_msgs REQUIRED)\nfind_package(xacro REQUIRED)\n\nxacro_add_files(\"urdf/uav.urdf.xacro\" INSTALL DESTINATION urdf/)\n\nrosidl_generate_interfaces(${PROJECT_NAME}\n \"msg/DesiredData.msg\"\n \"msg/ErrorData.msg\"\n \"msg/ModeData.msg\"\n \"msg/StateData.msg\"\n DEPENDENCIES std_msgs\n )\n\ninstall(DIRECTORY launch meshes worlds msg\n DESTINATION share/${PROJECT_NAME}\n)\n\ninstall(FILES model.config\n DESTINATION share/${PROJECT_NAME}\n)\n\nament_package()" }, { "path": "src/uav_gazebo/launch/uav_gazebo.launch.py", "content": "from ament_index_python.packages import get_package_share_directory\n\nfrom launch import LaunchDescription\nfrom launch.actions import DeclareLaunchArgument\nfrom launch.actions import IncludeLaunchDescription\nfrom launch.launch_description_sources import PythonLaunchDescriptionSource\nfrom launch_ros.actions import Node\n\nfrom scripts import GazeboRosPaths\n\nimport os\n\npkg_gazebo_ros = get_package_share_directory('gazebo_ros')\npkg_uav_gazebo = get_package_share_directory('uav_gazebo')\n\ndef generate_launch_description():\n\n model_path, plugin_path, media_path = GazeboRosPaths.get_paths()\n\n # print(plugin_path)\n\n env = {\n \"GAZEBO_MODEL_PATH\": model_path,\n \"GAZEBO_PLUGIN_PATH\": plugin_path,\n \"GAZEBO_RESOURCE_PATH\": media_path,\n }\n\n # World\n world_path = os.path.join(pkg_uav_gazebo, 'worlds', 'simple.world')\n world_file = DeclareLaunchArgument(\n 'world',\n default_value=[world_path],\n description='SDF world file'\n )\n\n # Gazebo\n gazebo = IncludeLaunchDescription(\n PythonLaunchDescriptionSource(\n os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'),\n )\n ) \n\n # UAV\n urdf_file_name = 'uav.urdf'\n urdf_file = os.path.join(pkg_uav_gazebo, 'urdf', urdf_file_name)\n\n spawn_uav = Node(\n package='gazebo_ros', \n executable='spawn_entity.py', \n arguments=['-entity', 'uav', '-file', urdf_file, \\\n '-x', '0', '-y', '0', '-z', '0.2'],\n output='screen',\n additional_env=env\n )\n\n return LaunchDescription([\n world_file,\n gazebo,\n spawn_uav\n ])" }, { "path": "src/uav_gazebo/meshes/quadrotor_base.dae", "content": "\n\n \n \n Stefan Kohlbrecher\n Blender 2.61.4 r43829\n \n 2012-02-03T14:20:49\n 2012-02-03T14:20:49\n \n Z_UP\n \n \n \n \n \n \n \n 0 0 0 1\n \n \n 0.1 0.1 0.1 1\n \n \n 1 0 0.0106101 1\n \n \n 0.5 0.5 0.5 1\n \n \n 50\n \n \n 1\n \n \n \n \n \n 1\n \n \n \n 1\n \n \n \n \n \n \n 0.15 0.15 0.15 1\n \n \n 0.2 0.2 0.2 1\n \n \n 0.48 0.48 0.48 1\n \n \n 1 1 1 1\n \n \n 33\n \n \n 1\n \n \n \n \n \n 1\n \n \n \n 1\n \n \n \n \n \n \n 0 0 0 1\n \n \n 0.1 0.1 0.1 1\n \n \n 0.05496641 0.05496641 0.05496641 1\n \n \n 0.5 0.5 0.5 1\n \n \n 50\n \n \n 1\n \n \n \n \n \n 1\n \n \n \n 1\n \n \n \n \n \n \n 0.15 0.15 0.15 1\n \n \n 0.23 0.23 0.23 1\n \n \n 0.5 0.5 0.5 1\n \n \n 1 1 1 1\n \n \n 20\n \n \n 1\n \n \n \n \n \n 1\n \n \n \n 1\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 0.8236175 0.673869 -1.366201 -0.8236175 0.6738689 -1.366201 -0.8236175 0.6852639 -1.361481 0.8236175 0.6852641 -1.361481 -0.8236175 0.6899839 -1.350086 0.8236175 0.689984 -1.350086 -0.8236175 0.6852639 -1.338691 0.8236175 0.685264 -1.338691 -0.8236175 0.6738689 -1.333971 0.8236175 0.673869 -1.333971 -0.8236175 0.6624739 -1.338691 0.8236175 0.662474 -1.338691 -0.8236175 0.657754 -1.350086 0.8236175 0.6577541 -1.350086 -0.8236175 0.6624739 -1.361481 0.8236175 0.6624741 -1.361481 0.8236175 0.673869 -1.350086 -0.8236175 0.6738689 -1.350086 0.8236174 -0.6738691 -1.350086 0.8236174 -0.662474 -1.361481 0.8236174 -0.6738691 -1.366201 -0.8236175 -0.6738691 -1.350086 -0.8236175 -0.6738691 -1.366201 -0.8236175 -0.6624742 -1.361481 0.8236174 -0.6577541 -1.350086 -0.8236175 -0.6577541 -1.350086 0.8236174 -0.662474 -1.338691 -0.8236175 -0.662474 -1.338691 0.8236174 -0.6738691 -1.333971 -0.8236175 -0.6738691 -1.333971 0.8236174 -0.6852641 -1.338691 -0.8236175 -0.6852641 -1.338691 0.8236174 -0.689984 -1.350086 -0.8236175 -0.689984 -1.350086 0.8236174 -0.6852641 -1.361481 -0.8236175 -0.6852641 -1.361481 0.5614412 0.08138328 0 0.5614412 0.07047998 -0.04069161 0.5614412 0.04069161 -0.07047998 0.5614412 0 -0.08138328 0.5614412 -0.04069161 -0.07047998 0.5614413 -0.07047998 -0.04069161 0.5614413 -0.08138328 0 0.5614413 -0.07047998 0.04069155 0.5614412 -0.04069167 0.07047992 0.5614412 0 0.08138328 0.5614412 0.04069155 0.07047998 0.5614412 0.07047992 0.04069167 2.084521 0.08138328 0 2.084521 0.07048004 -0.04069155 2.084521 0.04069167 -0.07047986 2.084521 0 -0.08138328 2.084521 -0.04069155 -0.07047998 2.084521 -0.07047986 -0.04069167 2.084521 -0.08138328 0 2.084521 -0.07048004 0.04069155 2.084521 -0.04069173 0.07047992 2.084521 0 0.08138328 2.084521 0.04069155 0.07048004 2.084521 0.07047992 0.04069173 0.5614412 0 0 2.084521 0 0 0.6738689 0.6899839 -1.332763 0.6852639 0.6852639 -1.332763 0.6899839 0.6738689 -1.332763 0.6852639 0.6624739 -1.332763 0.6738689 0.657754 -1.332763 0.6624739 0.6624739 -1.332763 0.657754 0.6738689 -1.332763 0.6624739 0.6852639 -1.332763 0.2994973 0.3156122 -0.303241 0.3108922 0.3108922 -0.303241 0.3156122 0.2994973 -0.303241 0.3108922 0.2881023 -0.303241 0.2994973 0.2833823 -0.303241 0.2881022 0.2881023 -0.303241 0.2833823 0.2994973 -0.303241 0.2881023 0.3108922 -0.303241 0.6738689 0.6738689 -1.332763 0.2994973 0.2994973 -0.303241 0.3156121 -0.2994973 -0.303241 0.6899838 -0.673869 -1.332763 0.6852638 -0.6624739 -1.332763 0.3108922 -0.2881023 -0.303241 0.6738688 -0.657754 -1.332763 0.2994972 -0.2833823 -0.303241 0.6624738 -0.6624739 -1.332763 0.2881022 -0.2881023 -0.303241 0.6577539 -0.673869 -1.332763 0.2833822 -0.2994973 -0.303241 0.6624738 -0.6852639 -1.332763 0.2881022 -0.3108923 -0.303241 0.6738688 -0.6899839 -1.332763 0.2994972 -0.3156122 -0.303241 0.6852638 -0.6852639 -1.332763 0.3108922 -0.3108922 -0.303241 0.2994972 -0.2994973 -0.303241 0.6738688 -0.673869 -1.332763 -0.673869 -0.6738688 -1.332763 -0.685264 -0.6852638 -1.332763 -0.6738691 -0.6899837 -1.332763 -0.2994973 -0.2994972 -0.303241 -0.2994973 -0.3156121 -0.303241 -0.3108923 -0.3108922 -0.303241 -0.689984 -0.6738687 -1.332763 -0.3156123 -0.2994972 -0.303241 -0.685264 -0.6624737 -1.332763 -0.3108923 -0.2881022 -0.303241 -0.673869 -0.6577538 -1.332763 -0.2994973 -0.2833822 -0.303241 -0.662474 -0.6624737 -1.332763 -0.2881023 -0.2881022 -0.303241 -0.6577541 -0.6738688 -1.332763 -0.2833824 -0.2994972 -0.303241 -0.662474 -0.6852638 -1.332763 -0.2881023 -0.3108922 -0.303241 -0.3156121 0.2994973 -0.303241 -0.6899837 0.6738693 -1.332763 -0.6852637 0.662474 -1.332763 -0.3108922 0.2881023 -0.303241 -0.6738687 0.6577541 -1.332763 -0.2994971 0.2833824 -0.303241 -0.6624737 0.662474 -1.332763 -0.2881022 0.2881023 -0.303241 -0.6577537 0.6738691 -1.332763 -0.2833822 0.2994973 -0.303241 -0.6624737 0.6852641 -1.332763 -0.2881022 0.3108924 -0.303241 -0.6738685 0.689984 -1.332763 -0.2994971 0.3156123 -0.303241 -0.6852637 0.6852641 -1.332763 -0.3108922 0.3108923 -0.303241 -0.2994971 0.2994973 -0.303241 -0.6738687 0.6738691 -1.332763 1.969194 0 0.1871857 2.886225 -0.3798463 0.2218515 2.671059 -0.7018645 0.2218514 1.969194 0 0.2620601 2.349041 -0.9170302 0.2218514 1.969195 -0.9925863 0.2218514 1.589348 -0.9170302 0.2218514 1.26733 -0.7018646 0.2218514 1.052164 -0.3798465 0.2218515 0.9766083 -2.36674e-7 0.2218515 1.052164 0.379846 0.2218516 1.26733 0.7018642 0.2218517 1.589348 0.9170299 0.2218517 1.969194 0.992586 0.2218518 2.34904 0.91703 0.2218517 2.671058 0.7018645 0.2218517 2.886224 0.3798464 0.2218516 2.96178 3.25924e-7 0.2218515 0 -0.5614412 0 0.08138316 -0.5614412 0 0.07047986 -0.5614412 -0.04069161 -2.69139e-7 -2.084521 0 0.07047975 -2.084521 -0.04069155 0.08138298 -2.084521 0 0.04069155 -0.5614412 -0.07047998 0.04069143 -2.084521 -0.07047992 0 -0.5614412 -0.08138328 -1.69274e-7 -2.084521 -0.08138328 -0.04069167 -0.5614412 -0.07047998 -0.04069179 -2.084521 -0.07047998 -0.07048004 -0.5614413 -0.04069167 -0.07048016 -2.084521 -0.04069167 -0.08138334 -0.5614413 0 -0.08138352 -2.084521 0 -0.07048004 -0.5614413 0.04069161 -0.07048028 -2.084521 0.04069155 -0.04069173 -0.5614412 0.07047998 -0.04069197 -2.084521 0.07047992 -1.3514e-7 -0.5614412 0.08138328 -3.81294e-7 -2.084521 0.08138328 0.04069143 -0.5614412 0.07047998 0.04069125 -2.084521 0.07048004 0.07047986 -0.5614412 0.04069167 0.07047963 -2.084521 0.04069173 0.379846 -2.886224 0.2218516 -2.61752e-7 -1.969194 0.1871857 0 -2.96178 0.2218515 -3.25223e-7 -1.969194 0.2620601 0.7018641 -2.671058 0.2218517 0.9170297 -2.34904 0.2218517 0.9925858 -1.969194 0.2218517 0.9170296 -1.589348 0.2218517 0.701864 -1.26733 0.2218517 0.3798459 -1.052164 0.2218516 -3.6789e-7 -0.9766083 0.2218515 -0.3798466 -1.052164 0.2218514 -0.7018647 -1.26733 0.2218514 -0.9170304 -1.589348 0.2218513 -0.9925865 -1.969194 0.2218513 -0.9170305 -2.34904 0.2218513 -0.7018648 -2.671059 0.2218514 -0.3798467 -2.886225 0.2218514 -2.96178 4.69959e-7 0.2218515 -2.886225 0.3798471 0.2218514 -1.969194 5.26331e-7 0.1871857 -1.969194 5.89802e-7 0.2620601 -2.671059 0.7018652 0.2218514 -2.34904 0.9170308 0.2218513 -1.969194 0.9925867 0.2218513 -1.589348 0.9170306 0.2218513 -1.26733 0.7018649 0.2218514 -1.052164 0.3798468 0.2218514 -0.9766083 4.99106e-7 0.2218515 -1.052164 -0.3798457 0.2218516 -1.26733 -0.7018638 0.2218517 -1.589348 -0.9170294 0.2218517 -1.969194 -0.9925855 0.2218517 -2.349041 -0.9170294 0.2218517 -2.671058 -0.7018638 0.2218517 -2.886224 -0.3798456 0.2218516 -0.5614412 -0.07047981 0.04069167 -0.5614412 -0.0813831 0 -2.084521 -0.08138269 0 -2.084521 -0.07047933 0.04069173 -0.5614412 -0.04069137 0.07047998 -2.084521 -0.04069095 0.07048004 -0.5614412 2.10574e-7 0.08138328 -2.084521 6.61367e-7 0.08138328 -0.5614412 0.04069179 0.07047998 -2.084521 0.04069226 0.07047992 -0.5614413 0.0704801 0.04069161 -2.084521 0.07048058 0.04069155 -0.5614413 0.0813834 0 -2.084521 0.08138382 0 -0.5614413 0.0704801 -0.04069167 -2.084521 0.07048046 -0.04069167 -0.5614412 0.04069179 -0.07047998 -2.084521 0.04069209 -0.07047998 -0.5614412 1.54427e-7 -0.08138328 -2.084521 4.49347e-7 -0.08138328 -0.5614412 -0.04069149 -0.07047998 -2.084521 -0.04069113 -0.07047992 -0.5614412 -0.07047981 -0.04069161 -2.084521 -0.07047945 -0.04069155 -2.084521 5.49213e-7 0 -0.5614412 1.61804e-7 0 2.37238e-7 0.5614412 0 -0.08138304 0.5614412 0 -0.07047975 0.5614412 -0.04069161 8.29287e-7 2.084521 0 -0.07047921 2.084521 -0.04069155 -0.08138245 2.084521 0 -0.04069137 0.5614412 -0.07047998 -0.04069083 2.084521 -0.07047992 2.29861e-7 0.5614412 -0.08138328 7.29421e-7 2.084521 -0.08138328 0.04069185 0.5614412 -0.07047998 0.04069238 2.084521 -0.07047998 0.07048016 0.5614413 -0.04069167 0.07048076 2.084521 -0.04069167 0.08138352 0.5614413 0 0.08138412 2.084521 0 0.07048022 0.5614413 0.04069161 0.07048088 2.084521 0.04069155 0.04069191 0.5614412 0.07047998 0.04069256 2.084521 0.07047992 2.86009e-7 0.5614412 0.08138328 9.41441e-7 2.084521 0.08138328 -0.04069131 0.5614412 0.07047998 -0.04069072 2.084521 0.07048004 -0.07047975 0.5614412 0.04069167 -0.07047903 2.084521 0.04069173 -0.3798453 2.886224 0.2218516 7.9091e-7 1.969194 0.1871857 8.67901e-7 2.96178 0.2218515 8.54381e-7 1.969194 0.2620601 -0.7018634 2.671058 0.2218517 -0.9170291 2.349041 0.2218517 -0.9925853 1.969195 0.2218517 -0.9170292 1.589348 0.2218517 -0.7018637 1.26733 0.2218517 -0.3798456 1.052164 0.2218516 6.30322e-7 0.9766083 0.2218515 0.3798469 1.052164 0.2218514 0.7018651 1.26733 0.2218514 0.9170309 1.589348 0.2218513 0.992587 1.969194 0.2218513 0.9170311 2.34904 0.2218513 0.7018656 2.671059 0.2218514 0.3798475 2.886225 0.2218514 0.3369342 0.5615574 -0.2994973 0.3369344 -0.5615572 -0.2994973 -0.3369344 -0.5615574 -0.2994973 -0.3369344 0.5615576 -0.2994973 0.3369346 0.5615574 0.2994973 0.3369344 -0.5615576 0.2994973 -0.3369344 -0.5615574 0.2994973 -0.3369344 0.5615572 0.2994973 0.5615572 0.3369344 -0.2994973 -0.5615572 0.3369347 -0.2994973 0.5615576 0.3369343 0.2994973 -0.5615572 0.3369344 0.2994973 0.5615572 -0.3369344 -0.2994973 -0.5615575 -0.3369343 -0.2994973 0.5615574 -0.3369347 0.2994973 -0.5615575 -0.3369344 0.2994973\n \n \n \n \n \n \n \n \n \n 1 0 0 0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 -0.6302378 0.548967 -0.548967 -0.6302378 0 -0.7763603 -1 0 0 0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 0.548967 0.548967 -0.6302378 0.548967 0.548967 0.6302378 0 0.7763603 -0.6302378 0 0.7763603 0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 -0.6302378 -0.7763603 0 0.6302378 -0.548967 -0.548967 -0.6302378 -0.548967 -0.548967 1 0 0 0.6302378 0 -0.7763603 0.6302378 0.548967 -0.548967 -1 0 0 -0.6302378 0.548967 -0.548967 -0.6302378 0 -0.7763603 0.6302378 0.7763603 0 -0.6302378 0.7763603 0 0.6302378 0.548967 0.548967 -0.6302378 0.548967 0.548967 0.6302378 0 0.7763603 -0.6302378 0 0.7763603 0.6302378 -0.548967 0.548967 -0.6302378 -0.548967 0.548967 0.6302378 -0.7763603 0 -0.6302378 -0.7763603 0 0.6302378 -0.548967 -0.548967 -0.6302378 -0.548967 -0.548967 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 -2.53709e-6 1.46479e-6 1 0 0 -1 -1.46479e-6 -3.92489e-7 1 0 0 -1 -1.46479e-6 3.92489e-7 1 0 0 -1 -2.53709e-6 -1.46479e-6 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 0 0 -1 0.06228822 0.8714866 -0.486404 0.6329844 0.6329844 -0.4456618 0 0 1 0.4264656 0.4264656 0.7976318 -0.06045717 0.6646626 0.7446516 0.8714866 0.06228822 -0.486404 0.6646626 -0.06045717 0.7446516 0.6226387 -0.4941862 -0.6066775 0.4941862 -0.6226387 0.6066775 0.06045717 -0.6646626 -0.7446516 -0.06228822 -0.8714866 0.486404 -0.4264656 -0.4264656 -0.7976318 -0.6329844 -0.6329844 0.4456618 -0.6646626 0.06045717 -0.7446516 -0.8714866 -0.06228822 0.486404 -0.4941862 0.6226387 -0.6066775 -0.6226387 0.4941862 0.6066775 0 0 1 0.6646626 0.06045717 0.7446516 0.4941862 0.6226387 0.6066775 0.6226387 0.4941862 -0.6066775 0.8714866 -0.06228822 -0.486404 0 0 -1 -0.06228822 0.8714866 0.486404 0.06045717 0.6646626 -0.7446516 -0.6329844 0.6329844 0.4456618 -0.4264656 0.4264656 -0.7976318 -0.8714866 0.06228822 0.486404 -0.6646626 -0.06045717 -0.7446516 -0.6226387 -0.4941862 0.6066775 -0.4941862 -0.6226387 -0.6066775 -0.06045717 -0.6646626 0.7446516 0.06228822 -0.8714866 -0.486404 0.4264656 -0.4264656 0.7976318 0.6329844 -0.6329844 -0.4456618 0 0 -1 -0.06228822 -0.8714866 -0.486404 -0.6329844 -0.6329844 -0.4456618 0 0 1 -0.4264656 -0.4264656 0.7976318 0.06045717 -0.6646626 0.7446516 -0.8714866 -0.06228822 -0.486404 -0.6646626 0.06045717 0.7446516 -0.6226387 0.4941862 -0.6066775 -0.4941862 0.6226387 0.6066775 -0.06045717 0.6646626 -0.7446516 0.06228822 0.8714866 0.486404 0.4264656 0.4264656 -0.7976318 0.6329844 0.6329844 0.4456618 0.6646626 -0.06045717 -0.7446516 0.8714866 0.06228822 0.486404 0.4941862 -0.6226387 -0.6066775 0.6226387 -0.4941862 0.6066775 0 0 1 -0.6646626 -0.06045717 0.7446516 -0.4941862 -0.6226387 0.6066775 -0.6226387 -0.4941862 -0.6066775 -0.8714866 0.06228822 -0.486404 0 0 -1 0.06228822 -0.8714866 0.486404 -0.06045717 -0.6646626 -0.7446516 0.6329844 -0.6329844 0.4456618 0.4264656 -0.4264656 -0.7976318 0.8714866 -0.06228822 0.486404 0.6646626 0.06045717 -0.7446516 0.6226387 0.4941862 0.6066775 0.4941862 0.6226387 -0.6066775 0.06045717 0.6646626 0.7446516 -0.06228822 0.8714866 -0.486404 -0.4264656 0.4264656 0.7976318 -0.6329844 0.6329844 -0.4456618 0.03490257 -0.006942212 -0.9993667 0.04047429 -0.008050978 0.9991481 0.03431242 -0.02292704 0.9991481 0.02958893 -0.0197705 -0.9993667 0.01977074 -0.02958875 -0.9993666 0.02292686 -0.03431266 0.9991481 0.008050799 -0.04047453 0.9991481 0.00694251 -0.03490239 -0.9993667 -0.00694251 -0.03490239 -0.9993667 -0.008050799 -0.04047447 0.9991481 -0.02292686 -0.03431266 0.9991481 -0.01977074 -0.02958875 -0.9993666 -0.02958893 -0.01977056 -0.9993666 -0.03431248 -0.02292704 0.9991481 -0.04047429 -0.008051037 0.9991481 -0.03490257 -0.006942331 -0.9993666 -0.03490257 0.006942749 -0.9993666 -0.04047429 0.00805062 0.9991481 -0.03431248 0.02292662 0.999148 -0.02958899 0.01977092 -0.9993666 -0.01977074 0.02958917 -0.9993667 -0.02292686 0.03431224 0.9991482 -0.008050799 0.04047405 0.9991481 -0.00694251 0.03490281 -0.9993667 0.00694251 0.03490281 -0.9993667 0.008050799 0.04047411 0.9991481 0.0229268 0.03431224 0.9991482 0.01977074 0.02958923 -0.9993667 0.02958899 0.01977092 -0.9993667 0.03431248 0.02292662 0.9991481 0.04047429 0.00805062 0.9991481 0.03490257 0.006942749 -0.9993667 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 -2.53709e-6 1 1.46479e-6 0 -1 0 -1.46479e-6 1 -3.92489e-7 0 -1 0 -1.46479e-6 1 3.92489e-7 0 -1 0 -2.53709e-6 1 -1.46479e-6 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0.006942689 -0.03490263 -0.9993667 0.00805068 -0.04047423 0.9991481 0.02292662 -0.03431242 0.9991482 0.01977092 -0.02958899 -0.9993667 0.02958923 -0.01977068 -0.9993667 0.03431218 -0.02292686 0.9991481 0.04047405 -0.008050739 0.9991482 0.03490281 -0.00694257 -0.9993666 0.03490281 0.00694257 -0.9993667 0.04047405 0.008050799 0.9991481 0.03431218 0.02292686 0.9991481 0.02958923 0.01977068 -0.9993667 0.01977092 0.02958899 -0.9993666 0.02292662 0.03431242 0.9991481 0.00805062 0.04047423 0.9991481 0.006942689 0.03490263 -0.9993666 -0.006942212 0.03490263 -0.9993666 -0.008051097 0.04047423 0.9991481 -0.02292704 0.03431242 0.9991481 -0.0197705 0.02958893 -0.9993666 -0.02958869 0.01977074 -0.9993666 -0.03431266 0.0229268 0.9991481 -0.04047447 0.008050858 0.9991481 -0.03490233 0.00694251 -0.9993667 -0.03490233 -0.00694251 -0.9993667 -0.04047447 -0.008050858 0.9991481 -0.03431266 -0.0229268 0.9991481 -0.02958875 -0.01977074 -0.9993667 -0.0197705 -0.02958893 -0.9993667 -0.02292704 -0.03431242 0.9991481 -0.008051037 -0.04047423 0.999148 -0.006942152 -0.03490263 -0.9993667 -0.03490263 0.006942152 -0.9993667 -0.04047423 0.008051037 0.9991481 -0.03431242 0.02292704 0.9991481 -0.02958893 0.0197705 -0.9993667 -0.01977068 0.02958875 -0.9993666 -0.02292674 0.03431272 0.9991482 -0.008050858 0.04047447 0.9991481 -0.006942451 0.03490233 -0.9993666 0.00694251 0.03490239 -0.9993667 0.008050858 0.04047447 0.9991481 0.0229268 0.03431266 0.9991481 0.01977074 0.02958869 -0.9993666 0.02958893 0.0197705 -0.9993666 0.03431248 0.02292698 0.9991481 0.04047423 0.008051097 0.9991481 0.03490263 0.006942212 -0.9993666 0.03490263 -0.006942689 -0.9993666 0.04047423 -0.00805062 0.9991481 0.03431242 -0.02292662 0.9991482 0.02958899 -0.01977092 -0.9993667 0.01977068 -0.02958923 -0.9993667 0.02292686 -0.03431218 0.9991481 0.008050799 -0.04047405 0.9991481 0.00694257 -0.03490281 -0.9993667 -0.00694257 -0.03490281 -0.9993667 -0.008050799 -0.04047405 0.9991481 -0.02292686 -0.03431218 0.9991482 -0.01977068 -0.02958917 -0.9993667 -0.02958899 -0.01977092 -0.9993667 -0.03431242 -0.02292662 0.9991482 -0.04047423 -0.00805068 0.9991481 -0.03490263 -0.006942689 -0.9993667 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 2.53709e-6 -1.46479e-6 -1 0 0 1 1.46479e-6 3.92489e-7 -1 0 0 1 1.46479e-6 -3.92489e-7 -1 0 0 1 2.53709e-6 1.46479e-6 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 2.53709e-6 -1 1.46479e-6 0 1 0 1.46479e-6 -1 -3.92489e-7 0 1 0 1.46479e-6 -1 3.92489e-7 0 1 0 2.53709e-6 -1 -1.46479e-6 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 0 -1 0 0 1 0 -0.006942689 0.03490263 -0.9993667 -0.00805068 0.04047423 0.9991481 -0.02292662 0.03431242 0.9991482 -0.01977092 0.02958899 -0.9993667 -0.02958917 0.01977074 -0.9993666 -0.03431218 0.02292692 0.9991483 -0.04047405 0.008050799 0.9991482 -0.03490281 0.00694257 -0.9993666 -0.03490281 -0.00694257 -0.9993667 -0.04047405 -0.008050799 0.9991481 -0.03431218 -0.02292686 0.9991481 -0.02958923 -0.01977068 -0.9993666 -0.01977092 -0.02958899 -0.9993667 -0.02292662 -0.03431242 0.9991481 -0.00805062 -0.04047423 0.9991481 -0.006942689 -0.03490263 -0.9993666 0.006942212 -0.03490263 -0.9993666 0.008051097 -0.04047423 0.9991481 0.02292698 -0.03431248 0.9991482 0.0197705 -0.02958899 -0.9993667 0.02958869 -0.01977074 -0.9993667 0.03431266 -0.0229268 0.999148 0.04047447 -0.008050858 0.9991481 0.03490233 -0.00694251 -0.9993667 0.03490239 0.006942451 -0.9993667 0.04047447 0.008050858 0.9991481 0.03431272 0.02292674 0.9991481 0.02958875 0.01977068 -0.9993667 0.0197705 0.02958893 -0.9993667 0.02292704 0.03431242 0.9991481 0.008051037 0.04047423 0.9991481 0.006942152 0.03490263 -0.9993667 -1 3.53806e-7 0 -1 3.53806e-7 0 -0.7071067 -0.707107 -1.40725e-7 -0.7071068 -0.7071068 0 0.7071068 -0.7071069 -4.92539e-7 0.7071067 -0.7071068 -4.8945e-7 1 -3.53806e-7 -1.99016e-7 1 0 -5.97047e-7 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.7071067 0.7071068 3.9156e-7 -0.7071068 0.7071067 4.92539e-7 0.7071067 0.7071068 -5.2772e-7 0.707107 0.7071065 -3.13248e-7 0 0 1 0 0 1 0 0 -1 0 0 -1 -2.65354e-7 1 0 3.53806e-7 1 6.96555e-7 -3.53806e-7 -1 -6.96555e-7 2.65354e-7 -1 0 -0.6557207 -0.6532182 0.3785516 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0 -0.6557207 0.6532182 0.3785516 -0.3785516 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 0 -0.6532182 0.7571337 0 0.6532182 0.7571337 0.3785516 -0.6532182 0.6557207 0.3785821 0.6532182 0.6557207 0.6557207 -0.6532182 0.3785516 0.6557207 0.6532182 0.3785516 0.7571337 -0.6532182 0 0.7571337 0.6532182 0 0.6557207 -0.6532182 -0.3785516 0.6557207 0.6532182 -0.3785516 0.3785516 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0 -0.6532182 -0.7571337 0 0.6532182 -0.7571337 -0.3785516 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.6557207 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 0.6532182 -0.7571337 0 -0.6532182 -0.7571337 0 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 0 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0.3785516 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 -0.3785516 -0.6532182 0.7571337 0 0.6532182 0.7571337 0 -0.6532182 0.6557207 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.3785821 0.6557207 0.6532182 0.3785821 0.6557207 -0.6532182 0 0.7571337 0.6532182 0 0.7571337 -0.6532182 -0.3785516 0.6557207 0.6532182 -0.3785516 0.6557207 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0.3785516 0.6557207 0.6532182 0.3785516 0.7571337 -0.6532182 0 0.7571337 0.6532182 0 0.6557207 -0.6532182 0.3785516 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.6557207 0 0.6532182 0.7571337 0 -0.6532182 0.7571337 -0.3785821 0.6532182 0.6557207 -0.3785821 -0.6532182 0.6557207 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.3785516 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 -0.3785516 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.6557207 0 0.6532182 -0.7571337 0 -0.6532182 -0.7571337 0.3785516 0.6532182 -0.6557207 0.3785516 -0.6532182 -0.6557207 0.6557207 0.6532182 -0.3785516 0.6557207 -0.6532182 -0.3785516 0.6532182 0.7571337 0 -0.6532182 0.7571337 0 -0.6532182 0.6557207 0.3785516 0.6532182 0.6557207 0.3785516 -0.6532182 0.3785516 0.6557207 0.6532182 0.3785516 0.6557207 -0.6532182 0 0.7571337 0.6532182 0 0.7571337 -0.6532182 -0.3785821 0.6556902 0.6532182 -0.3785821 0.6557207 -0.6532182 -0.6557207 0.3785516 0.6532182 -0.6557207 0.3785516 -0.6532182 -0.7571337 0 0.6532182 -0.7571337 0 -0.6532182 -0.6557207 -0.3785516 0.6532182 -0.6557207 -0.3785516 -0.6532182 -0.3785516 -0.6557207 0.6532182 -0.3785516 -0.6557207 -0.6532182 0 -0.7571337 0.6532182 0 -0.7571337 -0.6532182 0.3785516 -0.6557207 0.6532182 0.3785516 -0.6557207 -0.6532182 0.6557207 -0.3785516 0.6532182 0.6557207 -0.3785516\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 \n

60 36 36 36 37 36 61 37 49 37 48 37 60 38 37 38 38 38 61 39 50 39 49 39 60 40 38 40 39 40 61 41 51 41 50 41 60 42 39 42 40 42 61 43 52 43 51 43 60 44 40 44 41 44 61 45 53 45 52 45 60 46 41 46 42 46 61 47 54 47 53 47 60 48 42 48 43 48 61 49 55 49 54 49 60 50 43 50 44 50 61 51 56 51 55 51 60 52 44 52 45 52 61 53 57 53 56 53 60 54 45 54 46 54 61 55 58 55 57 55 60 56 46 56 47 56 61 57 59 57 58 57 47 58 36 58 60 58 61 59 48 59 59 59 48 432 36 433 47 434 48 432 47 434 59 435 46 436 58 437 59 435 46 436 59 435 47 434 45 438 57 439 58 437 45 438 58 437 46 436 44 440 56 441 57 439 44 440 57 439 45 438 43 442 55 443 56 441 43 442 56 441 44 440 42 444 54 445 55 443 42 444 55 443 43 442 41 446 53 447 54 445 41 446 54 445 42 444 40 448 52 449 53 447 40 448 53 447 41 446 39 450 51 451 52 449 39 450 52 449 40 448 38 452 50 453 51 451 38 452 51 451 39 450 37 454 49 455 50 453 37 454 50 453 38 452 36 433 48 432 49 455 36 433 49 455 37 454

\n \n \n \n \n 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 \n

151 132 135 132 134 132 137 133 135 133 151 133 136 136 138 136 134 136 137 137 138 137 136 137 139 140 140 140 134 140 137 141 140 141 139 141 141 144 142 144 134 144 137 145 142 145 141 145 143 148 144 148 134 148 137 149 144 149 143 149 145 152 146 152 134 152 137 153 146 153 145 153 147 156 148 156 134 156 137 157 148 157 147 157 149 160 150 160 134 160 137 161 150 161 149 161 181 190 178 190 182 190 182 191 178 191 179 191 181 194 183 194 184 194 184 195 183 195 179 195 181 198 185 198 186 198 186 199 185 199 179 199 181 202 187 202 188 202 188 203 187 203 179 203 181 206 189 206 190 206 190 207 189 207 179 207 181 210 191 210 192 210 192 211 191 211 179 211 181 214 193 214 194 214 194 215 193 215 179 215 181 218 195 218 180 218 180 219 195 219 179 219 196 220 197 220 198 220 199 221 197 221 196 221 200 224 201 224 198 224 199 225 201 225 200 225 202 228 203 228 198 228 199 229 203 229 202 229 204 232 205 232 198 232 199 233 205 233 204 233 206 236 207 236 198 236 199 237 207 237 206 237 208 240 209 240 198 240 199 241 209 241 208 241 210 244 211 244 198 244 199 245 211 245 210 245 212 248 213 248 198 248 199 249 213 249 212 249 269 302 266 302 270 302 270 303 266 303 267 303 269 306 271 306 272 306 272 307 271 307 267 307 269 310 273 310 274 310 274 311 273 311 267 311 269 314 275 314 276 314 276 315 275 315 267 315 269 318 277 318 278 318 278 319 277 319 267 319 269 322 279 322 280 322 280 323 279 323 267 323 269 326 281 326 282 326 282 327 281 327 267 327 269 330 283 330 268 330 268 331 283 331 267 331

\n \n \n \n \n 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 \n

16 0 0 1 3 2 2 3 1 4 17 5 16 0 3 2 5 6 17 5 4 7 2 3 16 0 5 6 7 8 17 5 6 9 4 7 16 0 7 8 9 10 17 5 8 11 6 9 16 0 9 10 11 12 17 5 10 13 8 11 16 0 11 12 13 14 17 5 12 15 10 13 16 0 13 14 15 16 17 5 14 17 12 15 15 16 0 1 16 0 17 5 1 4 14 17 18 18 20 19 19 20 21 21 23 22 22 23 18 18 19 20 24 24 21 21 25 25 23 22 18 18 24 24 26 26 21 21 27 27 25 25 18 18 26 26 28 28 21 21 29 29 27 27 18 18 28 28 30 30 21 21 31 31 29 29 18 18 30 30 32 32 21 21 33 33 31 31 18 18 32 32 34 34 21 21 35 35 33 33 34 34 20 19 18 18 35 35 21 21 22 23 78 60 62 61 63 62 79 63 71 64 70 65 78 60 63 62 64 66 79 63 72 67 71 64 78 60 64 66 65 68 79 63 73 69 72 67 78 60 65 68 66 70 79 63 74 71 73 69 78 60 66 70 67 72 79 63 75 73 74 71 78 60 67 72 68 74 79 63 76 75 75 73 78 60 68 74 69 76 79 63 77 77 76 75 69 76 62 61 78 60 79 63 70 65 77 77 96 78 80 79 83 80 82 81 81 82 97 83 96 78 83 80 85 84 97 83 84 85 82 81 96 78 85 84 87 86 97 83 86 87 84 85 96 78 87 86 89 88 97 83 88 89 86 87 96 78 89 88 91 90 97 83 90 91 88 89 96 78 91 90 93 92 97 83 92 93 90 91 96 78 93 92 95 94 97 83 94 95 92 93 96 78 95 94 80 79 97 83 81 82 94 95 98 96 100 97 99 98 101 99 103 100 102 101 98 96 99 98 104 102 101 99 105 103 103 100 98 96 104 102 106 104 101 99 107 105 105 103 98 96 106 104 108 106 101 99 109 107 107 105 98 96 108 106 110 108 101 99 111 109 109 107 98 96 110 108 112 110 101 99 113 111 111 109 98 96 112 110 114 112 101 99 115 113 113 111 114 112 100 97 98 96 101 99 102 101 115 113 132 114 116 115 119 116 118 117 117 118 133 119 132 114 119 116 121 120 133 119 120 121 118 117 132 114 121 120 123 122 133 119 122 123 120 121 132 114 123 122 125 124 133 119 124 125 122 123 132 114 125 124 127 126 133 119 126 127 124 125 132 114 127 126 129 128 133 119 128 129 126 127 132 114 129 128 131 130 133 119 130 131 128 129 132 114 131 130 116 115 133 119 117 118 130 131 152 164 153 164 154 164 155 165 156 165 157 165 152 166 154 166 158 166 155 167 159 167 156 167 152 168 158 168 160 168 155 169 161 169 159 169 152 170 160 170 162 170 155 171 163 171 161 171 152 172 162 172 164 172 155 173 165 173 163 173 152 174 164 174 166 174 155 175 167 175 165 175 152 176 166 176 168 176 155 177 169 177 167 177 152 178 168 178 170 178 155 179 171 179 169 179 152 180 170 180 172 180 155 181 173 181 171 181 152 182 172 182 174 182 155 183 175 183 173 183 152 184 174 184 176 184 155 185 177 185 175 185 152 186 176 186 153 186 155 187 157 187 177 187 238 252 216 252 217 252 239 253 214 253 215 253 238 254 217 254 219 254 239 255 218 255 214 255 238 256 219 256 221 256 239 257 220 257 218 257 238 258 221 258 223 258 239 259 222 259 220 259 238 260 223 260 225 260 239 261 224 261 222 261 238 262 225 262 227 262 239 263 226 263 224 263 238 264 227 264 229 264 239 265 228 265 226 265 238 266 229 266 231 266 239 267 230 267 228 267 238 268 231 268 233 268 239 269 232 269 230 269 238 270 233 270 235 270 239 271 234 271 232 271 238 272 235 272 237 272 239 273 236 273 234 273 238 274 237 274 216 274 239 275 215 275 236 275 240 276 241 276 242 276 243 277 244 277 245 277 240 278 242 278 246 278 243 279 247 279 244 279 240 280 246 280 248 280 243 281 249 281 247 281 240 282 248 282 250 282 243 283 251 283 249 283 240 284 250 284 252 284 243 285 253 285 251 285 240 286 252 286 254 286 243 287 255 287 253 287 240 288 254 288 256 288 243 289 257 289 255 289 240 290 256 290 258 290 243 291 259 291 257 291 240 292 258 292 260 292 243 293 261 293 259 293 240 294 260 294 262 294 243 295 263 295 261 295 240 296 262 296 264 296 243 297 265 297 263 297 240 298 264 298 241 298 243 299 245 299 265 299 299 332 295 332 297 332 295 333 293 333 297 333 290 334 299 334 297 334 290 335 297 335 286 335 298 336 289 336 285 336 298 337 285 337 296 337 294 338 298 338 296 338 294 339 296 339 292 339 299 340 290 340 298 340 290 341 289 341 298 341 295 342 299 342 298 342 295 343 298 343 294 343 296 344 285 344 286 344 296 345 286 345 297 345 292 346 296 346 297 346 292 347 297 347 293 347 295 348 291 348 293 348 291 349 287 349 293 349 292 350 284 350 288 350 292 351 288 351 294 351 291 352 295 352 288 352 295 353 294 353 288 353 284 354 292 354 293 354 284 355 293 355 287 355 288 356 284 356 291 356 284 357 287 357 291 357 285 358 289 358 290 358 285 359 290 359 286 359 264 360 245 361 241 362 264 360 265 363 245 361 262 364 265 363 264 360 262 364 263 365 265 363 260 366 263 365 262 364 260 366 261 367 263 365 258 368 261 367 260 366 258 368 259 369 261 367 256 370 259 369 258 368 256 370 257 371 259 369 254 372 257 371 256 370 254 372 255 373 257 371 252 374 255 373 254 372 252 374 253 375 255 373 250 376 253 375 252 374 250 376 251 377 253 375 248 378 251 377 250 376 248 378 249 379 251 377 246 380 249 379 248 378 246 380 247 381 249 379 242 382 247 381 246 380 242 382 244 383 247 381 241 362 244 383 242 382 241 362 245 361 244 383 215 384 216 385 237 386 215 384 237 386 236 387 236 387 237 386 235 388 236 387 235 388 234 389 234 389 235 388 233 390 234 389 233 390 232 391 232 391 233 390 231 392 232 391 231 392 230 393 230 393 231 392 229 394 230 393 229 394 228 395 228 395 229 394 227 396 228 395 227 396 226 397 226 397 227 396 225 398 226 397 225 398 224 399 224 399 225 398 223 400 224 399 223 400 222 401 222 401 223 400 221 402 222 401 221 402 220 403 220 403 221 402 219 404 220 403 219 404 218 405 218 405 219 404 217 406 218 405 217 406 214 407 214 407 217 406 216 385 214 407 216 385 215 384 176 408 157 409 153 410 176 408 177 411 157 409 174 412 177 411 176 408 174 412 175 413 177 411 172 414 175 413 174 412 172 414 173 415 175 413 170 416 173 415 172 414 170 416 171 417 173 415 168 418 171 417 170 416 168 418 169 419 171 417 166 420 169 419 168 418 166 420 167 421 169 419 164 422 167 421 166 420 164 422 165 423 167 421 162 424 165 423 164 422 162 424 163 425 165 423 160 426 163 425 162 424 160 426 161 427 163 425 158 428 161 427 160 426 158 428 159 429 161 427 154 430 159 429 158 428 154 430 156 431 159 429 153 410 156 431 154 430 153 410 157 409 156 431 117 118 131 130 130 131 117 118 116 115 131 130 130 131 131 130 128 129 128 129 131 130 129 128 128 129 129 128 126 127 126 127 129 128 127 126 126 127 127 126 124 125 124 125 127 126 125 124 124 125 125 124 122 123 122 123 125 124 123 122 122 123 121 120 120 121 122 123 123 122 121 120 120 121 119 116 118 117 120 121 121 120 119 116 116 115 118 117 119 116 116 115 117 118 118 117 102 101 100 97 114 112 102 101 114 112 115 113 112 110 113 111 115 113 112 110 115 113 114 112 110 108 111 109 113 111 110 108 113 111 112 110 108 106 109 107 110 108 109 107 111 109 110 108 106 104 107 105 108 106 107 105 109 107 108 106 104 102 105 103 106 104 105 103 107 105 106 104 99 98 103 100 104 102 103 100 105 103 104 102 100 97 102 101 103 100 100 97 103 100 99 98 81 82 95 94 94 95 81 82 80 79 95 94 94 95 95 94 92 93 92 93 95 94 93 92 92 93 93 92 90 91 90 91 93 92 91 90 90 91 91 90 88 89 88 89 91 90 89 88 88 89 89 88 86 87 86 87 89 88 87 86 86 87 85 84 84 85 86 87 87 86 85 84 84 85 83 80 82 81 84 85 85 84 83 80 80 79 82 81 83 80 80 79 81 82 82 81 70 65 62 61 69 76 70 65 69 76 77 77 68 74 76 75 77 77 68 74 77 77 69 76 67 72 75 73 76 75 67 72 76 75 68 74 66 70 74 71 67 72 74 71 75 73 67 72 65 68 73 69 66 70 73 69 74 71 66 70 64 66 72 67 65 68 72 67 73 69 65 68 63 62 71 64 64 66 71 64 72 67 64 66 62 61 70 65 71 64 62 61 71 64 63 62 22 23 20 19 34 34 22 23 34 34 35 35 32 32 33 33 35 35 32 32 35 35 34 34 30 30 31 31 33 33 30 30 33 33 32 32 28 28 29 29 31 31 28 28 31 31 30 30 26 26 27 27 29 29 26 26 29 29 28 28 24 24 25 25 27 27 24 24 27 27 26 26 19 20 23 22 25 25 19 20 25 25 24 24 23 22 19 20 20 19 23 22 20 19 22 23 1 4 0 1 15 16 15 16 14 17 1 4 14 17 13 14 12 15 14 17 15 16 13 14 12 15 11 12 10 13 12 15 13 14 11 12 10 13 9 10 8 11 10 13 11 12 9 10 8 11 7 8 6 9 8 11 9 10 7 8 6 9 5 6 4 7 6 9 7 8 5 6 4 7 3 2 2 3 4 7 5 6 3 2 0 1 2 3 3 2 0 1 1 4 2 3

\n \n \n \n \n 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 \n

137 134 136 134 135 134 135 135 136 135 134 135 137 138 139 138 138 138 138 139 139 139 134 139 137 142 141 142 140 142 140 143 141 143 134 143 137 146 143 146 142 146 142 147 143 147 134 147 137 150 145 150 144 150 144 151 145 151 134 151 137 154 147 154 146 154 146 155 147 155 134 155 137 158 149 158 148 158 148 159 149 159 134 159 137 162 151 162 150 162 150 163 151 163 134 163 178 188 180 188 179 188 181 189 180 189 178 189 183 192 182 192 179 192 181 193 182 193 183 193 185 196 184 196 179 196 181 197 184 197 185 197 187 200 186 200 179 200 181 201 186 201 187 201 189 204 188 204 179 204 181 205 188 205 189 205 191 208 190 208 179 208 181 209 190 209 191 209 193 212 192 212 179 212 181 213 192 213 193 213 195 216 194 216 179 216 181 217 194 217 195 217 199 222 200 222 197 222 197 223 200 223 198 223 199 226 202 226 201 226 201 227 202 227 198 227 199 230 204 230 203 230 203 231 204 231 198 231 199 234 206 234 205 234 205 235 206 235 198 235 199 238 208 238 207 238 207 239 208 239 198 239 199 242 210 242 209 242 209 243 210 243 198 243 199 246 212 246 211 246 211 247 212 247 198 247 199 250 196 250 213 250 213 251 196 251 198 251 266 300 268 300 267 300 269 301 268 301 266 301 271 304 270 304 267 304 269 305 270 305 271 305 273 308 272 308 267 308 269 309 272 309 273 309 275 312 274 312 267 312 269 313 274 313 275 313 277 316 276 316 267 316 269 317 276 317 277 317 279 320 278 320 267 320 269 321 278 321 279 321 281 324 280 324 267 324 269 325 280 325 281 325 283 328 282 328 267 328 269 329 282 329 283 329

\n \n \n 1\n \n \n \n \n \n 0 0 0\n 0 0 1 0\n 0 1 0 0\n 1 0 0 0\n 0.1335572 0.1335572 0.1335572\n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n \n\n" }, { "path": "src/uav_gazebo/model.config", "content": "\n\n\n uav\n 1.0\n urdf/uav.urdf\n\n \n Kanish Gamagedara\n kanishkegb@gwu.edu\n \n\n \n The UAV model\n \n\n" }, { "path": "src/uav_gazebo/msg/DesiredData.msg", "content": "std_msgs/Header header\n\nfloat64[3] position\nfloat64[3] velocity\nfloat64[3] acceleration\nfloat64[3] jerk\nfloat64[3] snap\n\nfloat64[9] attitude\nfloat64[3] angular_velocity\nfloat64[3] angular_acceleration\nfloat64[3] angular_jerk\n\nfloat64[3] b1c\n\nfloat64[3] b1\nfloat64[3] b1_dot\nfloat64[3] b1_2dot\n\nfloat64[3] b3\nfloat64[3] b3_dot\nfloat64[3] b3_2dot\n\n# Decoupled-yaw controller\nfloat64 wc3\nfloat64 wc3_dot\n\nbool is_landed" }, { "path": "src/uav_gazebo/msg/ErrorData.msg", "content": "std_msgs/Header header\n\nfloat64[3] position\nfloat64[3] velocity\nfloat64[3] attitude\nfloat64[3] angular_velocity\nfloat64[3] position_integral\nfloat64[3] attitude_integral" }, { "path": "src/uav_gazebo/msg/ModeData.msg", "content": "std_msgs/Header header\n\nint32 mode\nfloat64[3] x_offset\nfloat64 yaw_offset" }, { "path": "src/uav_gazebo/msg/StateData.msg", "content": "std_msgs/Header header\n\nfloat64[3] position\nfloat64[3] velocity\nfloat64[3] acceleration\nfloat64[9] attitude\nfloat64[3] angular_velocity" }, { "path": "src/uav_gazebo/package.xml", "content": "\n\n\n\n uav_gazebo\n 0.0.0\n FDCL-UAV Gazebo Simulation\n\n Kanish\n\n MIT\n\n ament_cmake\n\n rosidl_default_generators\n rosidl_default_runtime\n rosidl_interface_packages\n\n xacro\n\n uav_control_plugin\n\n \n ament_cmake\n \n \n\n" }, { "path": "src/uav_gazebo/urdf/uav.urdf.xacro", "content": "\n\n\n \n\n \n 0 0 0.2 0 0 0\n \n \n \n \n\n \n \n \n \n \n \n\n \n \n \n \n \n \n \n\n \n Gazebo/Orange\n true\n \n \n 0 0 0 0 0 0\n true\n 100\n true\n\n \n \n ~/out:=uav/imu\n \n\n 0 0 0 0 0 0\n link\n 100.0\n 0.0001\n \n \n\n \n\n \n \n \n odom:=uav/gps\n \n\n link\n world\n 0.01\n 10\n \n \n\n \n \n link\n /uav/fm\n \n \n\n\n" }, { "path": "src/uav_gazebo/worlds/simple.world", "content": "\n\n \n \n \n \n /gazebo\n \n 1.0\n \n\n\n \n model://ground_plane\n \n\n \n model://sun\n \n\n \n\n" }, { "path": "src/uav_plugins/CMakeLists.txt", "content": "cmake_minimum_required(VERSION 3.22.1)\nproject(uav_control_plugin)\n\nset(CMAKE_CXX_STANDARD 14)\n\nfind_package(ament_cmake REQUIRED)\nfind_package(rclcpp REQUIRED)\nfind_package(std_msgs REQUIRED)\nfind_package(geometry_msgs REQUIRED)\nfind_package(gazebo REQUIRED)\nfind_package(gazebo_ros REQUIRED)\n\nadd_library(uav_control_plugin SHARED \n src/uav_control_plugin.cpp\n src/fdcl_matrix_utils.cpp\n src/fdcl_ros_utils.cpp\n)\n\ntarget_include_directories(uav_control_plugin PUBLIC\n # ./include\n ../../libraries/eigen\n ${GAZEBO_INCLUDE_DIRS}\n $\n $\n)\n\nament_target_dependencies(uav_control_plugin\n rclcpp\n std_msgs\n geometry_msgs\n)\n\ninstall(TARGETS\n uav_control_plugin\n DESTINATION share/${PROJECT_NAME}\n)\n\nament_package()" }, { "path": "src/uav_plugins/include/fdcl/common_types.hpp", "content": "#ifndef FDCL_COMMON_TYPES\n#define FDCL_COMMON_TYPES\n\n#include \"Eigen/Dense\"\n\nnamespace fdcl\n{\n\ntypedef Eigen::Matrix Vector2;\ntypedef Eigen::Matrix Vector3;\ntypedef Eigen::Matrix Vector4;\ntypedef Eigen::Matrix Vector6;\ntypedef Eigen::Matrix Matrix3;\n\n} // End of namespace fdcl\n\n#endif\n" }, { "path": "src/uav_plugins/include/fdcl/matrix_utils.hpp", "content": "/** \\file matrix_utils.hpp\n* \\brief Miscellaneous math functions\n*\n* Miscellaneous math functions used in FDCL are defined here\n*/\n\n#ifndef FDCL_MATRIX_UTILS_HPP\n#define FDCL_MATRIX_UTILS_HPP\n\n#include \n#include \n#include \n\n#include \"fdcl/common_types.hpp\"\n\nnamespace fdcl\n{\n\n/** \\fn Matrix3 hat(const Vector3 v)\n* Returns the hat map of a given 3x1 vector. This is the inverse of vee map.\n* @param v vector which the hat map is needed to be operated on\n* @return hat map of the input vector\n*/\nMatrix3 hat(const Vector3 v);\n\n\n/** \\fn Vector3 vee(const Matrix3 V)\n* Returns the vee map of a given 3x3 matrix. This is the inverse of hat map.\n* @param V matrix which the vee map is needed to be operated on\n* @return vee map of the input matrix\n*/\nVector3 vee(const Matrix3 V);\n\n\n/** \\fn double sinx_over_x(const double x)\n * Calculates and returns the value of sin(x)/x, dealing with the case where\n * x = 0.\n * @param x input value\n * @return the value of sin(x)/x\n */\ndouble sinx_over_x(const double x);\n\n\n/** \\fn Matrix3 expm_SO3(const Vector3 r)\n * Calculates and returns the rotation matrix in SO(3) from rotation vector.\n * This is the inverse of logm_som3.\n * @param r rotation vector (angle * axis)\n * @return rotation matrix in SO(3) which corresponds to the input vector\n */\nMatrix3 expm_SO3(const Vector3 r);\n\n\n/** \\fn Vector3 logm_SO3(const Matrix3 R)\n * Calculates and returns the rotation vector from rotation matrix in SO(3).\n * This is the inverse of expm_SO3.\n * @param R rotation matrix in SO(3)\n * @return rotation vector (angle * axis)\n */\nVector3 logm_SO3(const Matrix3 R);\n\n\n/** \\fn bool assert_SO3(Matrix3 R,const char *R_name)\n * Check and returns if a given matrix is in SO(3)\n * @param R rotation matrix\n * @param R_name name of the rotation matrix\n * @return true if the input matrix is true, false otherwise\n */\nbool assert_SO3(Matrix3 R,const char *R_name);\n\n/** \\fn void saturate(Vector3 &x, const double x_min, const double x_max)\n * Saturate the elements of a given 3x1 vector between a minimum and a maximum\n * velue.\n * @param x vector which the elements needed to be saturated\n * @param x_min minimum value for each element\n * @param x_max maximum value for each element\n */\nvoid saturate(Vector3 &x, const double x_min, const double x_max);\n\n\n/** \\fn void saturate(Vector4 &x, const double x_min, const double x_max)\n * Saturate the elements of a given 4x1 vector between a minimum and a maximum\n * velue.\n * @param x vector which the elements needed to be saturated\n * @param x_min minimum value for each element\n * @param x_max maximum value for each element\n */\nvoid saturate(Vector4 &x, const double x_min, const double x_max);\n\n\n/** \\fn void saturate(Vector6 &x, const double x_min, const double x_max)\n * Saturate the elements of a given 6x1 vector between a minimum and a maximum\n * velue.\n * @param x vector which the elements needed to be saturated\n * @param x_min minimum value for each element\n * @param x_max maximum value for each element\n */\nvoid saturate(Vector6 &x, const double x_min, const double x_max);\n\n\n/** \\fn void saturate(int &x, const int x_min, const int x_max)\n * Saturate the elements of a given integer between a minimum and a maximum\n * velue.\n * @param x integer which needed to be saturated\n * @param x_min minimum value for x\n * @param x_max maximum value for x\n */\nvoid saturate(int &x, const int x_min, const int x_max);\n\n\n/** \\fn void saturate(double &x, const double x_min, const double x_max)\n * Saturate the elements of a given integer between a minimum and a maximum\n * velue.\n * @param x double which needed to be saturated\n * @param x_min minimum value for x\n * @param x_max maximum value for x\n */\nvoid saturate(double &x, const double x_min, const double x_max);\n\n\n/** \\fn void deriv_unit_vector(Vector3 B, Vector3 B_dot, Vector3 &q, \n * Vector3 &q_dot)\n * finds derivative q = -B/norm(B)\n */\nvoid deriv_unit_vector(Vector3 B, Vector3 B_dot, Vector3 &q, Vector3 &q_dot);\n\n\n/** \\fn void deriv_unit_vector(Vector3 B, Vector3 B_dot, Vector3 B_ddot, \n * Vector3 &q, Vector3 &q_dot, Vector3 &q_ddot)\n * finds first and second derivatives of q = -B/norm(B)\n */\nvoid deriv_unit_vector(Vector3 B, Vector3 B_dot, Vector3 B_ddot, Vector3 &q, \\\n Vector3 &q_dot, Vector3 &q_ddot);\n\n} // end of namespace fdcl\n#endif\n" }, { "path": "src/uav_plugins/include/fdcl/ros_utils.hpp", "content": "#ifndef FDCL_ROS_UTILS_HPP\n#define FDCL_ROS_UTILS_HPP\n\n\n#include \"fdcl/common_types.hpp\"\n\n\n#include \"Eigen/Dense\"\n#include \n#include \n\nnamespace fdcl\n{\n\nvoid eigen_to_ignition(\n const Vector3 input, ignition::math::Vector3d &output\n);\n\nvoid ignition_to_eigen(\n const ignition::math::Vector3d input, Vector3 &output\n);\n\nvoid quaternion_to_R(const ignition::math::Quaterniond q, Matrix3 &R);\n\n} // end of namespace fdcl\n\n#endif\n" }, { "path": "src/uav_plugins/package.xml", "content": "\n\n\n\n uav_control_plugin\n 0.0.0\n The control plugin package\n\n \n \n \n Kanish\n\n\n \n \n \n MIT\n\n\n ament_cmake\n \n\n launch\n ros2run\n\n \n\n geometry_msgs\n gazebo_ros\n rclcpp\n\n \n ament_cmake\n \n \n \n\n" }, { "path": "src/uav_plugins/src/fdcl_matrix_utils.cpp", "content": "#include \"fdcl/matrix_utils.hpp\"\n\n\nfdcl::Matrix3 fdcl::hat(const Vector3 v)\n{\n Matrix3 V;\n V.setZero();\n\n V(2,1) = v(0);\n V(1,2) = -V(2, 1);\n V(0,2) = v(1);\n V(2,0) = -V(0, 2);\n V(1,0) = v(2);\n V(0,1) = -V(1, 0);\n\n return V;\n}\n\n\nfdcl::Vector3 fdcl::vee(const Matrix3 V)\n{\n Vector3 v;\n Matrix3 E;\n\n v.setZero();\n E = V + V.transpose();\n if(E.norm() > 1.e-6)\n {\n std::cout << \"vee: E.norm() = \" << E.norm() << std::endl;\n }\n\n v(0) = V(2, 1);\n v(1) = V(0, 2);\n v(2) = V(1, 0);\n\n return v;\n}\n\n\ndouble fdcl::sinx_over_x(const double x)\n{\n double y;\n double eps = 1.e-6;\n\n if(abs(x) < eps)\n {\n y = - pow(x, 10) / 39916800. + pow(x, 8) / 362880.\n - pow(x, 6) / 5040. + pow(x, 4) / 120. - pow(x, 2) / 6. + 1.;\n }\n else\n {\n y = sin(x) / x;\n }\n\n return y;\n}\n\n\nfdcl::Matrix3 fdcl::expm_SO3(const Vector3 r)\n{\n Matrix3 R;\n double theta, y, y2;\n\n theta = r.norm();\n y = sinx_over_x(theta);\n y2 = sinx_over_x(theta / 2.);\n\n R.setIdentity();\n R += y * hat(r) + 1. / 2. * pow(y2, 2) * hat(r) * hat(r);\n\n return R;\n}\n\n\nfdcl::Vector3 fdcl::logm_SO3(const Matrix3 R)\n{\n Vector3 r;\n Matrix3 I;\n double eps = 1.e-6;\n\n r.setZero();\n I.setIdentity();\n\n if((I - R * R.transpose()).norm() > eps || abs(R.determinant() - 1) > eps)\n {\n std::cerr << \"logm_SO3: R is not a rotation matrix\" << std::endl;\n }\n else\n {\n Eigen::EigenSolver eig(R);\n double min_del_lam_1 = 1.0, cos_theta, theta;\n int i, i_min = -1;\n Vector3 v;\n Matrix3 R_new;\n\n for(i = 0; i < 3; i++)\n {\n if(eig.eigenvectors().col(i).imag().norm() < eps)\n {\n if(pow(eig.eigenvalues()[i].real(), 2) - 1. < min_del_lam_1 )\n {\n min_del_lam_1 = pow(eig.eigenvalues()[i].real(), 2) - 1.0;\n i_min = i;\n }\n }\n }\n v = eig.eigenvectors().col(i_min).real();\n\n cos_theta = (R.trace() - 1.0) / 2.0;\n\n if(cos_theta > 1.0) cos_theta = 1.0;\n else if(cos_theta < -1.0) cos_theta = - 1.0;\n\n theta = acos(cos_theta);\n R_new = expm_SO3(theta * v);\n\n if((R - R_new).norm() > (R - R_new.transpose()).norm()) v =- v;\n\n r = theta * v;\n\n return r;\n }\n\n return r;\n}\n\n\nbool fdcl::assert_SO3(Matrix3 R, const char *R_name)\n{\n bool isSO3;\n double errO, errD;\n Matrix3 eye3;\n double eps = 1e-5;\n\n eye3.setIdentity();\n errO = (R.transpose()*R - eye3).norm();\n errD = pow(1 - R.determinant(), 2);\n\n if (errO > eps || errD > eps)\n {\n isSO3 = false;\n std::cerr << \"Assert SO3: \" << R_name << \" ||I-R^TR|| = \"\n << errO << \", det(R) = \" << R.determinant()\n << std::endl;\n\n std::cout << R << std::endl << std::endl;\n }\n else\n {\n isSO3 = true;\n }\n\n return isSO3;\n}\n\n\n/* TODO: Write these as a template function, so they can accept diffrent types\n * of data.\n */\nvoid fdcl::saturate(Vector3 &x, const double x_min, const double x_max)\n{\n for (int i = 0; i < 3; i++)\n {\n if (x(i) > x_max) x(i) = x_max;\n else if (x(i) < x_min) x(i) = x_min;\n }\n}\n\n\nvoid fdcl::saturate(Vector4 &x, const double x_min, const double x_max)\n{\n for (int i = 0; i < 4; i++)\n {\n if (x(i) > x_max) x(i) = x_max;\n else if (x(i) < x_min) x(i) = x_min;\n }\n}\n\n\nvoid fdcl::saturate(Vector6 &x, const double x_min, const double x_max)\n{\n for (int i = 0; i < 6; i++)\n {\n if (x(i) > x_max) x(i) = x_max;\n else if (x(i) < x_min) x(i) = x_min;\n }\n}\n\n\nvoid fdcl::saturate(int &x, const int x_min, const int x_max)\n{\n if (x > x_max) \n {\n x = x_max;\n }\n else if (x < x_min)\n {\n x = x_min;\n }\n}\n\n\nvoid fdcl::saturate(double &x, const double x_min, const double x_max)\n{\n if (x > x_max)\n {\n x = x_max;\n }\n else if (x < x_min)\n {\n x = x_min;\n }\n}\n\n\nvoid fdcl::deriv_unit_vector(Vector3 B, Vector3 B_dot, \\\n Vector3 &q, Vector3 &q_dot)\n{\n double nB = B.norm();\n q = -B/nB;\n q_dot = hat(q)*hat(q)*B_dot/nB;\n}\n\n\nvoid fdcl::deriv_unit_vector(Vector3 B, Vector3 B_dot, Vector3 B_ddot, \\\n Vector3 &q, Vector3 &q_dot, Vector3 &q_ddot)\n{\n double nB = B.norm();\n q = -B/nB;\n q_dot = hat(q)*hat(q)*B_dot/nB;\n q_ddot=((2*q_dot*q.transpose()+q*q_dot.transpose())*B_dot \\\n +hat(q)*hat(q)*B_ddot)/nB;\n}\n" }, { "path": "src/uav_plugins/src/fdcl_ros_utils.cpp", "content": "#include \"fdcl/ros_utils.hpp\"\n#include \"fdcl/matrix_utils.hpp\"\n\n\nvoid fdcl::eigen_to_ignition(\n const Vector3 input, ignition::math::Vector3d &output\n)\n{\n output[0] = input(0);\n output[1] = input(1);\n output[2] = input(2);\n}\n\n\nvoid fdcl::ignition_to_eigen(\n const ignition::math::Vector3d input, Vector3 &output\n)\n{\n output(1) = input[0];\n output(2) = input[1];\n output(3) = input[2];\n}\n\n\nvoid fdcl::quaternion_to_R(const ignition::math::Quaterniond q, Matrix3 &R)\n{\n fdcl::Matrix3 eye3 = fdcl::Matrix3::Identity();\n fdcl::Vector3 q13(q.X(), q.Y(), q.Z());\n double q4 = q.W();\n\n fdcl::Matrix3 hat_q = fdcl::hat(q13);\n R = eye3 + 2 * q4 * hat_q + 2 * hat_q * hat_q;\n}" }, { "path": "src/uav_plugins/src/uav_control_plugin.cpp", "content": "// #include \n// #include \n// #include \n// #include \n\n// #include \n// #include \n// // #include \n// // #include \n\n#include \n#include \n\n#include \n#include \n\n// #include \n// #include \"std_msgs/String.h\"\n\n#include \n#include \n#include \n\n#include \n\n\n#include \"fdcl/common_types.hpp\"\n#include \"fdcl/ros_utils.hpp\"\n#include \"fdcl/matrix_utils.hpp\"\n\n\nnamespace igm = ignition::math;\n\n\nnamespace gazebo\n{\n\nclass UavControlPlugin : public ModelPlugin\n{\n\npublic: \n void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)\n {\n std::cout << \"Loading UAV Control Plugin\\n\";\n\n rn_ = gazebo_ros::Node::Get(sdf);\n\n world_ = model->GetWorld();\n model_ = model;\n\n link_name_ = sdf->GetElement(\"body_name\")->Get();\n link_ = model->GetLink(link_name_);\n\n // Listen to the update event. This event is broadcast every\n // simulation iteration.\n update_connection_ = event::Events::ConnectWorldUpdateBegin( \\\n std::bind(&UavControlPlugin::update, this));\n\n // Start the ROS subscriber.\n topic_name_ = sdf->GetElement(\"topic_name\")->Get();\n\n sub_fm_ = rn_->create_subscription( \\\n topic_name_, 1, std::bind(&UavControlPlugin::update_fm, this, \\\n std::placeholders::_1));\n }\n\n\n void update(void)\n {\n no_msg_counter++;\n if (no_msg_counter > 100)\n {\n reset_uav();\n\n if (!print_reset_message)\n {\n std::cout << \"UAV Plugin: No new force messages\"\n << \"\\tresetting UAV\\n\";\n print_reset_message = true;\n }\n return;\n }\n\n // Both must be in world frame.\n calculate_force();\n link_->SetForce(force);\n link_->SetTorque(M_out);\n }\n\n\n void calculate_force(void)\n {\n update_uav_rotation();\n\n fdcl::Vector3 force_body;\n ignition_to_eigen(this->f, force_body);\n\n fdcl::Vector3 force_world = this->R * force_body;\n eigen_to_ignition(force_world, this->force);\n\n fdcl::Vector3 M_body;\n ignition_to_eigen(this->M, M_body);\n\n fdcl::Vector3 M_world = R * M_body;\n eigen_to_ignition(M_world, this->M_out);\n }\n\n\n void update_uav_rotation(void)\n {\n ignition::math::Pose3d pose = link_->WorldPose();\n ignition::math::Quaterniond q = pose.Rot();\n\n fdcl::Vector3 q13(q.X(), q.Y(), q.Z());\n double q4 = q.W();\n\n fdcl::Matrix3 hat_q = fdcl::hat(q13);\n R = eye3 + 2 * q4 * hat_q + 2 * hat_q * hat_q;\n }\n\n\n void update_fm(const geometry_msgs::msg::WrenchStamped &msg) const\n {\n f[0] = msg.wrench.force.x;\n f[1] = msg.wrench.force.y;\n f[2] = msg.wrench.force.z;\n\n M[0] = msg.wrench.torque.x;\n M[1] = msg.wrench.torque.y;\n M[2] = msg.wrench.torque.z;\n\n no_msg_counter = 0;\n print_reset_message = false;\n }\n\n\n void reset_uav(void)\n {\n link_->SetForce(zero_fM);\n link_->SetTorque(zero_fM);\n }\n\n\n void ignition_to_eigen(\n const ignition::math::Vector3d input, fdcl::Vector3 &output\n )\n {\n output(0) = input[0];\n output(1) = input[1];\n output(2) = input[2];\n } \n\n\n void eigen_to_ignition(\n const fdcl::Vector3 input, ignition::math::Vector3d &output\n )\n {\n output[0] = input(0);\n output[1] = input(1);\n output[2] = input(2);\n }\n\n\nprivate:\n physics::ModelPtr model_;\n physics::WorldPtr world_;\n physics::LinkPtr link_;\n\n std::string link_name_;\n std::string topic_name_;\n \n event::ConnectionPtr update_connection_;\n\n rclcpp::Node::SharedPtr rn_;\n rclcpp::Subscription::SharedPtr sub_fm_;\n\n\n static igm::Vector3d f;\n static igm::Vector3d M;\n igm::Vector3d M_out;\n\n fdcl::Matrix3 R = fdcl::Matrix3::Identity();\n igm::Vector3d force = igm::Vector3d::Zero;\n\n static int no_msg_counter;\n static bool print_reset_message;\n \n const igm::Vector3d zero_fM = igm::Vector3d::Zero;\n const fdcl::Matrix3 eye3 = fdcl::Matrix3::Identity();\n};\n\n\n// Register this plugin with the simulator.\nGZ_REGISTER_MODEL_PLUGIN(UavControlPlugin)\n\n\n} // End of namespace gazebo.\n\nigm::Vector3d gazebo::UavControlPlugin::f = igm::Vector3d::Zero;\nigm::Vector3d gazebo::UavControlPlugin::M = igm::Vector3d::Zero;\nint gazebo::UavControlPlugin::no_msg_counter = 0;\nbool gazebo::UavControlPlugin::print_reset_message = false;\n\n" }, { "path": "tests/__init__.py", "content": "" }, { "path": "tests/test_matrix_utils.py", "content": "import imp\nimport numpy as np\nimport unittest\n\nfrom scripts.matrix_utils import *\n\n\nclass TestMatrixUtils(unittest.TestCase):\n\n def setUp(self):\n self.x = np.array([1.0, 2.0, 3.0])\n self.x_hat = np.array([\n [0.0, -3.0, 2.0],\n [3.0, 0.0, -1.0],\n [-2.0, 1.0, 0.0]\n ])\n\n theta = np.pi / 2.0\n self.R = np.array([\n [np.cos(theta), np.sin(theta), 0.0],\n [-np.sin(theta), np.cos(theta), 0.0],\n [0.0, 0.0, 1.0]\n ])\n\n self.q1 = np.array([0.7071068, 0.0, 0.0, 0.7071068])\n self.R_from_q1 = np.array([\n [1.0, 0.0, 0.0],\n [0.0, 0.0, -1.0],\n [0.0, 1.0, 0.0]\n ])\n\n self.q2 = np.array([0.033166, -0.1902412, 0.264125, 0.9449584])\n self.R_from_q2 = np.array([\n [0.7880926, -0.5117933, -0.3420202],\n [0.4865551, 0.8582760, -0.1631759],\n [0.3770600, -0.0378139, 0.9254166]\n ])\n\n self.A = np.array([1.0, 0.0, 0.0])\n self.A_dot = np.array([0.0, 0.0, 0.0])\n self.A_2dot = np.array([0.0, 0.0, 0.0])\n\n ## ensure_vector\n def test_ensure_vector(self):\n self.assertTrue(ensure_vector(self.x, 3))\n\n def test_ensure_vector_wrong_input_length(self):\n self.assertRaises(ValueError, ensure_vector, self.x, 2)\n\n def test_ensure_vector_matrix_input(self):\n self.assertRaises(ValueError, ensure_vector, self.x_hat, 3)\n\n def test_ensure_vector_3d_matrix_input(self):\n self.assertRaises(ValueError, ensure_vector, np.zeros((3, 3, 3)), 3)\n\n def test_ensure_vector_list_input(self):\n self.assertTrue(ensure_vector([0, 1, 2], 3))\n\n ## ensure_matrix\n def test_ensure_matrix(self):\n self.assertTrue(ensure_matrix(self.x_hat, 3, 3))\n\n def test_ensure_matrix_2d_input(self):\n self.assertRaises(ValueError, ensure_matrix, self.x, 3, 3)\n \n def test_ensure_matrix_3d_input(self):\n self.assertRaises(ValueError, ensure_matrix, np.zeros((3, 3, 3)), 3, 3)\n\n ## ensure_skew\n def test_ensure_skew(self):\n self.assertTrue(ensure_skew(self.x_hat, 3))\n\n def test_ensure_skew_nonskew_input(self):\n self.assertRaises(ValueError, ensure_skew, np.eye((3)), 3)\n\n def test_ensure_skew_3d_matrix(self):\n self.assertRaises(ValueError, ensure_skew, np.zeros((3, 3, 3)), 3)\n\n ## ensure_SO3\n def test_ensure_SO3(self):\n self.assertTrue(ensure_SO3(self.R))\n\n def test_ensure_SO3_eye(self):\n self.assertTrue(ensure_SO3(np.eye(3)))\n\n def test_ensure_SO3_wrong_R(self):\n R = self.R.copy()\n R[2, 2] = 1.001\n self.assertRaises(ValueError, ensure_SO3, R)\n\n def test_ensure_SO3_array_input(self):\n self.assertRaises(ValueError, ensure_SO3, [1.0, 2.0, -10.0, 5.5])\n\n def test_ensure_SO3_3d_matrix_input(self):\n self.assertRaises(ValueError, ensure_SO3, np.zeros((3, 3, 3)))\n\n ## hat\n def test_hat(self):\n np.testing.assert_array_almost_equal(hat(self.x), self.x_hat)\n\n def test_hat_short_input(self):\n self.assertRaises(ValueError, hat, np.array([1.0, 2.0]))\n\n def test_hat_long_input(self):\n self.assertRaises(ValueError, hat, [1.0, 2.0, -10.0, 5.5])\n\n ## vee\n def test_vee(self):\n np.testing.assert_array_almost_equal(vee(self.x_hat), self.x)\n\n def test_vee_non_skew(self):\n self.assertRaises(ValueError, vee, np.eye(3))\n\n ## q_to_R\n def test_q_to_R_q1(self):\n np.testing.assert_array_almost_equal(q_to_R(self.q1), self.R_from_q1, 6)\n\n def test_q_to_R_q2(self):\n np.testing.assert_array_almost_equal(q_to_R(self.q2), self.R_from_q2, 6)\n\n def test_q_to_R_short_array(self):\n self.assertRaises(ValueError, ensure_SO3, [0.0, 1.0, 0.0])\n\n def test_q_to_R_long_array(self):\n self.assertRaises(ValueError, ensure_SO3, [0.0, 1.0, 0.0, 0.0, 1.0])\n\n def test_q_to_R_wrong_q(self):\n R = q_to_R([1.0, 1.0, 1.0, 1.0])\n self.assertRaises(ValueError, ensure_SO3, R)\n\n ## deriv_unit_vector\n def test_deriv_unit_vector(self):\n q, q_dot, q_2dot = deriv_unit_vector(self.A, self.A_dot, self.A_2dot)\n A_zero = np.array([0.0, 0.0, 0.0])\n self.assertTrue(\n np.array_equal(q, self.A) and\n np.array_equal(q_dot, A_zero) and\n np.array_equal(q_2dot, A_zero)\n )\n\n def test_deriv_unit_vector_non_zero(self):\n A = np.array([1.0, 2.0, 3.0])\n A_dot = np.array([-1.2, -2.0, -4.5])\n A_2dot = np.array([0.1, 0.2, 1.7])\n q, q_dot, q_2dot = deriv_unit_vector(A, A_dot, A_2dot)\n\n A_q = np.array([0.26726124, 0.53452248, 0.80178373])\n A_q_dot = np.array([0.03627117, 0.17944683, -0.13172161])\n A_q_2dot = np.array([0.00312259, 0.29183291, -0.25903887])\n\n self.assertTrue(\n np.allclose(q, A_q) and\n np.allclose(q_dot, A_q_dot) and\n np.allclose(q_2dot, A_q_2dot)\n )\n\n def test_deriv_unit_vector_zero_norm_A(self):\n self.assertRaises(ZeroDivisionError, deriv_unit_vector, self.A_dot, \\\n self.A_dot, self.A_dot)\n\n def test_deriv_unit_vector_short_A(self):\n self.assertRaises(ValueError, deriv_unit_vector, [0.0, 1.0], \\\n self.A_dot, self.A_dot)\n\n def test_deriv_unit_vector_long_A(self):\n self.assertRaises(ValueError, deriv_unit_vector, self.q1, \\\n self.A_dot, self.A_dot)\n\n def test_deriv_unit_vector_short_A_dot(self):\n self.assertRaises(ValueError, deriv_unit_vector, self.A_dot, \\\n [0.0, 1.0], self.A_dot)\n\n def test_deriv_unit_vector_long_A_dot(self):\n self.assertRaises(ValueError, deriv_unit_vector, self.A, \\\n self.q1, self.A_dot)\n\n def test_deriv_unit_vector_short_A_2dot(self):\n self.assertRaises(ValueError, deriv_unit_vector, self.A_dot, \\\n self.A_dot, [0.0, 1.0])\n\n def test_deriv_unit_vector_long_A_2dot(self):\n self.assertRaises(ValueError, deriv_unit_vector, self.A, \\\n self.A_dot, self.q1)\n\n ## Saturate\n def test_saturate(self):\n x = np.array([2.0, -6.0, 25.0])\n self.assertTrue(np.allclose(saturate(x, -5.0, 5.0), [2.0, -5.0, 5.0]))\n\n ## expm_SO3\n def test_expm_SO3(self):\n self.assertTrue(\n np.allclose(\n expm_SO3([1.0, 0.0, 0.0]), np.array([\n [1.0, 0.0, 0.0],\n [0.0, 0.54030230586814, -0.841470984807897],\n [0.0, 0.841470984807897, 0.54030230586814]\n ])\n )\n )\n \n def test_expm_SO3_zero_inputs(self):\n self.assertTrue(np.allclose(expm_SO3([0.0, 0.0, 0.0]), np.eye(3)))\n \n def test_expm_SO3_nonzero_inputs(self):\n self.assertTrue(\n np.allclose(\n expm_SO3([2.3, -1.0, 3.3]), np.array([\n [-0.0641039645213566, 0.465517438128782, 0.882714108038758],\n [-0.87719769353654, -0.44804055054294, 0.172580043815485],\n [0.475830734806843, -0.763251714617921, 0.43707199858374]\n ])\n )\n )\n \n def test_expm_SO3_short_input_array(self):\n self.assertRaises(ValueError, expm_SO3, [0.0, 0.0])\n \n def test_expm_SO3_long_input_array(self):\n self.assertRaises(ValueError, expm_SO3, [0.0, 0.0, 0.0, 0.0])\n \n ## sinx_over_x\n def test_sinx_over_x(self):\n self.assertTrue(np.isclose(sinx_over_x(np.pi / 3.0), 0.826993343132688))\n \n def test_sinx_over_x_pi(self):\n self.assertTrue(np.isclose(sinx_over_x(np.pi), 0.0))\n \n def test_sinx_over_x_zero(self):\n self.assertTrue(np.isclose(sinx_over_x(0.0), 1.0))\n\n\nif __name__ == '__main__':\n unittest.main()\n" } ]