Repository: fdcl-gwu/uav_simulator Branch: main Commit: 7270fe4e274f Files: 47 Total size: 170.2 KB Directory structure: gitextract_72id4v6x/ ├── .catkin_workspace ├── .gitattributes ├── .gitignore ├── .gitmodules ├── .travis.yml ├── Dockerfile ├── License ├── README.md ├── docker_release.sh ├── docker_run.sh ├── scripts/ │ ├── data_logs/ │ │ └── .gitignore │ ├── plot_utils.py │ └── thread_log.py ├── src/ │ ├── fdcl_uav/ │ │ ├── fdcl_uav/ │ │ │ ├── __init__.py │ │ │ ├── control.py │ │ │ ├── estimator.py │ │ │ ├── freq_utils.py │ │ │ ├── gui.py │ │ │ ├── integral_utils.py │ │ │ ├── matrix_utils.py │ │ │ └── trajectory.py │ │ ├── launch/ │ │ │ └── fdcl_uav_launch.py │ │ ├── package.xml │ │ ├── resource/ │ │ │ └── fdcl_uav │ │ ├── setup.cfg │ │ └── setup.py │ ├── uav_gazebo/ │ │ ├── CMakeLists.txt │ │ ├── launch/ │ │ │ └── uav_gazebo.launch.py │ │ ├── meshes/ │ │ │ └── quadrotor_base.dae │ │ ├── model.config │ │ ├── msg/ │ │ │ ├── DesiredData.msg │ │ │ ├── ErrorData.msg │ │ │ ├── ModeData.msg │ │ │ └── StateData.msg │ │ ├── package.xml │ │ ├── urdf/ │ │ │ └── uav.urdf.xacro │ │ └── worlds/ │ │ └── simple.world │ └── uav_plugins/ │ ├── CMakeLists.txt │ ├── include/ │ │ └── fdcl/ │ │ ├── common_types.hpp │ │ ├── matrix_utils.hpp │ │ └── ros_utils.hpp │ ├── package.xml │ └── src/ │ ├── fdcl_matrix_utils.cpp │ ├── fdcl_ros_utils.cpp │ └── uav_control_plugin.cpp └── tests/ ├── __init__.py └── test_matrix_utils.py ================================================ FILE CONTENTS ================================================ ================================================ FILE: .catkin_workspace ================================================ # This file currently only serves to mark the location of a catkin workspace for tool integration ================================================ FILE: .gitattributes ================================================ # LaTeX *.tex text eol=lf *.cls text eol=lf *.sty text eol=lf *.bst text eol=lf *.bib text eol=lf #sources *.c text eol=lf *.cc text eol=lf *.cxx text eol=lf *.cpp text eol=lf *.c++ text eol=lf *.hpp text eol=lf *.h text eol=lf *.h++ text eol=lf *.hh text eol=lf # Compiled Object files *.slo binary *.lo binary *.o binary *.obj binary # Precompiled Headers *.gch binary *.pch binary # Compiled Dynamic libraries *.so binary *.dylib binary *.dll binary # Compiled Static libraries *.lai binary *.la binary *.a binary *.lib binary # Executables *.exe binary *.out binary *.app binary #common settings that generally should always be used with your language specific settings # Auto detect text files and perform LF normalization # http://davidlaing.com/2012/09/19/customise-your-gitattributes-to-become-a-git-ninja/ * text=auto # # The above will handle all files NOT found below # # Documents *.doc diff=astextplain *.DOC diff=astextplain *.docx diff=astextplain *.DOCX diff=astextplain *.dot diff=astextplain *.DOT diff=astextplain *.pdf diff=astextplain *.PDF diff=astextplain *.rtf diff=astextplain *.RTF diff=astextplain *.md text eol=lf *.adoc text *.textile text *.mustache text *.csv text eol=lf *.tab text eol=lf *.tsv text eol=lf *.sql text eol=lf # Graphics *.png binary *.jpg binary *.jpeg binary *.gif binary *.tif binary *.tiff binary *.ico binary *.svg binary *.eps binary # Basic .gitattributes for a MATLAB repo. # This template includes Simulink and MuPAD extensions, in addition # to the MATLAB extensions. # Source files # ============ *.m text eol=lf *.mu text eol=lf # Caution: *.m also matches Mathematica packages. # Binary files # ============ *.p binary *.mex* binary *.fig binary *.mat binary *.mdl binary *.slx binary *.mdlp binary *.slxp binary *.sldd binary *.mltbx binary *.mlappinstall binary *.mlpkginstall binary *.mn binary # Basic .gitattributes for a python repo. # Source files # ============ *.pxd text eol=lf *.py text eol=lf *.py3 text eol=lf *.pyw text eol=lf *.pyx text eol=lf # Binary files # ============ *.db binary *.p binary *.pkl binary *.pyc binary *.pyd binary *.pyo binary # Note: .db, .p, and .pkl files are associated # with the python modules ``pickle``, ``dbm.*``, # ``shelve``, ``marshal``, ``anydbm``, & ``bsddb`` # (among others).# Auto detect text files and perform LF normalization # http://davidlaing.com/2012/09/19/customise-your-gitattributes-to-become-a-git-ninja/ * text=auto # Custom for Visual Studio *.sln text eol=crlf *.csproj text eol=crlf *.vbproj text eol=crlf *.fsproj text eol=crlf *.dbproj text eol=crlf *.vcxproj text eol=crlf *.vcxitems text eol=crlf *.props text eol=crlf *.filters text eol=crlf ================================================ FILE: .gitignore ================================================ .vscode/ # Created by https://www.toptal.com/developers/gitignore/api/c++,jupyternotebooks,python,linux,windows,macos,vscode,vim,ros,matlab,libreoffice,microsoftoffice # Edit at https://www.toptal.com/developers/gitignore?templates=c++,jupyternotebooks,python,linux,windows,macos,vscode,vim,ros,matlab,libreoffice,microsoftoffice ### C++ ### # Prerequisites *.d # Compiled Object files *.slo *.lo *.o *.obj # Precompiled Headers *.gch *.pch # Linker files *.ilk # Debugger Files *.pdb # Compiled Dynamic libraries *.so *.dylib *.dll # Fortran module files *.mod *.smod # Compiled Static libraries *.lai *.la *.a *.lib # Executables *.exe *.out *.app ### JupyterNotebooks ### # gitignore template for Jupyter Notebooks # website: http://jupyter.org/ .ipynb_checkpoints */.ipynb_checkpoints/* # IPython profile_default/ ipython_config.py # Remove previous ipynb_checkpoints # git rm -r .ipynb_checkpoints/ ### LibreOffice ### # LibreOffice locks .~lock.*# ### Linux ### *~ # temporary files which can be created if a process still has a handle open of a deleted file .fuse_hidden* # KDE directory preferences .directory # Linux trash folder which might appear on any partition or disk .Trash-* # .nfs files are created when an open file is removed but is still being accessed .nfs* ### macOS ### # General .DS_Store .AppleDouble .LSOverride # Icon must end with two \r Icon # Thumbnails ._* # Files that might appear in the root of a volume .DocumentRevisions-V100 .fseventsd .Spotlight-V100 .TemporaryItems .Trashes .VolumeIcon.icns .com.apple.timemachine.donotpresent # Directories potentially created on remote AFP share .AppleDB .AppleDesktop Network Trash Folder Temporary Items .apdisk ### MATLAB ### # Windows default autosave extension *.asv # OSX / *nix default autosave extension *.m~ # Compiled MEX binaries (all platforms) *.mex* # Packaged app and toolbox files *.mlappinstall *.mltbx # Generated helpsearch folders helpsearch*/ # Simulink code generation folders slprj/ sccprj/ # Matlab code generation folders codegen/ # Simulink autosave extension *.autosave # Simulink cache files *.slxc # Octave session info octave-workspace ### MicrosoftOffice ### *.tmp # Word temporary ~$*.doc* # Word Auto Backup File Backup of *.doc* # Excel temporary ~$*.xls* # Excel Backup File *.xlk # PowerPoint temporary ~$*.ppt* # Visio autosave temporary files *.~vsd* ### Python ### # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] *$py.class # C extensions # Distribution / packaging .Python build/ develop-eggs/ dist/ downloads/ eggs/ .eggs/ lib/ lib64/ parts/ sdist/ var/ wheels/ pip-wheel-metadata/ share/python-wheels/ *.egg-info/ .installed.cfg *.egg MANIFEST # PyInstaller # Usually these files are written by a python script from a template # before PyInstaller builds the exe, so as to inject date/other infos into it. *.manifest *.spec # Installer logs pip-log.txt pip-delete-this-directory.txt # Unit test / coverage reports htmlcov/ .tox/ .nox/ .coverage .coverage.* .cache nosetests.xml coverage.xml *.cover *.py,cover .hypothesis/ .pytest_cache/ pytestdebug.log # Translations *.mo *.pot # Django stuff: *.log local_settings.py db.sqlite3 db.sqlite3-journal # Flask stuff: instance/ .webassets-cache # Scrapy stuff: .scrapy # Sphinx documentation docs/_build/ doc/_build/ # PyBuilder target/ # Jupyter Notebook # IPython # pyenv .python-version # pipenv # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. # However, in case of collaboration, if having platform-specific dependencies or dependencies # having no cross-platform support, pipenv may install dependencies that don't work, or not # install all needed dependencies. #Pipfile.lock # PEP 582; used by e.g. github.com/David-OConnor/pyflow __pypackages__/ # Celery stuff celerybeat-schedule celerybeat.pid # SageMath parsed files *.sage.py # Environments .env .venv env/ venv/ ENV/ env.bak/ venv.bak/ pythonenv* # Spyder project settings .spyderproject .spyproject # Rope project settings .ropeproject # mkdocs documentation /site # mypy .mypy_cache/ .dmypy.json dmypy.json # Pyre type checker .pyre/ # pytype static type analyzer .pytype/ # profiling data .prof ### ROS ### .catkin_tools/ devel/ logs/ bin/ msg_gen/ srv_gen/ msg/*Action.msg msg/*ActionFeedback.msg msg/*ActionGoal.msg msg/*ActionResult.msg msg/*Feedback.msg msg/*Goal.msg msg/*Result.msg msg/_*.py build_isolated/ devel_isolated/ # Generated by dynamic reconfigure *.cfgc /cfg/cpp/ /cfg/*.py # Ignore generated docs *.dox *.wikidoc # eclipse stuff .project .cproject # qcreator stuff CMakeLists.txt.user srv/_*.py *.pcd *.pyc qtcreator-* *.user /planning/cfg /planning/docs /planning/src # Emacs .#* # Catkin custom files CATKIN_IGNORE ### Vim ### # Swap [._]*.s[a-v][a-z] !*.svg # comment out if you don't need vector files [._]*.sw[a-p] [._]s[a-rt-v][a-z] [._]ss[a-gi-z] [._]sw[a-p] # Session Session.vim Sessionx.vim # Temporary .netrwhist # Auto-generated tag files tags # Persistent undo [._]*.un~ ### ROS2 ### install/ log/ build/ # Ignore generated docs *.dox *.wikidoc # eclipse stuff .project .cproject # qcreator stuff CMakeLists.txt.user srv/_*.py *.pcd *.pyc qtcreator-* *.user *~ # Emacs .#* # Colcon custom files COLCON_IGNORE AMENT_IGNORE ### vscode ### .vscode/* !.vscode/settings.json !.vscode/tasks.json !.vscode/launch.json !.vscode/extensions.json *.code-workspace ### Windows ### # Windows thumbnail cache files Thumbs.db Thumbs.db:encryptable ehthumbs.db ehthumbs_vista.db # Dump file *.stackdump # Folder config file [Dd]esktop.ini # Recycle Bin used on file shares $RECYCLE.BIN/ # Windows Installer files *.cab *.msi *.msix *.msm *.msp # Windows shortcuts *.lnk # End of https://www.toptal.com/developers/gitignore/api/c++,jupyternotebooks,python,linux,windows,macos,vscode,vim,ros,matlab,libreoffice,microsoftoffice ================================================ FILE: .gitmodules ================================================ [submodule "libraries/eigen"] path = libraries/eigen url = https://github.com/fdcl-gwu/eigen.git ================================================ FILE: .travis.yml ================================================ language: python python: - "3.9" # command to install dependencies install: - pip install -r requirements.txt # command to run tests script: - python -m unittest ================================================ FILE: Dockerfile ================================================ FROM ros:iron-ros-core-jammy RUN apt update RUN apt -y install python3-pip tmux RUN apt -y install python3-colcon-common-extensions RUN apt -y install ros-iron-gazebo-ros-pkgs ros-iron-gazebo-ros2-control ros-iron-xacro RUN python3 -m pip install pip --upgrade RUN python3 -m pip install numpy pandas matplotlib RUN python3 -m pip install PyQt5 RUN adduser --disabled-password sim USER sim WORKDIR /home ================================================ FILE: License ================================================ MIT License Copyright (c) 2020 Flight Dynamics and Control Lab Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. ================================================ FILE: README.md ================================================ |![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)| |-|-| |[![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) | # Python - Gazebo Simulation Environment for a UAV with Geometric Control This 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). ## Features * Developed using Python * Uses a geometric controller that works great with aggressive maneuvers * Uses Gazebo as the physics engine * Has a nice GUI for controlling the UAV * 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. ![Landing](images/trajectory.gif) ### Why Python? * Python makes developing/debugging easier and shorter (no compiling) * Can easily find open-source modules or libraries for different tasks ## Which controller is used for the UAV control? * A geometric controller with decoupled-yaw attitude control is used * The controller is published in: ```sh @InProceedings{Gamagedara2019b, title={Geometric controls of a quadrotor uav with decoupled yaw control}, author={Gamagedara, Kanishke and Bisheban, Mahdis and Kaufman, Evan and Lee, Taeyoung}, booktitle={2019 American Control Conference (ACC)}, pages={3285--3290}, year={2019}, organization={IEEE} } ``` * 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) ## Which estimator is used for the state estimation? * The estimator defined in the following paper is implemented here (except for the sensor bias estimation terms): ```sh @InProceedings{Gamagedara2019a, author = {Kanishke Gamagedara and Taeyoung Lee and Murray R. Snyder}, title = {Real-time Kinematics {GPS} Based Telemetry System for Airborne Measurements of Ship Air Wake}, booktitle = {{AIAA} Scitech 2019 Forum}, year = {2019}, month = {jan}, publisher = {American Institute of Aeronautics and Astronautics}, doi = {10.2514/6.2019-2377} } ``` * Matlab implementation of the above controller can be found at [https://github.com/fdcl-gwu/dkf-comparison](https://github.com/fdcl-gwu/dkf-comparison). * 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. ## Setting-up ### Releases The 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. 1. [v1.0](https://github.com/fdcl-gwu/uav_simulator/tree/v1.0): Ubuntu 18.04 with ROS-Melodic 1. [v2.0](https://github.com/fdcl-gwu/uav_simulator/tree/v2.0): Ubuntu 20.04 with ROS-Noetic 1. [v3.0](https://github.com/fdcl-gwu/uav_simulator/tree/v3.0): Ubuntu 22.04 with ROS2-Iron :bangbang: If you are trying to use an older release, please checkout that release, and use setup instructions there. Each release has different instructions. :bangbang: If you are running this on a virtual machine, please make sure that Gazebo can run at real-time speed. It 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)). ### Setting-up the repository 1. Clone the repository. ```sh git clone https://github.com/fdcl-gwu/uav_simulator.git ``` 1. Update the submodules. ```sh cd uav_simulator git submodule update --init --recursive ``` ### Dependencies You have to options here: 1. Installing everything locally 1. Running a docker container Installing 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. Docker solves this by streamlining all the dependencies, up-to the OS. For example, if you are on Ubuntu 22.04 and want to test the ROS-Melodic version, docker will be the only way. If you want to install everything locally, follow [Local Install](#local-install). If you want to run a docker container instead, skip to [Docker Setup](#docker-setup). #### Local Install 1. [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. 1. Python modules: these libraries must be installed in the system 1. NumPy 1. Pandas 1. Matplotlib ```sh python3 -m pip install numpy pandas matplotlib ``` Now, skip to [Setting-up the plugins and Gazebo](#setting-up-the-plugins-and-gazebo). #### Docker Setup The instructions here assume you are on Ubuntu. This has not been tested on other OS versions. 1. Install docker following [official instructions](https://docs.docker.com/engine/install/ubuntu/). 1. If you are not already there, `cd uav_simulator` 1. Enable xhost (required for Gazebo and GUI): `xhost +` 1. Build the docker image: `docker build -t uav_simulator .` (see following paragraph if you just want to pull the already built image instead) 1. Run a container: `bash docker_run.sh` The last command will start a docker container, install all the dependencies, and mount the local directory there. The first time you run the build command will take a while as it installs all the libraries. You can skip the build command altogether by pulling the built docker from the Docker Hub with the following command. This is NOT required if you are building it locally using the build command. ```sh docker pull kanishgama/uav_simulator:ros2-iron bash docker_run.sh ``` After that, you only need to run the `bash docker_run.sh` every time you need to run the simulation. Since 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. For running the code, simply follow [Setting-up the plugins and Gazebo](#setting-up-the-plugins-and-gazebo), and onwards. ### Setting-up the plugins and Gazebo You only need to do the followings once (unless you change the Gazebo plugins) 1. Make the plugging. ```sh # From uav_simulator colcon build ``` 1. Source the relevant directories (**NOTE**: you need to do this on every new terminal). ```sh # From uav_simulator source install/local_setup.bash ``` ### Running the simulation environment 1. In the current terminal window, launch the Gazebo environment: ```sh # From uav_simulator ros2 launch uav_gazebo uav_gazebo.launch.py ``` 1. 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): ```sh # From uav_simulator ros2 launch fdcl_uav fdcl_uav_launch.py ``` Everytime you change the Python code, run the following commands ```sh # From uav_simulator colcon build --packages-select fdcl_uav ros2 launch fdcl_uav fdcl_uav_launch.py ``` The code has been tested with Python3.10.12, which comes default with Ubuntu 22.04. ![Terminal](images/running.gif) ### Tips 1. Every time you change the simulation environment, you have to kill the program, `colcon build` and re-run it. 1. If you do not make any changes to the simulation environment, you only need to kill the Python program. ## Control Guide * Simply click on the buttons on the GUI to control the UAV. * You can easily switch between each trajectory mode simply clicking on the radio buttons. * Stay mode simply commands the UAV to stay at the current position. * When take-off, stay, and circle trajectories end, the UAV switches to the "manual" mode. * When the UAV is in manual, you can use following keys (these are not case sensitive): * `WASD`: to move in horizontal plane * `P`: increase altitude * `L`: decrease altitude * `Q`: yaw rotation in anti-clockwise direction * `E`: yaw rotation in clockwise direction * At any point of the flight, you can use following keys (these are not case sensitive): * `M`: kill motors * `0-5`: set the flight mode without clicking on the GUI * Please not that the GUI must be in focus for any of the above keys to work. * If you want to change the above keyboard shortcuts, you can do so by editing `on_key_press` function in `gui.py`. ## Running Unit-Tests * Make sure you are in the main directory. * Run `python -m unittest`. * Unit tests have only been tested on Python 3.9. * Currently, unit test only covers the `matrix_utils.py` module. ================================================ FILE: docker_release.sh ================================================ docker build --tag -t uav_simulator . docker tag uav_simulator kanishgama/uav_simulator: docker push kanishgama/uav_simulator: ================================================ FILE: docker_run.sh ================================================ echo "Starting docker image .." if [ "$(docker ps -a -q -f name=uav-simulator-container)" ]; then echo "Previously created container exists, starting it .." docker start --interactive uav-simulator-container else echo "No previously created container, creating a new one .." docker run \ --name uav-simulator-container \ --mount source="$(pwd)",target=/home/uav_simulator,type=bind \ --net host \ -v /tmp/.X11-unix:/tmp/.X11-unix \ -u sim \ -e DISPLAY=unix$DISPLAY \ --privileged \ -it uav_simulator bash fi echo "Docker run complete" ================================================ FILE: scripts/data_logs/.gitignore ================================================ *.txt ================================================ FILE: scripts/plot_utils.py ================================================ import itertools import matplotlib.pyplot as plt import numpy as np import pandas as pd import pdb # plt.style.use('seaborn') def plot_data(): file_name = get_file_name() data = Data(file_name) t = data.t / 1000.0 plot_31_2(t, data.x, t, data.xd, title='x', legend=['Estimated', 'Desired']) plot_33_2(t, data.R, t, data.Rd, title='R', legend=['Estimated', 'Desired']) print('Press Q to close each figure.') plt.show() def get_file_name(): with open('data_logs/last_log.txt', 'r') as f: return f.readline() def plot_31_1(x, y, x_label='', y_label=['1', '2', '3'], title=''): fig, ax = plt.subplots(3, 1) fig.suptitle(r'${{{}}}$'.format(title), fontsize=12) for i in range(3): ax[i].plot(x[:], y[i][:], 'r') ax[i].set_ylabel(r'${{{}}}$'.format(y_label[i])) ax[i].grid('on') def plot_31_2(x1, y1, x2, y2, x_label='', y_label=['', '', ''], title='', legend=['', '']): fig, ax = plt.subplots(3, 1) fig.suptitle(r'${{{}}}$'.format(title), fontsize=12) for i in range(3): ax[i].plot(x1[:], y1[i][:], 'r', label=legend[0]) ax[i].plot(x2[:], y2[i][:], 'g', label=legend[1]) ax[i].set_ylabel(r'${{{}}}$'.format(y_label[i])) ax[i].set_xlabel(r'${{{}}}$'.format(x_label)) ax[i].grid('on') if not legend[0] == '': ax[0].legend() def plot_33_1(x, y, x_label='', title=''): fig, ax = plt.subplots(3, 3) fig.suptitle(r'${{{}}}$'.format(title), fontsize=12) for i, j in list(itertools.product(range(0, 3), range(0, 3))): ax[i, j].plot(x[:], y[i, j, :], 'r') ax[i, j].set_ylim((-1.2, 1.2)) ax[i, j].grid('on') def plot_33_2(x1, y1, x2, y2, x_label='', title='', legend=['', '']): fig, ax = plt.subplots(3, 3) fig.suptitle(r'${{{}}}$'.format(title), fontsize=12) for i, j in list(itertools.product(range(0, 3), range(0, 3))): ax[i, j].plot(x1[:], y1[i, j, :], 'r', label=legend[0]) ax[i, j].plot(x2[:], y2[i, j, :], 'g', label=legend[1]) ax[i, j].set_ylim((-1.2, 1.2)) ax[i, j].grid('on') if not legend[0] == '': ax[0, 0].legend() class Data: def __init__(self, file_name): self.data = pd.read_csv(file_name, header=0, skipinitialspace=True) self.t_system = self.data['time'] self.t = self.data['t'] N = len(self.t) self.x = self.get_data_3x1('x') self.v = self.get_data_3x1('v') self.a = self.get_data_3x1('a') self.W = self.get_data_3x1('W') self.xd = self.get_data_3x1('xd') self.vd = self.get_data_3x1('xd_dot') self.b1d = self.get_data_3x1('b1d') self.R = self.rot_mat_from_df('R', N) self.Rd = self.rot_mat_from_df('Rd', N) def get_data_3x1(self, var_name): return [self.data['{}_0'.format(var_name)], self.data['{}_1'.format(var_name)], self.data['{}_2'.format(var_name)]] def rot_mat_from_df(self, mat, N): R = np.zeros((3, 3, N)) for i in range(0, N): for j, k in list(itertools.product(range(0, 3), range(0, 3))): R[j, k, i] = self.data['{}_{}{}'.format(mat, j, k)][i] return R if __name__ == '__main__': plot_data() ================================================ FILE: scripts/thread_log.py ================================================ from rover import rover import datetime import numpy as np import rospy def thread_log(): print('LOG: thread starting ..') freq = 50.0 t0 = datetime.datetime.now() t = datetime.datetime.now() t_pre = datetime.datetime.now() avg_number = 100 header_written = False file_open = False file_name = rover.t0.strftime('data_logs/log_%Y%m%d_%H%M%S.txt') rate = rospy.Rate(freq) while not rospy.is_shutdown() and rover.on: t = datetime.datetime.now() dt = (t - t_pre).total_seconds() if dt < 1e-6: continue freq = (freq * (avg_number - 1) + (1 / dt)) / avg_number t_pre = t rover.freq_log = freq dt_millis = t - t0 t_millis = int(dt_millis.seconds * 1e3 + dt_millis.microseconds / 1e3) if rover.save_on: if not header_written: header_written = True write_header(file_name) else: if not file_open: f = open(file_name, 'a') file_open = True write_date(f, t_millis) rate.sleep() if file_open: f.close() print('LOG: thread closed!') def write_header(file_name): # NOTE: header order and the data order must be of the same order. with open(file_name, 'w') as f: f.write('time,') f.write('t,') f.write(string_vector('x')) f.write(string_vector('v')) f.write(string_vector('a')) f.write(string_vector('W')) f.write(string_3x3('R')) f.write(string_vector('xd')) f.write(string_vector('xd_dot')) f.write(string_vector('b1d')) f.write(string_vector('Wd')) f.write(string_3x3('Rd')) f.write('\n') with open('data_logs/last_log.txt', 'w') as f: f.write(file_name) def write_date(f, t_millis): # NOTE: header order and the data order must be of the same order. write_scalar(f, datetime.datetime.now().strftime('%H%M%S.%f')) write_scalar(f, t_millis) write_vector(f, rover.x) write_vector(f, rover.v) write_vector(f, rover.a) write_vector(f, rover.W) write_3x3(f, rover.R) write_vector(f, rover.control.xd) write_vector(f, rover.control.xd_dot) write_vector(f, rover.control.b1d) write_vector(f, rover.control.Wd) write_3x3(f, rover.control.Rd) f.write('\n') def string_vector(name, length=3): out = '' for i in range(length): out += '{}_{},'.format(name, i) return out def string_3x3(name): out = '' for i in range(3): for j in range(3): out += '{}_{}{},'.format(name, i, j) return out def write_scalar(f, data): f.write('{},'.format(data)) def write_vector(f, data, length=3): line = '' for i in range(length): line += '{},'.format(data[i]) f.write(line) def write_3x3(f, data): for i in range(3): for j in range(3): f.write('{},'.format(data[i, j])) ================================================ FILE: src/fdcl_uav/fdcl_uav/__init__.py ================================================ ================================================ FILE: src/fdcl_uav/fdcl_uav/control.py ================================================ from .matrix_utils import hat, vee, deriv_unit_vector, saturate from .integral_utils import IntegralError, IntegralErrorVec3 import datetime import numpy as np import rclpy from rclpy.node import Node from geometry_msgs.msg import Vector3, WrenchStamped from std_msgs.msg import Int32, Bool from uav_gazebo.msg import DesiredData, ErrorData, ModeData, StateData class ControlNode(Node): """Controller for the UAV trajectory tracking. This class detemines the control outputs for a quadrotor UAV given it's current state and the desired states. Attributes: t0: (datetime object) time at epoch t: (float) current time since epoch [s] t_prev: (float) time since epoch in the previous loop [s] dt: (float) time difference between two calls to the controller [s] x: (3x1 numpy array) current position of the UAV [m] v: (3x1 numpy array) current velocity of the UAV [m/s] a: (3x1 numpy array) current acceleration of the UAV [m/s^s] R: (3x3 numpy array) current attitude of the UAV in SO(3) W: (3x1 numpy array) current angular velocity of the UAV [rad/s] xd: (3x1 numpy array) desired position of the UAV [m] xd_dot: (3x1 numpy array) desired velocity of the UAV [m/s] xd_2dot: (3x1 numpy array) desired accleration of the UAV [m/s^2] xd_3dot: (3x1 numpy array) desired third derivative of the UAV position [m/s^3] xd_4dot: (3x1 numpy array) desired fourth derivative of the UAV position [m/s^4] b1d: (3x1 numpy array) desired direction of the first body axis b1d_dot: (3x1 numpy array) first time derivative of the desired direction of the first body axis b1d_2dot: (3x1 numpy array) second time desired direction of the first body axis Wd: (3x1 numpy array) desired body angular velocity [rad/s] Wd_dot: (3x1 numpy array) desired body angular acceleration [rad/s^2] Rd: (3x3 numpy array) desired attitude in SO(3) b3d: (3x1 numpy array) desired direction of the third body axis b3d_dot: (3x1 numpy array) first time derivative of the desired direction of the third body axis b3d_2dot: (3x1 numpy array) second time desired direction of the third body axis b1c: (3x1 numpy array) calculated direction of the first body axis wc3: (float) angular velocity around third body axis [rad/s] wc3_dot: (float) angular acceleration around third body axis [rad/s^s] use_integral: (bool) flag to enable/disable integral control e1 = (3x1 numpy array) direction of the e1 axis e2 = (3x1 numpy array) direction of the e2 axis e3 = (3x1 numpy array) direction of the e3 axis m: (float) mass of the rover [kg] g: (float) gravitational acceleration [m/s^2] c_tf: (float) torsional moment generated by the propellers l: (float) length of the rover arm [m] J: (3x3 numpy array) inertia matrix of the rover kR: (3x3 numpy array) attitude gains kW: (3x3 numpy array) angular rate gains ky: (float) yaw gain for decoupled-yaw controller kwy: (float) yaw angular rate gain for decoupled-yaw controller kX: (3x3 numpy array) position gains kV: (3x3 numpy array) velocity gains kIR: (float) attitude integral gain ki: (float) position integral gain kI: (float) ttitude integral gain for roll and pitch kyI: (float) attitude integral gain for yaw kIX: (float) position integral gains c1: (float) parameters for decoupled-yaw integral control c2: (float) parameters for decoupled-yaw integral control c3: (float) parameters for decoupled-yaw integral control fM: (4x1 numpy array) force-moment vector f_total: (float) calculated forces required by each moter fM_to_forces: (4x4 numpy array) force to force-moment conversion matrix fM_to_forces_inv: (4x4 numpy array) force-moment to force conversion matrix eIX: (IntegralErrorVec3) position integral error eIR: (IntegralErrorVec3) attitude integral error eI1: (IntegralError) attitude integral error for roll axis eI2: (IntegralError) attitude integral error for pitch axis eIy: (IntegralError) attitude integral error for yaw axis eIX: (IntegralError) position integral error sat_sigma """ def __init__(self): Node.__init__(self, 'control') self.t0 = self.get_clock().now() self.t = 0.0 self.t_pre = 0.0 self.dt = 1e-9 # Current state self.x = np.zeros(3) self.v = np.zeros(3) self.a = np.zeros(3) self.R = np.identity(3) self.W = np.zeros(3) # Desired states self.xd = np.zeros(3) self.xd_dot = np.zeros(3) self.xd_2dot = np.zeros(3) self.xd_3dot = np.zeros(3) self.xd_4dot = np.zeros(3) self.b1d = np.zeros(3) self.b1d[0] = 1.0 self.b1d_dot = np.zeros(3) self.b1d_2dot = np.zeros(3) self.Wd = np.zeros(3) self.Wd_dot = np.zeros(3) self.Rd = np.identity(3) self.b3d = np.zeros(3) self.b3d_dot = np.zeros(3) self.b3d_2dot = np.zeros(3) self.b1c = np.zeros(3) self.wc3 = 0.0 self.wc3_dot = 0.0 # Flag to enable/disable integral control self.use_integral = False self.e1 = np.zeros(3) self.e1[0] = 1.0 self.e2 = np.zeros(3) self.e2[1] = 1.0 self.e3 = np.zeros(3) self.e3[2] = 1.0 self.m = 1.95 # Mass of the rover (kg) self.g = 9.81 # Gravitational acceleration (m/s^2) self.c_tf = 0.0135 # Torsional moment generated by the propellers self.l = 0.23 # Length of the rover arm (m) self.J = np.diag([0.02, 0.02, 0.04]) # Inertia matrix of the rover # Attitude gains self.kR = np.diag([1.6, 1.6, 0.6]) # Attitude gains self.kW = np.diag([0.40, 0.40, 0.10]) # Angular rate gains self.ky = self.kR[2, 2] # Yaw gain for decoupled-yaw controller self.kwy = self.kR[2, 2] # Yaw angular rate gain for decoupled-yaw # controller # Position gains self.kX = np.diag([16.0, 16.0, 20.0]) # Position gains self.kV = np.diag([12.0, 12.0, 12.0]) # Velocity gains # Integral gains self.kIR = 0.015 # Attitude integral gain self.ki = 0.01 # Position integral gain self.kI = 0.01 # Attitude integral gain for roll and pitch self.kyI = 0.02 # Attitude integral gain for yaw self.kIX = 4.0 # Position integral gains self.c1 = 1.0 # Parameters for decoupled-yaw integral control self.c2 = 1.0 # Parameters for decoupled-yaw integral control self.c3 = 1.0 # Parameters for decoupled-yaw integral control self.fM = np.zeros((4, 1)) # Force-moment vector self.f_total = 0.0 # Calculated forces required by each moter fM_to_forces = np.array([ [1.0, 1.0, 1.0, 1.0], [0.0, -self.l, 0.0, self.l], [self.l, 0.0, -self.l, 0.0], [-self.c_tf, self.c_tf, -self.c_tf, self.c_tf] ]) self.fM_to_forces_inv = np.linalg.inv(fM_to_forces) # Force to # force-moment conversion matrix # Control errors self.ex = np.zeros(3) self.ev = np.zeros(3) self.eR = np.zeros(3) self.eW = np.zeros(3) # Integral errors self.eIX = IntegralErrorVec3() # Position integral error self.eIR = IntegralErrorVec3() # Attitude integral error self.eI1 = IntegralError() # Attitude integral error for roll axis self.eI2 = IntegralError() # Attitude integral error for pitch axis self.eIy = IntegralError() # Attitude integral error for yaw axis self.sat_sigma = 1.8 self.is_landed = True self.mode = 0 self.motor_on = False self.init_subscribers() self.init_publishers() def control_run(self): """Run the controller to get the force-moments required to achieve the the desired states from the current state. Args: state: (x, v, a, R, W) current states of the UAV desired: (xd, xd_dot, xd_2dot, xd_3dot, xd_4dot, b1d, b1d_dot, b1d_2dot, is_landed) desired states of the UAV Return: fM: (4x1 numpy array) force-moments vector """ # If the UAV is landed, do not run the controller and produce zero # force-moments. if self.is_landed: self.fM = np.zeros(4) else: self.position_control() self.attitude_control() if (not self.motor_on) or (self.mode < 2): force = Vector3(x=0.0, y=0.0, z=0.0) torque = Vector3(x=0.0, y=0.0, z=0.0) else: # FDCL uses NED, Gazebo uses NWU force = Vector3(x=0.0, y=0.0, z=self.fM[0]) torque = Vector3(x=self.fM[1], y=-self.fM[2], z=-self.fM[3]) fM_message = WrenchStamped() fM_message.wrench.force = force fM_message.wrench.torque = torque self.pub_fM.publish(fM_message) self.publish_desired() self.publish_errors() def position_control(self): """Position controller to determine desired attitude and angular rates to achieve the deisred states. This uses the controller defined in "Control of Complex Maneuvers for a Quadrotor UAV using Geometric Methods on SE(3)" URL: https://arxiv.org/pdf/1003.2005.pdf """ m = self.m g = self.g e3 = self.e3 kX = self.kX kV = self.kV kIX = self.kIX R = self.R R_T = self.R.T x = self.x v = self.v W = self.W b1d = self.b1d b1d_dot = self.b1d_dot b1d_2dot = self.b1d_2dot xd = self.xd xd_dot = self.xd_dot xd_2dot = self.xd_2dot xd_3dot = self.xd_3dot xd_4dot = self.xd_4dot self.update_current_time() self.dt = self.t - self.t_pre # Translational error functions eX = x - xd # position error - eq (11) eV = v - xd_dot # velocity error - eq (12) # Position integral terms if self.use_integral: self.eIX.integrate(self.c1 * eX + eV, self.dt) # eq (13) self.eIX.error = saturate(self.eIX.error, \ -self.sat_sigma, self.sat_sigma) else: self.eIX.set_zero() # Force 'f' along negative b3-axis - eq (14) # This term equals to R.e3 A = - kX @ eX \ - kV @ eV \ - kIX * self.eIX.error \ - m * g * e3 \ + m * xd_2dot hatW = hat(W) b3 = R @ e3 b3_dot = R @ hatW @ e3 # eq (22) f_total = -A @ b3 # Intermediate terms for rotational errors ea = g * e3 \ - f_total / m * b3 \ - xd_2dot A_dot = - kX @ eV \ - kV @ ea \ + m * xd_3dot fdot = - A_dot @ b3 \ - A @ b3_dot eb = - fdot / m * b3 \ - f_total / m * b3_dot \ - xd_3dot A_2dot = - kX @ ea \ - kV @ eb \ + m * xd_4dot b3c, b3c_dot, b3c_2dot = deriv_unit_vector(-A, -A_dot, -A_2dot) hat_b1d = hat(b1d) hat_b1d_dot = hat(b1d_dot) A2 = -hat_b1d @ b3c A2_dot = - hat_b1d_dot @ b3c - hat_b1d @ b3c_dot A2_2dot = - hat(b1d_2dot) @ b3c \ - 2.0 * hat_b1d_dot @ b3c_dot \ - hat_b1d @ b3c_2dot b2c, b2c_dot, b2c_2dot = deriv_unit_vector(A2, A2_dot, A2_2dot) hat_b2c = hat(b2c) hat_b2c_dot = hat(b2c_dot) b1c = hat_b2c @ b3c b1c_dot = hat_b2c_dot @ b3c + hat_b2c @ b3c_dot b1c_2dot = hat(b2c_2dot) @ b3c \ + 2.0 * hat_b2c_dot @ b3c_dot \ + hat_b2c @ b3c_2dot Rd = np.vstack((b1c, b2c, b3c)).T Rd_dot = np.vstack((b1c_dot, b2c_dot, b3c_dot)).T Rd_2dot = np.vstack((b1c_2dot, b2c_2dot, b3c_2dot)).T Rd_T = Rd.T Wd = vee(Rd_T @ Rd_dot) hat_Wd = hat(Wd) Wd_dot = vee(Rd_T @ Rd_2dot - hat_Wd @ hat_Wd) self.f_total = f_total self.Rd = Rd self.Wd = Wd self.Wd_dot = Wd_dot # Roll / pitch self.b3d = b3c self.b3d_dot = b3c_dot self.b3d_2dot = b3c_2dot # Yaw self.b1c = b1c self.wc3 = e3 @ (R_T @ Rd @ Wd) self.wc3_dot = e3 @ (R_T @ Rd @ Wd_dot) \ - e3 @ (hatW @ R_T @ Rd @ Wd) self.ex = eX self.ev = eV def attitude_control(self): """Deacouple-yaw attitude controller to achieve the desired attitude and angular rates. This uses the controller defined in "Geometric Controls of a Quadrotor with a Decoupled Yaw Control" URL: https://doi.org/10.23919/ACC.2019.8815189 """ R = self.R R_T = self.R.T Rd = self.Rd Rd_T = self.Rd.T b3d = self.b3d b3d_dot = self.b3d_dot b3d_2dot = self.b3d_2dot W = self.W Wd = self.Wd J = self.J b1 = R @ self.e1 b2 = R @ self.e2 b3 = R @ self.e3 hat_b3 = hat(b3) # Roll/pitch angular velocity vector W_12 = W[0] * b1 + W[1] * b2 b3_dot = hat(W_12) @ b3 # eq (26) hat_b3d = hat(b3d) W_12d = hat_b3d @ b3d_dot W_12d_dot = hat_b3d @ b3d_2dot eb = hat_b3d @ b3 # eq (27) ew = W_12 + hat_b3 @ hat_b3 @ W_12d # eq (28) # Yaw ey = -b2 @ self.b1c ewy = W[2] - self.wc3 # Attitude integral terms eI = ew + self.c2 * eb self.eI1.integrate(eI @ b1, self.dt) # b1 axis - eq (29) self.eI2.integrate(eI @ b2, self.dt) # b2 axis - eq (30) self.eIy.integrate(ewy + self.c3 * ey, self.dt) # Control moment for the roll/pitch dynamics - eq (31) tau = -self.kR[0, 0] * eb \ - self.kW[0, 0] * ew \ - J[0, 0] * b3.T @ W_12d * b3_dot \ - J[0, 0] * hat_b3 @ hat_b3 @ W_12d_dot if self.use_integral: tau += -self.kI * self.eI1.error * b1 \ - self.kI * self.eI2.error * b2 # Control moment around b1 axis - roll - eq (24) M1 = b1.T @ tau + J[2, 2] * W[2] * W[1] # Control moment around b2 axis - pitch - eq (24) M2 = b2.T @ tau - J[2, 2] * W[2] * W[0] # Control moment around b3 axis - yaw - eq (52) M3 = - self.ky * ey \ - self.kwy * ewy \ + self.J[2, 2] * self.wc3_dot if self.use_integral: M3 += - self.kyI * self.eIy.error M = np.array([M1, M2, M3]) self.fM[0] = self.f_total for i in range(3): self.fM[i + 1] = M[i] f_motor = self.fM_to_forces_inv @ self.fM # For saving: RdtR = Rd_T @ R self.eR = 0.5 * vee(RdtR - RdtR.T) self.eIR.error = np.array([self.eI1.error, self.eI2.error, \ self.eIy.error]) self.eW = W - R_T @ Rd @ Wd # self.get_logger().info('eR ' + str(self.eR)) def set_integral_errors_to_zero(self): """Set all integrals to zero.""" self.eIX.set_zero() self.eIR.set_zero() self.eI1.set_zero() self.eI2.set_zero() self.eIy.set_zero() self.eIX.set_zero() def update_current_time(self): """Update the current time since epoch.""" self.t_pre = self.t t_now = self.get_clock().now() self.t = float((t_now - self.t0).nanoseconds) * 1e-9 def init_publishers(self): self.pub_desired = self.create_publisher(DesiredData, '/uav/desired', 1) self.pub_errors = self.create_publisher(ErrorData, '/uav/errors', 1) self.pub_fM = self.create_publisher(WrenchStamped, '/uav/fm', 1) timer_period = 0.005 self.timer = self.create_timer(timer_period, self.control_run) def init_subscribers(self): self.sub_trajectory = self.create_subscription( \ StateData, '/uav/states', self.states_callback, 1) self.sub_trajectory = self.create_subscription( \ DesiredData, '/uav/trajectory', self.trajectory_callback, 1) self.sub_mode = self.create_subscription( \ ModeData, '/uav/mode', self.mode_callback, 1) self.sub_motors_on = self.create_subscription( \ Bool, '/uav/motors_on', self.motors_on_callback, 1) def mode_callback(self, msg): if self.mode == msg.mode: return self.mode = msg.mode self.get_logger().info('Mode switched to {}'.format(self.mode)) def states_callback(self, msg): for i in range(3): self.x[i] = msg.position[i] self.v[i] = msg.velocity[i] self.a[i] = msg.acceleration[i] self.W[i] = msg.angular_velocity[i] for j in range(3): self.R[i, j] = msg.attitude[3*i + j] def trajectory_callback(self, msg): """Callback function for the trajectory subscriber. Args: msg: (DesiredData) Trajectory data message """ for i in range(3): self.xd[i] = msg.position[i] self.xd_dot[i] = msg.velocity[i] self.xd_2dot[i] = msg.acceleration[i] self.xd_3dot[i] = msg.jerk[i] self.xd_4dot[i] = msg.snap[i] self.b1d[i] = msg.b1[i] self.b1d_dot[i] = msg.b1_dot[i] self.b1d_2dot[i] = msg.b1_2dot[i] self.is_landed = msg.is_landed def motors_on_callback(self, msg): self.motor_on = msg.data if self.motor_on: self.get_logger().info('Turning motors on') else: self.get_logger().info('Turning motors off') def publish_desired(self): """Publish the desired states.""" msg = DesiredData() for i in range(3): msg.position[i] = self.xd[i] msg.velocity[i] = self.xd_dot[i] msg.acceleration[i] = self.xd_2dot[i] msg.jerk[i] = self.xd_3dot[i] msg.snap[i] = self.xd_4dot[i] msg.b1[i] = self.b1d[i] msg.b1_dot[i] = self.b1d_dot[i] msg.b1_2dot[i] = self.b1d_2dot[i] msg.angular_velocity[i] = self.Wd[i] msg.angular_acceleration[i] = self.Wd_dot[i] # msg.angular_jerk = self.Wd_2dot msg.b1c[i] = self.b1c[i] msg.b3[i] = self.b3d[i] msg.b3_dot[i] = self.b3d_dot[i] msg.b3_2dot[i] = self.b3d_2dot[i] for j in range(3): msg.attitude[3*i + j] = self.Rd[i, j] msg.wc3 = self.wc3 msg.wc3_dot = self.wc3_dot msg.is_landed = self.is_landed self.pub_desired.publish(msg) def publish_errors(self): msg = ErrorData() for i in range(3): msg.position[i] = self.ex[i] msg.velocity[i] = self.ev[i] msg.attitude[i] = self.eR[i] msg.angular_velocity[i] = self.eW[i] msg.position_integral[i] = self.eIX.error[i] msg.attitude_integral[i] = self.eIR.error[i] self.pub_errors.publish(msg) def main(args=None): print("Starting control node") rclpy.init(args=args) control = ControlNode() try: rclpy.spin(control) except KeyboardInterrupt: pass # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) control.destroy_node() print("Terminating control node") ================================================ FILE: src/fdcl_uav/fdcl_uav/estimator.py ================================================ from .matrix_utils import hat, vee, expm_SO3, q_to_R import rclpy from rclpy.node import Node import datetime import numpy as np import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry from sensor_msgs.msg import Imu from uav_gazebo.msg import StateData class EstimatorNode(Node): """Estimates the states of the UAV. This uses the estimator defined in "Real-time Kinematics GPS Based Telemetry System for Airborne Measurements of Ship Air Wake", but without the bias estimation terms. DOI: 10.2514/6.2019-2377 x (3x1 numpy array) current position of the UAV [m] x: (3x1 numpy array) current position of the UAV [m] v: (3x1 numpy array) current velocity of the UAV [m/s] a: (3x1 numpy array) current acceleration of the UAV [m/s^s] b_a: (float) accelerometer bias in e3 direction [m/s^2] R: (3x3 numpy array) current attitude of the UAV in SO(3) W: (3x1 numpy array) current angular velocity of the UAV [rad/s] Q: (7x7 numpy array) variances of w_k P: (10x10 numpy array) covariances of the states t0: (datetime object) time at epoch t: (float) current time since epoch [s] t_prev: (float) time since epoch in the previous loop [s] W_pre: (3x1 numpy array) angular velocity of the previous loop [rad/s] a_imu_pre: (3x1 numpy array) acceleration of the previous loop [m/s^2] R_pre: (3x3 numpy array) attitude in the previous loop in SO(3) b_a_pre: (3x1 numpy array) accelerometer bias in the previous loop [m/s^2] g: (float) gravitational acceleration [m/s^2] ge3: (3x1 numpy array) gravitational acceleration direction [m/s^2] R_bi: (3x3 numpy array) transformation from IMU frame to the body frame R_ib: (3x3 numpy array) transformation from IMU frame to the body frame e3 : (3x1 numpy array) direction of the e3 axis eye3: (3x3 numpy array) 3x3 identity matrix eye10: (10x10 numpy array) 10x10 identity matrix zero3: (3x3 numpy array) 3x3 zero matrix """ def __init__(self): Node.__init__(self, 'estimator') self.x = np.zeros(3) self.v = np.zeros(3) self.a = np.zeros(3) self.b_a = 0.0 self.R = np.eye(3) self.W = np.zeros(3) # Variances of w_k self.Q = np.diag([ 0.001, 0.001, 0.001, # acceleration 0.025, 0.025, 0.025, # angular velocity 0.0001 # acclerometer z bias ]) # Initial covariances of x self.P = np.diag([ 1.0, 1.0, 1.0, # position 1.0, 1.0, 1.0, # velocity 0.01, 0.01, 0.01, # attitude 1.0 # accelerometer z bias ]) self.t0 = self.get_clock().now() self.t = 0.0 self.t_pre = 0.0 self.W_pre = np.zeros(3) self.a_imu_pre = np.zeros(3) self.W_imu_pre = np.zeros(3) self.R_pre = np.eye(3) self.b_a_pre = 0.0 self.g = 9.81 self.ge3 = np.array([0.0, 0.0, self.g]) # Transformation from IMU frame to the body frame. self.R_bi = np.array([ [1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0] ]) self.R_ib = self.R_bi.T self.e3 = np.array([0.0, 0.0, 1.0]) self.eye3 = np.eye(3) self.eye10 = np.eye(10) self.zero3 = np.zeros((3, 3)) self.V_R_imu = np.diag([0.01, 0.01, 0.01]) self.V_x_gps = np.diag([0.01, 0.01, 0.01]) self.V_v_gps = np.diag([0.01, 0.01, 0.01]) # Gazebo uses ENU frame, but NED frame is used in FDCL. # Note that ENU and the NED here refer to their direction order. # ENU: E - axis 1, N - axis 2, U - axis 3 # NED: N - axis 1, E - axis 2, D - axis 3 self.R_fg = np.array([ [1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0] ]) self.init_subscribers() self.init_publishers() def prediction(self, a_imu, W_imu): """Prediction step of the estimator. Args: a_imu: (3x1 numpy array) acceleration measured by the IMU [m/s^2] W_imu: (3x1 numpy array) angular rate measured by the IMU [rad/s] """ h = self.get_dt() self.R_pre = np.copy(self.R) self.W_pre = np.copy(self.W) self.b_a_pre = self.b_a * 1.0 self.W = self.R_bi @ W_imu self.R = self.R @ expm_SO3(h / 2.0 * (self.W + self.W_pre)) # This assumes IMU provide acceleration without g self.a = self.R @ self.R_bi @ a_imu + self.b_a * self.e3 a_pre = self.R_pre @ self.R_bi @ self.a_imu_pre + self.b_a_pre * self.e3 self.x = self.x + h * self.v + h**2 / 2.0 * a_pre self.v = self.v + h / 2.0 * (self.a + a_pre) # Calculate A(t_{k-1}) A = np.zeros((10, 10)) A[0:3, 3:6] = self.eye3 A[3:6, 6:9] = - self.R_pre @ hat(self.R_bi @ self.a_imu_pre) A[3:6, 9] = self.e3 A[6:9, 6:9] = -hat(self.R_bi @ W_imu) # Calculate F(t_{k-1}) F = np.zeros((10, 7)) F[3:6, 0:3] = self.R_pre @ self.R_bi F[6:9, 3:6] = self.R_bi F[9, 6] = 1.0 # Calculate \Psi using A(t) psi = self.eye10 + h / 2.0 * A A = self.eye10 + h * A @ psi F = h * psi @ F self.P = A @ self.P @ A.T + F @ self.Q @ F.T self.a_imu_pre = a_imu self.W_imu_pre = W_imu # rover.x = self.x # rover.v = self.v # rover.a = self.a # rover.R = self.R # rover.W = self.W def imu_correction(self, R_imu, V_R_imu): """IMU correction step of the estimator. Args: R_imu: (3x3 numpy array) attitude measured by the IMU in SO(3) V_R_imu: (3x3 numpy array) attitude measurement covariance """ imu_R = self.R.T @ R_imu @ self.R_ib del_z = 0.5 * vee(imu_R - imu_R.T) H = np.block([self.zero3, self.zero3, self.eye3, np.zeros((3, 1))]) H_T = H.T G = self.R_bi G_T = G.T V = V_R_imu S = H @ self.P @ H_T + G @ V @ G_T K = self.P @ H_T @ np.linalg.inv(S) X = K @ del_z eta = X[6:9] self.R = self.R @ expm_SO3(eta) I_KH = self.eye10 - K @ H self.P = I_KH @ self.P @ I_KH.T + K @ G @ V @ G_T @ K.T def gps_correction(self, x_gps, v_gps, V_x_gps, V_v_gps): """GPS correction step of the estimator. Args: x_gps: (3x1 numpy array) position measured by the GPS [m] v_gps: (3x1 numpy array) velocity measured by the GPS [m] V_x_gps: (3x1 numpy array) position measurement covariance V_v_gps: (3x1 numpy array) velocity measurement covariance """ del_z = np.hstack((x_gps - self.x, v_gps - self.v)) H = np.block([ [self.eye3, self.zero3, self.zero3, np.zeros((3, 1))], [self.zero3, self.eye3, self.zero3, np.zeros((3, 1))] ]) H_T = H.T V = np.block([ [V_x_gps, self.zero3], [self.zero3, V_v_gps] ]) S = H @ self.P @ H_T + V K = self.P @ H_T @ np.linalg.inv(S) X = K @ del_z dx = X[0:3] dv = X[3:6] db_a = X[9] self.x = self.x + dx self.v = self.v + dv self.b_a = self.b_a + db_a I_KH = self.eye10 - K @ H self.P = I_KH @ self.P @ I_KH.T + K @ V @ K.T def get_dt(self): """Get the time difference between two loops. Return: dt: (float) time difference between two loops """ self.t_pre = self.t * 1.0 t_now = self.get_clock().now() self.t = float((t_now - self.t0).nanoseconds) * 1e-9 return self.t - self.t_pre def get_states(self): """Return the current states of the estimator. Return: x: (3x1 numpy array) current position of the UAV [m] v: (3x1 numpy array) current velocity of the UAV [m/s] a: (3x1 numpy array) current acceleration of the UAV [m/s^s] R: (3x3 numpy array) current attitude of the UAV in SO(3) W: (3x1 numpy array) current angular velocity of the UAV [rad/s] """ return (self.x, self.v, self.a, self.R, self.W) def init_subscribers(self): self.sub_imu = self.create_subscription( \ Imu, '/uav/imu', self.imu_callback, 1) self.sub_gps = self.create_subscription( \ Odometry, '/uav/gps', self.gps_callback, 1) def imu_callback(self, msg): """Callback function for the IMU subscriber. Args: msg: (Imu) IMU message """ q_gazebo = msg.orientation a_gazebo = msg.linear_acceleration W_gazebo = msg.angular_velocity q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w]) R_gi = q_to_R(q) # IMU to Gazebo frame R_fi = self.R_fg @ R_gi # IMU to FDCL frame (NED frame) # FDCL-UAV expects IMU accelerations without gravity. a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z]) a_imu = R_gi.T @ (R_gi @ a_i - self.ge3) W_imu = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z]) # TODO: get covariances from the message self.prediction(a_imu, W_imu) self.imu_correction(R_fi, self.V_R_imu) self.publish_states() def gps_callback(self, msg): """Callback function for the GPS subscriber. Args: msg: (GPS) GPS message """ x_gazebo = msg.pose.pose.position v_gazebo = msg.twist.twist.linear # Gazebo uses ENU frame, but NED frame is used in FDCL. x_gps = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z]) v_gps = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z]) self.gps_correction(x_gps, v_gps, self.V_x_gps, self.V_v_gps) self.publish_states() def init_publishers(self): self.pub_state = self.create_publisher(StateData, '/uav/states', 1) def publish_states(self): states = StateData() for i in range(3): states.position[i] = self.x[i] states.velocity[i] = self.v[i] states.acceleration[i] = self.a[i] states.angular_velocity[i] = self.W[i] for j in range(3): states.attitude[3*i + j] = self.R[i, j] self.pub_state.publish(states) def main(args=None): print("Starting estimator node") rclpy.init(args=args) estimator = EstimatorNode() try: rclpy.spin(estimator) except KeyboardInterrupt: pass # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) estimator.destroy_node() print("Terminating estimator node") ================================================ FILE: src/fdcl_uav/fdcl_uav/freq_utils.py ================================================ class Freq(): def __init__(self, name, freq): self.name = name self.t_prev = 0.0 self.num_avg = 50 self.freq_avg = freq self.dt = 0 def update(self, t_now): dt = t_now - self.t_prev if dt <= 0.0: return self.dt = dt freq = 1.0 / dt self.freq_avg = (freq + (self.num_avg - 1) * self.freq_avg) / self.num_avg self.t_prev = t_now def get_freq(self): return self.freq_avg def get_freq_str(self): return f'{self.name}: {self.freq_avg:5.1f} Hz' ================================================ FILE: src/fdcl_uav/fdcl_uav/gui.py ================================================ from .matrix_utils import q_to_R, R_to_RPY from .freq_utils import Freq import numpy as np import os from PyQt5.QtCore import Qt, QTimer from PyQt5.QtWidgets import QApplication, QLabel, QWidget, QVBoxLayout, \ QHBoxLayout, QPushButton, QRadioButton, QMainWindow import rclpy from rclpy.node import Node from geometry_msgs.msg import WrenchStamped from nav_msgs.msg import Odometry from sensor_msgs.msg import Imu from std_msgs.msg import Bool from uav_gazebo.msg import DesiredData, ErrorData, ModeData, StateData class GuiNode(Node, QMainWindow): def __init__(self): Node.__init__(self, 'gui') QMainWindow.__init__(self) self.t0 = self.get_clock().now() self.motor_on = False # States self.freq_estimator = Freq("EST", 100) self.x = np.zeros(3) self.v = np.zeros(3) self.a = np.zeros(3) self.R = np.identity(3) self.W = np.zeros(3) # Desired states self.xd = np.zeros(3) self.vd = np.zeros(3) self.b1d = np.zeros(3) self.b1d[0] = 1.0 self.Rd = np.identity(3) self.Wd = np.zeros(3) # Errors self.ex = np.zeros(3) self.ev = np.zeros(3) self.eR = np.identity(3) self.eW = np.zeros(3) self.eIX = np.zeros(3) self.eIR = np.zeros(3) # Sensor measurements self.freq_imu = Freq("IMU", 100) self.R_imu = np.identity(3) self.a_imu = np.zeros([3, 1]) self.W_imu = np.zeros([3, 1]) self.freq_gps = Freq("GPS", 10) self.x_gps = np.zeros([3, 1]) self.v_gps = np.zeros([3, 1]) # Motor forces self.freq_control = Freq("CTRL", 100) self.fM = np.zeros(4) # Manual mode self.x_offset = np.zeros(3) self.x_offset_delta = 0.1 self.yaw_offset = 0.0 self.yaw_offset_delta = 0.02 self.mode = 0 self.g = 9.81 self.ge3 = np.array([0.0, 0.0, self.g]) self.RAD2DEG = 180.0 / np.pi self.init_gui() self.init_subscribers() self.init_publishers() self.timer = QTimer() self.timer.timeout.connect(self.update) self.timer.start(100) def init_gui(self): window = QWidget() window.setWindowTitle("FDCL UAV Simulator") main_layout = QVBoxLayout() main_layout.setSpacing(10) # Status box self.label_time = QLabel("0.0") self.label_time.setMinimumWidth(80) self.label_freq_imu = QLabel("0.0") self.label_freq_gps = QLabel("0.0") self.label_freq_control = QLabel("0.0") self.label_freq_estimator = QLabel("0.0") thread_status_box = QHBoxLayout() thread_status_box.setAlignment(Qt.AlignLeft) thread_status_box.addWidget(self.label_time) thread_status_box.addWidget(self.label_freq_imu) thread_status_box.addWidget(self.label_freq_gps) thread_status_box.addWidget(self.label_freq_control) thread_status_box.addWidget(self.label_freq_estimator) # Control box self.button_on = QPushButton("Shutdown") self.button_on.clicked.connect(self.on_btn_close_clicked) self.button_motor_on = QPushButton("Turn on motors") self.button_motor_on.clicked.connect(self.on_btn_motor_on_clicked) controls_box = QHBoxLayout() controls_box.setAlignment(Qt.AlignLeft) controls_box.addWidget(self.button_on) controls_box.addWidget(self.button_motor_on) # Flight mode box label_flight_mode = QLabel("Mode: ") self.radio_button_modes = \ [ \ QRadioButton("Idle"), \ QRadioButton("Warmup"), \ QRadioButton("Take-off"), \ QRadioButton("Stay"), \ QRadioButton("Circle"), \ QRadioButton("Land") \ ] num_flight_modes = len(self.radio_button_modes) flight_mode_box = QHBoxLayout() flight_mode_box.setAlignment(Qt.AlignLeft) flight_mode_box.addWidget(label_flight_mode) for i in range(num_flight_modes): flight_mode_box.addWidget(self.radio_button_modes[i]) self.radio_button_modes[i].toggled.connect(self.on_flight_mode_changed) # State box state_box = QHBoxLayout() state_box.setAlignment(Qt.AlignLeft) state_box.addSpacing(5) [state_box, self.label_x] = self.init_vbox(state_box, "x", 4) [state_box, self.label_v] = self.init_vbox(state_box, "v", 4) [state_box, self.label_a] = self.init_vbox(state_box, "a", 4) [state_box, self.label_R] = self.init_attitude_vbox(state_box, "R") [state_box, self.label_W] = self.init_vbox(state_box, "W", 4) # Desired box desired_box = QHBoxLayout() desired_box.setAlignment(Qt.AlignLeft) desired_box.addSpacing(5) [desired_box, self.label_xd] = self.init_vbox(desired_box, "xd", 4) [desired_box, self.label_vd] = self.init_vbox(desired_box, "vd", 4) [desired_box, self.label_b1d] = self.init_vbox(desired_box, "b1d", 4) [desired_box, self.label_Rd] = self.init_attitude_vbox(desired_box, "Rd") [desired_box, self.label_Wd] = self.init_vbox(desired_box, "Wd", 4) # Sensor box sensor_box = QHBoxLayout() sensor_box.setAlignment(Qt.AlignLeft) sensor_box.addSpacing(5) [sensor_box, self.label_imu_ypr] = self.init_vbox(sensor_box, "IMU RPY", 4) [sensor_box, self.label_imu_a] = self.init_vbox(sensor_box, "IMU a", 4) [sensor_box, self.label_imu_W] = self.init_vbox(sensor_box, "IMU W", 4) [sensor_box, self.label_gps_x] = self.init_vbox(sensor_box, "GPS x", 4) [sensor_box, self.label_gps_v] = self.init_vbox(sensor_box, "GPS v", 4) # Error box error_box = QHBoxLayout() error_box.setAlignment(Qt.AlignLeft) error_box.addSpacing(5) [error_box, self.label_ex] = self.init_vbox(error_box, "ex", 4) [error_box, self.label_ev] = self.init_vbox(error_box, "ev", 4) [error_box, self.label_fM] = self.init_vbox(error_box, "f-M", 5) # [error_box, self.label_f] = self.init_vbox(error_box, "f", 5) # [error_box, self.label_thr] = self.init_vbox(error_box, "Throttle", 5) # Left box left_box = QVBoxLayout() left_box.setAlignment(Qt.AlignTop) left_box.addSpacing(5) left_box.addLayout(state_box) main_layout.addSpacing(10) left_box.addLayout(desired_box) # Right box right_box = QVBoxLayout() right_box.setAlignment(Qt.AlignTop) right_box.addSpacing(5) right_box.addLayout(sensor_box) main_layout.addSpacing(10) right_box.addLayout(error_box) # Data box data_box = QHBoxLayout() data_box.setAlignment(Qt.AlignLeft) data_box.addLayout(left_box) data_box.addSpacing(5) data_box.addLayout(right_box) main_layout.setAlignment(Qt.AlignTop) main_layout.addLayout(thread_status_box) main_layout.addSpacing(5) main_layout.addLayout(controls_box) main_layout.addSpacing(5) main_layout.addLayout(flight_mode_box) main_layout.addSpacing(5) main_layout.addLayout(data_box) window.setLayout(main_layout) self.setCentralWidget(window) window.keyPressEvent = self.on_key_press def init_subscribers(self): self.sub_states = self.create_subscription( \ StateData, '/uav/states', self.states_callback, 1) self.sub_desired = self.create_subscription( \ DesiredData, '/uav/desired', self.desired_callback, 1) self.sub_errors = self.create_subscription( \ ErrorData, '/uav/errors', self.errors_callback, 1) self.sub_fM = self.create_subscription( \ WrenchStamped, '/uav/fm', self.fM_callback, 1) self.sub_imu = self.create_subscription( \ Imu, '/uav/imu', self.imu_callback, 1) self.sub_gps = self.create_subscription( \ Odometry, '/uav/gps', self.gps_callback, 1) def init_publishers(self): self.pub_mode = self.create_publisher(ModeData, '/uav/mode', 1) self.pub_motors_on = self.create_publisher(Bool, '/uav/motors_on', 1) def update(self): t_sec = self.get_t_now() self.label_time.setText(f't: {t_sec:5.1f} s') self.label_freq_imu.setText(self.freq_imu.get_freq_str()) self.label_freq_gps.setText(self.freq_gps.get_freq_str()) self.label_freq_control.setText(self.freq_control.get_freq_str()) self.label_freq_estimator.setText(self.freq_estimator.get_freq_str()) self.update_vbox(self.label_x, self.x) self.update_vbox(self.label_v, self.v) self.update_vbox(self.label_a, self.a) self.update_attitude_vbox(self.label_R, self.R) self.update_vbox(self.label_W, self.W) self.update_vbox(self.label_xd, self.xd) self.update_vbox(self.label_vd, self.vd) self.update_vbox(self.label_b1d, self.b1d) self.update_attitude_vbox(self.label_Rd, self.Rd) self.update_vbox(self.label_Wd, self.Wd) imu_ypr = R_to_RPY(self.R_imu) * self.RAD2DEG self.update_vbox(self.label_imu_ypr, imu_ypr) self.update_vbox(self.label_imu_a, self.a_imu) self.update_vbox(self.label_imu_W, self.W_imu) self.update_vbox(self.label_gps_x, self.x_gps) self.update_vbox(self.label_gps_v, self.v_gps) self.update_vbox(self.label_ex, self.ex) self.update_vbox(self.label_ev, self.ev) self.update_vbox(self.label_fM, self.fM, data_size=4) # self.update_vbox(self.label_f, self.f) # self.update_vbox(self.label_thr, self.thr) msg = ModeData() msg.mode = self.mode msg.x_offset = self.x_offset msg.yaw_offset = self.yaw_offset self.pub_mode.publish(msg) def init_vbox(self, box, name, num): vbox = QVBoxLayout() vbox.setAlignment(Qt.AlignTop) labels = [] for _ in range(num): label_i = QLabel("{:5.2}".format(0.0)) labels.append(label_i) vbox.addWidget(label_i) labels[0].setText(name) box.addLayout(vbox) return [box, labels] def init_attitude_vbox(self, box, name): vbox_array = [QVBoxLayout(), QVBoxLayout(), QVBoxLayout()] labels = [] for vbox_index in range(len(vbox_array)): vbox = vbox_array[vbox_index] vbox.setAlignment(Qt.AlignTop) for _ in range(4): label_i = QLabel("{:6.4}".format(0.0)) labels.append(label_i) vbox.addWidget(label_i) box.addLayout(vbox) labels[0].setText(name) labels[4].setText("") labels[8].setText("") return [box, labels] def update_vbox(self, labels, data, data_size=3): for i in range(data_size): try: labels[i + 1].setText("{:>5.2f}".format(data[i])) except TypeError: labels[i + 1].setText("{:>5.2f}".format(-1)) def update_attitude_vbox(self, labels, data): for i in range(3): for j in range(1, 4): labels[i * 4 + j].setText("{:>5.2f}".format(data[i, j - 1])) def on_btn_close_clicked(self): self.get_logger().info("Shutdown button clicked") self.get_logger().info('Turning motors off') self.motor_on = False msg = Bool() msg.data = self.motor_on self.pub_motors_on.publish(msg) self.destroy_node() self.close() rclpy.shutdown() def on_btn_motor_on_clicked(self): if self.motor_on: self.get_logger().info('Turning motors off') self.motor_on = False self.button_motor_on.setText("Turn on motors") else: self.get_logger().info('Turning motors on') self.motor_on = True self.button_motor_on.setText("Turn off motors") msg = Bool() msg.data = self.motor_on self.pub_motors_on.publish(msg) def on_flight_mode_changed(self): for i in range(len(self.radio_button_modes)): if self.radio_button_modes[i].isChecked(): self.get_logger().info('Mode switched to {}'.format(i)) self.mode = i msg = ModeData() msg.mode = self.mode self.pub_mode.publish(msg) self.x_offset = np.zeros(3) self.yaw_offset = 0.0 break def on_key_press(self, event): self.get_logger().info('Key presed {}'.format(event.key())) if event.key() == Qt.Key_M: self.on_btn_motor_on_clicked() elif event.key() == Qt.Key_QuoteLeft: self.radio_button_modes[0].setChecked(True) elif event.key() == Qt.Key_1: self.radio_button_modes[1].setChecked(True) elif event.key() == Qt.Key_2: self.radio_button_modes[2].setChecked(True) elif event.key() == Qt.Key_3: self.radio_button_modes[3].setChecked(True) elif event.key() == Qt.Key_4: self.radio_button_modes[4].setChecked(True) elif event.key() == Qt.Key_5: self.radio_button_modes[5].setChecked(True) elif event.key() == Qt.Key_6: self.radio_button_modes[6].setChecked(True) elif event.key() == Qt.Key_W: self.x_offset[0] += self.x_offset_delta elif event.key() == Qt.Key_S: self.x_offset[0] -= self.x_offset_delta elif event.key() == Qt.Key_D: self.x_offset[1] += self.x_offset_delta elif event.key() == Qt.Key_A: self.x_offset[1] -= self.x_offset_delta elif event.key() == Qt.Key_L: self.x_offset[2] += self.x_offset_delta elif event.key() == Qt.Key_P: self.x_offset[2] -= self.x_offset_delta elif event.key() == Qt.Key_E: self.yaw_offset += self.yaw_offset_delta elif event.key() == Qt.Key_Q: self.yaw_offset -= self.yaw_offset_delta def get_t_now(self): t_now = self.get_clock().now() t_sec = float((t_now - self.t0).nanoseconds) * 1e-9 return t_sec def states_callback(self, msg): """Callback function for the states subscriber. Args: msg: (StateData) State data message """ self.freq_estimator.update(self.get_t_now()) for i in range(3): self.x[i] = msg.position[i] self.v[i] = msg.velocity[i] self.a[i] = msg.acceleration[i] self.W[i] = msg.angular_velocity[i] for j in range(3): self.R[i, j] = msg.attitude[3*i + j] def desired_callback(self, msg): """Callback function for the desired subscriber. Args: msg: (DesiredData) Desired data message """ for i in range(3): self.xd[i] = msg.position[i] self.vd[i] = msg.velocity[i] self.b1d[i] = msg.b1[i] self.Wd[i] = msg.angular_velocity[i] for j in range(3): self.Rd[i, j] = msg.attitude[3*i + j] def errors_callback(self, msg): """Callback function for the error subscriber. Args: msg: (ErrorData) Error data message """ for i in range(3): self.ex[i] = msg.position[i] self.ev[i] = msg.velocity[i] self.eR[i] = msg.attitude[i] self.eW[i] = msg.angular_velocity[i] self.eIX[i] = msg.position_integral[i] self.eIR[i] = msg.attitude_integral[i] def imu_callback(self, msg): """Callback function for the IMU subscriber. Args: msg: (Imu) IMU message """ self.freq_imu.update(self.get_t_now()) # Gazebo uses ENU frame, but NED frame is used in FDCL. # Note that ENU and the NED here refer to their direction order. # ENU: E - axis 1, N - axis 2, U - axis 3 # NED: N - axis 1, E - axis 2, D - axis 3 self.R_fg = np.array([ [1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0] ]) q_gazebo = msg.orientation a_gazebo = msg.linear_acceleration W_gazebo = msg.angular_velocity q = np.array([q_gazebo.x, q_gazebo.y, q_gazebo.z, q_gazebo.w]) R_gi = q_to_R(q) # IMU to Gazebo frame R_fi = self.R_fg @ R_gi # IMU to FDCL frame (NED frame) self.R_imu = R_fi # FDCL-UAV expects IMU accelerations without gravity. a_i = np.array([a_gazebo.x, a_gazebo.y, a_gazebo.z]) self.a_imu = R_gi.T @ (R_gi @ a_i - self.ge3) self.W_imu = np.array([W_gazebo.x, W_gazebo.y, W_gazebo.z]) def gps_callback(self, msg): """Callback function for the GPS subscriber. Args: msg: (GPS) GPS message """ self.freq_gps.update(self.get_t_now()) x_gazebo = msg.pose.pose.position v_gazebo = msg.twist.twist.linear # Gazebo uses ENU frame, but NED frame is used in FDCL. self.x_gps = np.array([x_gazebo.x, -x_gazebo.y, -x_gazebo.z]) self.v_gps = np.array([v_gazebo.x, -v_gazebo.y, -v_gazebo.z]) def fM_callback(self, msg): self.freq_control.update(self.get_t_now()) self.fM[0] = msg.wrench.force.z self.fM[1] = msg.wrench.torque.x self.fM[2] = msg.wrench.torque.y self.fM[3] = msg.wrench.torque.z def main(args=None): print("Starting gui node") rclpy.init(args=args) app = QApplication([]) app.processEvents() gui = GuiNode() gui.show() executor = rclpy.executors.SingleThreadedExecutor() executor.add_node(gui) while rclpy.ok(): executor.spin_once() app.processEvents() app.quit() # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) gui.destroy_node() # rclpy.shutdown() print("Terminating gui node") ================================================ FILE: src/fdcl_uav/fdcl_uav/integral_utils.py ================================================ import numpy as np class IntegralErrorVec3: def __init__(self): self.error = np.zeros(3) self.integrand = np.zeros(3) def integrate(self, current_integrand, dt): self.error += (self.integrand + current_integrand) * dt / 2.0 self.integrand = current_integrand def set_zero(self): self.error = np.zeros(3) self.integrand = np.zeros(3) class IntegralError: def __init__(self): self.error = 0.0 self.integrand = 0.0 def integrate(self, current_integrand, dt): self.error += (self.integrand + current_integrand) * dt / 2.0 self.integrand = current_integrand def set_zero(self): self.error = 0.0 self.integrand = 0.0 ================================================ FILE: src/fdcl_uav/fdcl_uav/matrix_utils.py ================================================ import numpy as np # import pdb def hat(x): """Returns the hat map of a given 3x1 vector. Args: x: (3x1 numpy array) vector Returns: x_hat: (3x3 numpy array) hat of the input vector """ ensure_vector(x, 3) x_hat = np.array([ [0.0, -x[2], x[1]], [x[2], 0.0, -x[0]], [-x[1], x[0], 0.0] ]) return x_hat def vee(x): """Returns the vee map of a given 3x3 matrix. Args: x: (3x3 numpy array) hat of the input vector Returns: (3x1 numpy array) vee map of the input matrix """ ensure_skew(x, 3) return np.array([x[2,1], x[0,2], x[1,0]]) def q_to_R(q): """Converts a quaternion of a rotation matrix in SO(3). Args: q: (4x1 numpy array) quaternion in [x, y, z, w] format Returns: R: (3x3 numpy array) rotation matrix corresponding to the quaternion """ # TODO: ensure quaternion instead of ensure vector ensure_vector(q, 4) R = np.identity(3) q13 = np.array([q[0], q[1], q[2]]) q4 = q[3] hat_q = hat(q13) R += 2.0 * q4 * hat_q + 2.0 * hat_q @ hat_q return R def R_to_RPY(R): """Converts a rotation matrix to Euler angles. Args: R: (3x3 numpy array) rotation matrix Returns: (3x1 numpy array) Euler angles in [roll, pitch, yaw] format """ threshold = 0.999999999999999 if np.abs(R[2, 0]) > threshold: sign_pitch = -np.sign(R[2, 0]) pitch = sign_pitch * np.pi/2 # Set yaw to 0 yaw = 0 roll = np.arctan2(R[0, 1] * sign_pitch, R[0, 2] * sign_pitch) return np.array((roll, pitch, yaw)) else: pitch = np.arcsin(-R[2, 0]) cos_pitch = np.cos(pitch) sin_roll = R[2, 1] / cos_pitch cos_roll = R[2, 2] / cos_pitch roll = np.pi - np.arctan2(sin_roll, cos_roll) sin_yaw = R[1, 0] / cos_pitch cos_yaw = R[0, 0] / cos_pitch yaw = np.arctan2(sin_yaw, cos_yaw) return np.array((roll,pitch,yaw)) def deriv_unit_vector(A, A_dot, A_2dot): """Returns the unit vector and it's derivatives for a given vector. Args: A: (3x1 numpy array) vector A_dot: (3x1 numpy array) first derivative of the vector A_2dot: (3x1 numpy array) second derivative of the vector Returns: q: (3x1 numpy array) unit vector of A q_dot: (3x1 numpy array) first derivative of q q_2dot: (3x1 numpy array) second derivative of q """ ensure_vector(A, 3) ensure_vector(A_dot, 3) ensure_vector(A_2dot, 3) nA = np.linalg.norm(A) if abs(np.linalg.norm(nA)) < 1.0e-9: raise ZeroDivisionError('The 2-norm of A should not be zero') nA3 = nA * nA * nA nA5 = nA3 * nA * nA A_A_dot = A.dot(A_dot) q = A / nA q_dot = A_dot / nA - A.dot(A_A_dot) / nA3 q_2dot = A_2dot / nA \ - A_dot.dot(2.0 * A_A_dot) / nA3 \ - A.dot(A_dot.dot(A_dot) + A.dot(A_2dot)) / nA3 \ + 3.0 * A.dot(A_A_dot).dot(A_A_dot) / nA5 return (q, q_dot, q_2dot) def saturate(x, x_min, x_max): """Saturate input vector between two values. Args: x: (nx1 numpy ndarray) value x_min: (float) minimum value for x x_max: (float) maximum value for x Returns: (nx1 numpy ndarray) saturated x """ for i in range(len(x)): if x[i] > x_max: x[i] = x_max elif x[i] < x_min: x[i] = x_min return x def expm_SO3(r): """Returns the matrix exponential of a matrix in SO(3). Args: r: (3x1 numpy array) vector Returns: R: (3x3 numpy array) matrix exponential of r """ ensure_vector(r, 3) theta = np.linalg.norm(r) y = sinx_over_x(theta) y2 = sinx_over_x(theta / 2.0) hat_r = hat(r) R = np.eye(3) + y * hat_r + 0.5 * y2**2 * hat_r @ hat_r return R def sinx_over_x(x): """Calculate sin(x)/x, while dealing with the cases where denominator is zero. Args: x: (float) value Returns: y: (float) value of sin(x)/x """ eps = 1e-6 if abs(x) < eps: y = - x**10 / 39916800.0 + x**8 / 362880.0 - x**6 / 5040.0 \ + x**4 / 120.0 - x**2 / 6.0 + 1.0 else: y = np.sin(x) / x return y def ensure_vector(x, n): """Make sure the given input array x is a vector of size n. Args: x: (nx1 numpy array) vector n: (int) desired length of the vector Returns: True if the input array is satisfied with size constraint. Raises an exception otherwise. """ np.atleast_2d(x) # Make sure the array is atleast 2D. if not len(np.ravel(x)) == n: raise ValueError('Input array needs to be of length {}, but the size' \ 'detected is {}'.format(n, np.shape(x))) return True def ensure_matrix(x, m, n): """Make sure the given input array x is a matrix of size mxn. Args: x: (mxn numpy array) array m: (int) desired number of rows n: (int) desired number of columns Returns: True if the input array is satisfied with size constraint. Raises an exception otherwise. """ np.atleast_2d(x) # Make sure the array is atleast 2D. if not np.shape(x) == (m, n): raise ValueError('Input array needs to be of size {} x {}, but the ' \ 'size detected is {}'.format(m, n, np.shape(x))) return True def ensure_skew(x, n): """Make sure the given input array is a skew-symmetric matrix of size nxn. Args: x: (nxn numpy array) array m: (int) desired number of rows and columns Returns: True if the input array is a skew-symmetric matrix. Raises an exception otherwise. """ ensure_matrix(x, n, n) if not np.allclose(x.T, -x): raise ValueError('Input array must be a skew-symmetric matrix') return True def ensure_SO3(x): """Make sure the given input array is in SO(3). Args: x: (3x3 numpy array) matrix Returns: True if the input array is in SO(3). Raises an exception otherwise. """ ensure_matrix(x, 3, 3) if not np.array_equal(x.T @ x, np.eye(3)): raise ValueError('Input array does not satisfy R^TR = I') if not abs(np.linalg.det(x)) - 1.0 < 1e-9: raise ValueError('Input array does not det(R) = 1') return True ================================================ FILE: src/fdcl_uav/fdcl_uav/trajectory.py ================================================ import datetime import numpy as np import rclpy from rclpy.node import Node from uav_gazebo.msg import DesiredData, ModeData, StateData class TrajectoryNode(Node): def __init__(self): Node.__init__(self, 'trajectory') self.mode = 0 self.is_mode_changed = False self.is_landed = False self.t0 = self.get_clock().now() self.t = 0.0 self.t_traj = 0.0 self.xd = np.zeros(3) self.xd_dot = np.zeros(3) self.xd_2dot = np.zeros(3) self.xd_3dot = np.zeros(3) self.xd_4dot = np.zeros(3) self.b1d = np.zeros(3) self.b1d[0] = 1.0 self.b1d_dot = np.zeros(3) self.b1d_2dot = np.zeros(3) self.x = np.zeros(3) self.v = np.zeros(3) self.a = np.zeros(3) self.R = np.identity(3) self.W = np.zeros(3) self.x_init = np.zeros(3) self.v_init = np.zeros(3) self.a_init = np.zeros(3) self.R_init = np.identity(3) self.W_init = np.zeros(3) self.b1_init = np.zeros(3) self.theta_init = 0.0 self.trajectory_started = False self.trajectory_complete = False # Manual mode self.manual_mode = False self.manual_mode_init = False self.x_offset = np.zeros(3) self.yaw_offset = 0.0 # Take-off self.takeoff_end_height = -1.0 # (m) self.takeoff_velocity = -1.0 # (m/s) # Landing self.landing_velocity = 1.0 # (m/s) self.landing_motor_cutoff_height = -0.25 # (m) # Circle self.circle_center = np.zeros(3) self.circle_linear_v = 1.0 self.circle_W = 1.2 self.circle_radius = 1.2 self.waypoint_speed = 2.0 # (m/s) self.e1 = np.array([1.0, 0.0, 0.0]) self.init_subscribers() self.init_publishers() def init_subscribers(self): self.sub_states = self.create_subscription( \ StateData, '/uav/states', self.states_callback, 1) self.sub_mode = self.create_subscription( \ ModeData, '/uav/mode', self.mode_callback, 1) def init_publishers(self): self.pub_trajectory = self.create_publisher( \ DesiredData, '/uav/trajectory', 1) def states_callback(self, msg): for i in range(3): self.x[i] = msg.position[i] self.v[i] = msg.velocity[i] self.a[i] = msg.acceleration[i] self.W[i] = msg.angular_velocity[i] for j in range(3): self.R[i, j] = msg.attitude[3*i + j] self.calculate_trajectory() self.publish_trajectory() def mode_callback(self, msg): self.x_offset = msg.x_offset self.yaw_offset = msg.yaw_offset if self.mode != msg.mode: self.mode = msg.mode self.is_mode_changed = True self.mark_traj_start() self.get_logger().info('Mode switched to {}'.format(self.mode)) def publish_trajectory(self): msg = DesiredData() for i in range(3): msg.position[i] = self.xd[i] msg.velocity[i] = self.xd_dot[i] msg.acceleration[i] = self.xd_2dot[i] msg.jerk[i] = self.xd_3dot[i] msg.snap[i] = self.xd_4dot[i] msg.b1[i] = self.b1d[i] msg.b1_dot[i] = self.b1d_dot[i] msg.b1_2dot[i] = self.b1d_2dot[i] msg.is_landed = self.is_landed self.pub_trajectory.publish(msg) def calculate_trajectory(self): if self.manual_mode: self.manual() return if self.mode == 0 or self.mode == 1: # idle and warm-up self.set_desired_states_to_zero() self.mark_traj_start() elif self.mode == 2: # take-off self.takeoff() elif self.mode == 3: # stay self.stay() elif self.mode == 4: # circle self.circle() elif self.mode == 5: # land self.land() else: self.get_logger().info('Undefined mode {}'.format(self.mode)) def mark_traj_start(self): self.trajectory_started = False self.trajectory_complete = False self.manual_mode = False self.manual_mode_init = False self.is_landed = False self.t = 0.0 self.t_traj = 0.0 self.t0 = self.get_clock().now() self.x_offset = np.zeros(3) self.yaw_offset = 0.0 self.update_initial_state() def mark_traj_end(self, switch_to_manual=False): self.trajectory_complete = True if switch_to_manual: self.manual_mode = True def set_desired_states_to_zero(self): self.xd = np.zeros(3) self.xd_dot = np.zeros(3) self.xd_2dot = np.zeros(3) self.xd_3dot = np.zeros(3) self.xd_4dot = np.zeros(3) self.b1d = np.array([1.0, 0.0, 0.0]) self.b1d_dot = np.zeros(3) self.b1d_2dot = np.zeros(3) def set_desired_states_to_current(self): self.xd = np.copy(self.x) self.xd_dot = np.copy(self.v) self.xd_2dot = np.zeros(3) self.xd_3dot = np.zeros(3) self.xd_4dot = np.zeros(3) self.b1d = self.get_current_b1() self.b1d_dot = np.zeros(3) self.b1d_2dot = np.zeros(3) def update_initial_state(self): self.x_init = np.copy(self.x) self.v_init = np.copy(self.v) self.a_init = np.copy(self.a) self.R_init = np.copy(self.R) self.W_init = np.copy(self.W) self.b1_init = self.get_current_b1() self.theta_init = np.arctan2(self.b1_init[1], self.b1_init[0]) def get_current_b1(self): b1 = self.R @ self.e1 theta = np.arctan2(b1[1], b1[0]) return np.array([np.cos(theta), np.sin(theta), 0.0]) def waypoint_reached(self, waypoint, current, radius): delta = waypoint - current if abs(np.linalg.norm(delta) < radius): return True else: return False def update_current_time(self): t_now = self.get_clock().now() self.t = float((t_now - self.t0).nanoseconds) * 1e-9 def manual(self): if not self.manual_mode_init: self.set_desired_states_to_current() self.update_initial_state() self.manual_mode_init = True self.x_offset = np.zeros(3) self.yaw_offset = 0.0 print('Switched to manual mode') self.xd = self.x_init + self.x_offset self.xd_dot = (self.xd - self.x) / 1.0 theta = self.theta_init + self.yaw_offset self.b1d = np.array([np.cos(theta), np.sin(theta), 0.0]) def takeoff(self): if not self.trajectory_started: self.set_desired_states_to_zero() # Take-off starts from the current horizontal position. self.xd = np.copy(self.x) self.xd_dot = np.zeros(3) self.xd_dot[2] = self.takeoff_velocity self.t_traj = abs((self.takeoff_end_height - self.x[2]) / self.takeoff_velocity) # Set the takeoff attitude to the current attitude. self.b1d = self.get_current_b1() self.trajectory_started = True takeoff_end_position = np.copy(self.x_init) takeoff_end_position[2] += self.takeoff_velocity * self.t_traj self.get_logger().info('Takeoff started') self.get_logger().info('Starting from ' + str(self.x)) self.get_logger().info('Takeoff time ' + str(self.t_traj)) self.get_logger().info('Reaching ' + str(takeoff_end_position)) self.get_logger().info('Takeoff velocity ' + str(self.takeoff_velocity) + ' m/s') self.update_current_time() if self.t < self.t_traj: self.xd[2] = self.x_init[2] + self.takeoff_velocity * self.t self.xd_dot[2] = self.takeoff_velocity elif not self.waypoint_reached(self.xd, self.x, 0.04): self.xd[2] = self.takeoff_end_height xd_dot_gain = 0.1 self.xd_dot = xd_dot_gain * (self.x - self.xd) else: self.xd[2] = self.takeoff_end_height self.xd_dot[2] = 0.0 if not self.trajectory_complete: self.get_logger().info('Takeoff complete') self.get_logger().info('Switching to manual mode') self.mark_traj_end(True) def land(self): if not self.trajectory_started: self.set_desired_states_to_current() self.t_traj = (self.landing_motor_cutoff_height - self.x[2]) / \ self.landing_velocity # Set the landing attitude to the current attitude. self.b1d = self.get_current_b1() self.trajectory_started = True self.update_current_time() if self.t < self.t_traj: self.xd[2] = self.x_init[2] + self.landing_velocity * self.t self.xd_2dot[2] = self.landing_velocity else: if self.x[2] > self.landing_motor_cutoff_height: self.xd[2] = self.landing_motor_cutoff_height self.xd_dot[2] = 0.0 if not self.trajectory_complete: self.get_logger().info('Landing complete') self.mark_traj_end(False) self.is_landed = True else: self.xd[2] = self.landing_motor_cutoff_height self.xd_dot[2] = self.landing_velocity def stay(self): if not self.trajectory_started: self.set_desired_states_to_current() self.trajectory_started = True self.mark_traj_end(True) def circle(self): if not self.trajectory_started: self.set_desired_states_to_current() self.trajectory_started = True self.circle_center = np.copy(self.x) self.circle_W = 2 * np.pi / 8 num_circles = 2 self.t_traj = self.circle_radius / self.circle_linear_v \ + num_circles * 2 * np.pi / self.circle_W self.update_current_time() if self.t < (self.circle_radius / self.circle_linear_v): self.xd[0] = self.circle_center[0] + self.circle_linear_v * self.t self.xd_dot[0] = self.circle_linear_v elif self.t < self.t_traj: circle_W = self.circle_W circle_radius = self.circle_radius t = self.t - circle_radius / self.circle_linear_v th = circle_W * t circle_W2 = circle_W * circle_W circle_W3 = circle_W2 * circle_W circle_W4 = circle_W3 * circle_W # axis 1 self.xd[0] = circle_radius * np.cos(th) + self.circle_center[0] self.xd_dot[0] = - circle_radius * circle_W * np.sin(th) self.xd_2dot[0] = - circle_radius * circle_W2 * np.cos(th) self.xd_3dot[0] = circle_radius * circle_W3 * np.sin(th) self.xd_4dot[0] = circle_radius * circle_W4 * np.cos(th) # axis 2 self.xd[1] = circle_radius * np.sin(th) + self.circle_center[1] self.xd_dot[1] = circle_radius * circle_W * np.cos(th) self.xd_2dot[1] = - circle_radius * circle_W2 * np.sin(th) self.xd_3dot[1] = - circle_radius * circle_W3 * np.cos(th) self.xd_4dot[1] = circle_radius * circle_W4 * np.sin(th) w_b1d = 2.0 * np.pi / 10.0 th_b1d = w_b1d * t self.b1d = np.array([np.cos(th_b1d), np.sin(th_b1d), 0]) self.b1d_dot = np.array([- w_b1d * np.sin(th_b1d), \ w_b1d * np.cos(th_b1d), 0.0]) self.b1d_2dot = np.array([- w_b1d * w_b1d * np.cos(th_b1d), w_b1d * w_b1d * np.sin(th_b1d), 0.0]) else: self.mark_traj_end(True) def main(args=None): print("Starting trajectory node") rclpy.init(args=args) trajectory = TrajectoryNode() try: rclpy.spin(trajectory) except KeyboardInterrupt: pass # Destroy the node explicitly # (optional - otherwise it will be done automatically # when the garbage collector destroys the node object) trajectory.destroy_node() print("Terminating trajectory node") ================================================ FILE: src/fdcl_uav/launch/fdcl_uav_launch.py ================================================ from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( package='fdcl_uav', namespace='estimator', executable='estimator', name='sim' ), Node( package='fdcl_uav', namespace='control', executable='control', name='sim' ), Node( package='fdcl_uav', namespace='trajectory', executable='trajectory', name='sim' ), Node( package='fdcl_uav', namespace='gui', executable='gui', name='sim' ) ]) ================================================ FILE: src/fdcl_uav/package.xml ================================================ fdcl_uav 0.0.0 FDCL UAV Simulator kani MIT rclpy std_msgs ros2launch ament_python ================================================ FILE: src/fdcl_uav/resource/fdcl_uav ================================================ ================================================ FILE: src/fdcl_uav/setup.cfg ================================================ [develop] script_dir=$base/lib/fdcl_uav [install] install_scripts=$base/lib/fdcl_uav ================================================ FILE: src/fdcl_uav/setup.py ================================================ from setuptools import find_packages, setup from glob import glob import os package_name = 'fdcl_uav' setup( name=package_name, version='0.0.0', packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))) ], install_requires=['setuptools'], zip_safe=True, maintainer='Kanishke Gamagedara', maintainer_email='kanishkegb@gwu.edu', description='FDCL UAV Simulator', license='MIT', tests_require=['pytest'], entry_points={ 'console_scripts': [ 'estimator = fdcl_uav.estimator:main', 'control = fdcl_uav.control:main', 'trajectory = fdcl_uav.trajectory:main', 'gui = fdcl_uav.gui:main', ], }, ) ================================================ FILE: src/uav_gazebo/CMakeLists.txt ================================================ cmake_minimum_required(VERSION 3.22.1) project(uav_gazebo) set(CMAKE_CXX_STANDARD 14) find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) find_package(xacro REQUIRED) xacro_add_files("urdf/uav.urdf.xacro" INSTALL DESTINATION urdf/) rosidl_generate_interfaces(${PROJECT_NAME} "msg/DesiredData.msg" "msg/ErrorData.msg" "msg/ModeData.msg" "msg/StateData.msg" DEPENDENCIES std_msgs ) install(DIRECTORY launch meshes worlds msg DESTINATION share/${PROJECT_NAME} ) install(FILES model.config DESTINATION share/${PROJECT_NAME} ) ament_package() ================================================ FILE: src/uav_gazebo/launch/uav_gazebo.launch.py ================================================ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node from scripts import GazeboRosPaths import os pkg_gazebo_ros = get_package_share_directory('gazebo_ros') pkg_uav_gazebo = get_package_share_directory('uav_gazebo') def generate_launch_description(): model_path, plugin_path, media_path = GazeboRosPaths.get_paths() # print(plugin_path) env = { "GAZEBO_MODEL_PATH": model_path, "GAZEBO_PLUGIN_PATH": plugin_path, "GAZEBO_RESOURCE_PATH": media_path, } # World world_path = os.path.join(pkg_uav_gazebo, 'worlds', 'simple.world') world_file = DeclareLaunchArgument( 'world', default_value=[world_path], description='SDF world file' ) # Gazebo gazebo = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_gazebo_ros, 'launch', 'gazebo.launch.py'), ) ) # UAV urdf_file_name = 'uav.urdf' urdf_file = os.path.join(pkg_uav_gazebo, 'urdf', urdf_file_name) spawn_uav = Node( package='gazebo_ros', executable='spawn_entity.py', arguments=['-entity', 'uav', '-file', urdf_file, \ '-x', '0', '-y', '0', '-z', '0.2'], output='screen', additional_env=env ) return LaunchDescription([ world_file, gazebo, spawn_uav ]) ================================================ FILE: src/uav_gazebo/meshes/quadrotor_base.dae ================================================ Stefan Kohlbrecher Blender 2.61.4 r43829 2012-02-03T14:20:49 2012-02-03T14:20:49 Z_UP 0 0 0 1 0.1 0.1 0.1 1 1 0 0.0106101 1 0.5 0.5 0.5 1 50 1 1 1 0.15 0.15 0.15 1 0.2 0.2 0.2 1 0.48 0.48 0.48 1 1 1 1 1 33 1 1 1 0 0 0 1 0.1 0.1 0.1 1 0.05496641 0.05496641 0.05496641 1 0.5 0.5 0.5 1 50 1 1 1 0.15 0.15 0.15 1 0.23 0.23 0.23 1 0.5 0.5 0.5 1 1 1 1 1 20 1 1 1 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 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 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

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

 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

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

 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

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

 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3

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

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