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
================================================
| | |
|-|-|
|[](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.
### 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.
### 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.
<!-- 1. The UAV will re-spawn at the position and orientation defined in `reset_uav()` in `rover.py` when you run the Python code. -->
## 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.
<!-- `export GAZEBO_PLUGIN_PATH=/home/kani/Documents/uav_simulator/build:$GAZEBO_PLUGIN_PATH` -->
================================================
FILE: docker_release.sh
================================================
docker build --tag <tag-name> -t uav_simulator .
docker tag uav_simulator kanishgama/uav_simulator:<tag-name>
docker push kanishgama/uav_simulator:<tag-name>
================================================
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
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>fdcl_uav</name>
<version>0.0.0</version>
<description>FDCL UAV Simulator</description>
<maintainer email="kanishkegb@gwu.edu">kani</maintainer>
<license>MIT</license>
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>ros2launch</exec_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>
================================================
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
================================================
<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author>Stefan Kohlbrecher</author>
<authoring_tool>Blender 2.61.4 r43829</authoring_tool>
</contributor>
<created>2012-02-03T14:20:49</created>
<modified>2012-02-03T14:20:49</modified>
<unit name="meter" meter="1"/>
<up_axis>Z_UP</up_axis>
</asset>
<library_effects>
<effect id="BodyDark_001-effect">
<profile_COMMON>
<technique sid="common">
<phong>
<emission>
<color sid="emission">0 0 0 1</color>
</emission>
<ambient>
<color sid="ambient">0.1 0.1 0.1 1</color>
</ambient>
<diffuse>
<color sid="diffuse">1 0 0.0106101 1</color>
</diffuse>
<specular>
<color sid="specular">0.5 0.5 0.5 1</color>
</specular>
<shininess>
<float sid="shininess">50</float>
</shininess>
<index_of_refraction>
<float sid="index_of_refraction">1</float>
</index_of_refraction>
</phong>
</technique>
<extra>
<technique profile="GOOGLEEARTH">
<double_sided>1</double_sided>
</technique>
</extra>
</profile_COMMON>
<extra><technique profile="MAX3D"><double_sided>1</double_sided></technique></extra>
</effect>
<effect id="RotorGrey-effect">
<profile_COMMON>
<technique sid="common">
<phong>
<emission>
<color sid="emission">0.15 0.15 0.15 1</color>
</emission>
<ambient>
<color sid="ambient">0.2 0.2 0.2 1</color>
</ambient>
<diffuse>
<color sid="diffuse">0.48 0.48 0.48 1</color>
</diffuse>
<specular>
<color sid="specular">1 1 1 1</color>
</specular>
<shininess>
<float sid="shininess">33</float>
</shininess>
<index_of_refraction>
<float sid="index_of_refraction">1</float>
</index_of_refraction>
</phong>
</technique>
<extra>
<technique profile="GOOGLEEARTH">
<double_sided>1</double_sided>
</technique>
</extra>
</profile_COMMON>
<extra><technique profile="MAX3D"><double_sided>1</double_sided></technique></extra>
</effect>
<effect id="BodyDark-effect">
<profile_COMMON>
<technique sid="common">
<phong>
<emission>
<color sid="emission">0 0 0 1</color>
</emission>
<ambient>
<color sid="ambient">0.1 0.1 0.1 1</color>
</ambient>
<diffuse>
<color sid="diffuse">0.05496641 0.05496641 0.05496641 1</color>
</diffuse>
<specular>
<color sid="specular">0.5 0.5 0.5 1</color>
</specular>
<shininess>
<float sid="shininess">50</float>
</shininess>
<index_of_refraction>
<float sid="index_of_refraction">1</float>
</index_of_refraction>
</phong>
</technique>
<extra>
<technique profile="GOOGLEEARTH">
<double_sided>1</double_sided>
</technique>
</extra>
</profile_COMMON>
<extra><technique profile="MAX3D"><double_sided>1</double_sided></technique></extra>
</effect>
<effect id="RotorGrey2-effect">
<profile_COMMON>
<technique sid="common">
<phong>
<emission>
<color sid="emission">0.15 0.15 0.15 1</color>
</emission>
<ambient>
<color sid="ambient">0.23 0.23 0.23 1</color>
</ambient>
<diffuse>
<color sid="diffuse">0.5 0.5 0.5 1</color>
</diffuse>
<specular>
<color sid="specular">1 1 1 1</color>
</specular>
<shininess>
<float sid="shininess">20</float>
</shininess>
<index_of_refraction>
<float sid="index_of_refraction">1</float>
</index_of_refraction>
</phong>
</technique>
<extra>
<technique profile="GOOGLEEARTH">
<double_sided>1</double_sided>
</technique>
</extra>
</profile_COMMON>
<extra><technique profile="MAX3D"><double_sided>1</double_sided></technique></extra>
</effect>
</library_effects>
<library_materials>
<material id="BodyDark_001-material" name="BodyDark.001">
<instance_effect url="#BodyDark_001-effect"/>
</material>
<material id="RotorGrey-material" name="RotorGrey">
<instance_effect url="#RotorGrey-effect"/>
</material>
<material id="BodyDark-material" name="BodyDark">
<instance_effect url="#BodyDark-effect"/>
</material>
<material id="RotorGrey2-material" name="RotorGrey2">
<instance_effect url="#RotorGrey2-effect"/>
</material>
</library_materials>
<library_geometries>
<geometry id="Cube_002-mesh">
<mesh>
<source id="Cube_002-mesh-positions">
<float_array id="Cube_002-mesh-positions-array" count="900">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</float_array>
<technique_common>
<accessor source="#Cube_002-mesh-positions-array" count="300" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="Cube_002-mesh-normals">
<float_array id="Cube_002-mesh-normals-array" count="1368">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</float_array>
<technique_common>
<accessor source="#Cube_002-mesh-normals-array" count="456" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="Cube_002-mesh-vertices">
<input semantic="POSITION" source="#Cube_002-mesh-positions"/>
</vertices>
<polylist material="BodyDark_0011" count="48">
<input semantic="VERTEX" source="#Cube_002-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_002-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>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</p>
</polylist>
<polylist material="RotorGrey2" count="64">
<input semantic="VERTEX" source="#Cube_002-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_002-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>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</p>
</polylist>
<polylist material="BodyDark3" count="364">
<input semantic="VERTEX" source="#Cube_002-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_002-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>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</p>
</polylist>
<polylist material="RotorGrey24" count="64">
<input semantic="VERTEX" source="#Cube_002-mesh-vertices" offset="0"/>
<input semantic="NORMAL" source="#Cube_002-mesh-normals" offset="1"/>
<vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
<p>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</p>
</polylist>
</mesh>
<extra><technique profile="MAYA"><double_sided>1</double_sided></technique></extra>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="Scene" name="Scene">
<node id="MainBody_002" type="NODE">
<translate sid="location">0 0 0</translate>
<rotate sid="rotationZ">0 0 1 0</rotate>
<rotate sid="rotationY">0 1 0 0</rotate>
<rotate sid="rotationX">1 0 0 0</rotate>
<scale sid="scale">0.1335572 0.1335572 0.1335572</scale>
<instance_geometry url="#Cube_002-mesh">
<bind_material>
<technique_common>
<instance_material symbol="BodyDark_0011" target="#BodyDark_001-material"/>
<instance_material symbol="RotorGrey2" target="#RotorGrey-material"/>
<instance_material symbol="BodyDark3" target="#BodyDark-material"/>
<instance_material symbol="RotorGrey24" target="#RotorGrey2-material"/>
</technique_common>
</bind_material>
</instance_geometry>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#Scene"/>
</scene>
</COLLADA>
================================================
FILE: src/uav_gazebo/model.config
================================================
<?xml version="1.0"?>
<model>
<name>uav</name>
<version>1.0</version>
<sdf>urdf/uav.urdf</sdf>
<author>
<name>Kanish Gamagedara</name>
<email>kanishkegb@gwu.edu</email>
</author>
<description>
The UAV model
</description>
</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
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>uav_gazebo</name>
<version>0.0.0</version>
<description>FDCL-UAV Gazebo Simulation</description>
<maintainer email="kanishkegb@gwu.edu">Kanish</maintainer>
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<build_depend>xacro</build_depend>
<exec_depend>uav_control_plugin</exec_depend>
<export>
<build_type>ament_cmake</build_type>
<gazebo_ros gazebo_model_path="${prefix}/.." />
</export>
</package>
================================================
FILE: src/uav_gazebo/urdf/uav.urdf.xacro
================================================
<?xml version="1.0"?>
<robot name="uav" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- <xacro:include filename="$(find uav_gazebo)/urdf/uav.gazebo" /> -->
<link name="link">
<pose>0 0 0.2 0 0 0</pose>
<inertial>
<mass value="1.95"/>
<inertia ixx="0.02" ixy="0" ixz="0" iyy="0.02" iyz="0" izz="0.04"/>
</inertial>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://uav_gazebo/meshes/quadrotor_base.dae"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://uav_gazebo/meshes/quadrotor_base.dae"/>
</geometry>
</visual>
</link>
<gazebo reference="link">
<material>Gazebo/Orange</material>
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<pose>0 0 0 0 0 0</pose>
<always_on>true</always_on>
<update_rate>100</update_rate>
<visualize>true</visualize>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
<ros>
<remapping>~/out:=uav/imu</remapping>
</ros>
<pose>0 0 0 0 0 0</pose>
<body_name>link</body_name>
<update_rate>100.0</update_rate>
<gaussian_noise>0.0001</gaussian_noise>
</plugin>
</sensor>
</gazebo>
<gazebo>
<plugin name="pose_3d_plugin" filename="libgazebo_ros_p3d.so">
<ros>
<remapping>odom:=uav/gps</remapping>
</ros>
<body_name>link</body_name>
<frame_name>world</frame_name>
<gaussian_noise>0.01</gaussian_noise>
<update_rate>10</update_rate>
</plugin>
</gazebo>
<gazebo>
<plugin name="uav_control_plugin" filename="libuav_control_plugin.so">
<body_name>link</body_name>
<topic_name>/uav/fm</topic_name>
</plugin>
</gazebo>
</robot>
================================================
FILE: src/uav_gazebo/worlds/simple.world
================================================
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<plugin name="gazebo_ros_state" filename="libgazebo_ros_state.so">
<ros>
<namespace>/gazebo</namespace>
</ros>
<update_rate>1.0</update_rate>
</plugin>
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
</world>
</sdf>
================================================
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}
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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<double, 2, 1> Vector2;
typedef Eigen::Matrix<double, 3, 1> Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 3, 3> 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 <iostream>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#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 <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>
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
================================================
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>uav_control_plugin</name>
<version>0.0.0</version>
<description>The control plugin package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="kanishkegb@gwu.edu">Kanish</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>MIT</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- <build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<build_depend>geometry_msgs</build_depend>
<exec_depend>geometry_msgs</exec_depend> -->
<exec_depend>launch</exec_depend>
<exec_depend>ros2run</exec_depend>
<!-- <exec_depend>uav_gazebo</exec_depend> -->
<depend>geometry_msgs</depend>
<depend>gazebo_ros</depend>
<depend>rclcpp</depend>
<export>
<build_type>ament_cmake</build_type>
<gazebo_ros gazebo_model_path="${prefix}/.." />
<gazebo_ros plugin_path="${prefix}" />
</export>
</package>
================================================
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<Eigen::MatrixXd> 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 <functional>
// #include <gazebo/gazebo.hh>
// #include <gazebo/physics/physics.hh>
// #include <gazebo/common/common.hh>
// #include <boost/thread.hpp>
// #include <boost/thread/mutex.hpp>
// // #include <ros/callback_queue.h>
// // #include <ros/subscribe_options.h>
#include <ignition/math.hh>
#include <ignition/math/Vector3.hh>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/wrench_stamped.hpp>
// #include <ros/ros.h>
// #include "std_msgs/String.h"
#include <gazebo/physics/physics.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo_ros/node.hpp>
#include <rclcpp/rclcpp.hpp>
#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<std::string>();
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<std::string>();
sub_fm_ = rn_->create_subscription<geometry_msgs::msg::WrenchStamped>( \
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<geometry_msgs::msg::WrenchStamped>::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()
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
SYMBOL INDEX (177 symbols across 16 files)
FILE: scripts/plot_utils.py
function plot_data (line 9) | def plot_data():
function get_file_name (line 21) | def get_file_name():
function plot_31_1 (line 26) | def plot_31_1(x, y, x_label='', y_label=['1', '2', '3'], title=''):
function plot_31_2 (line 37) | def plot_31_2(x1, y1, x2, y2, x_label='', y_label=['', '', ''], title='',
function plot_33_1 (line 54) | def plot_33_1(x, y, x_label='', title=''):
function plot_33_2 (line 65) | def plot_33_2(x1, y1, x2, y2, x_label='', title='', legend=['', '']):
class Data (line 80) | class Data:
method __init__ (line 81) | def __init__(self, file_name):
method get_data_3x1 (line 100) | def get_data_3x1(self, var_name):
method rot_mat_from_df (line 106) | def rot_mat_from_df(self, mat, N):
FILE: scripts/thread_log.py
function thread_log (line 8) | def thread_log():
function write_header (line 55) | def write_header(file_name):
function write_date (line 79) | def write_date(f, t_millis):
function string_vector (line 99) | def string_vector(name, length=3):
function string_3x3 (line 106) | def string_3x3(name):
function write_scalar (line 114) | def write_scalar(f, data):
function write_vector (line 118) | def write_vector(f, data, length=3):
function write_3x3 (line 125) | def write_3x3(f, data):
FILE: src/fdcl_uav/fdcl_uav/control.py
class ControlNode (line 16) | class ControlNode(Node):
method __init__ (line 111) | def __init__(self):
method control_run (line 224) | def control_run(self):
method position_control (line 264) | def position_control(self):
method attitude_control (line 396) | def attitude_control(self):
method set_integral_errors_to_zero (line 489) | def set_integral_errors_to_zero(self):
method update_current_time (line 499) | def update_current_time(self):
method init_publishers (line 508) | def init_publishers(self):
method init_subscribers (line 517) | def init_subscribers(self):
method mode_callback (line 544) | def mode_callback(self, msg):
method states_callback (line 552) | def states_callback(self, msg):
method trajectory_callback (line 564) | def trajectory_callback(self, msg):
method motors_on_callback (line 585) | def motors_on_callback(self, msg):
method publish_desired (line 594) | def publish_desired(self):
method publish_errors (line 631) | def publish_errors(self):
function main (line 649) | def main(args=None):
FILE: src/fdcl_uav/fdcl_uav/estimator.py
class EstimatorNode (line 18) | class EstimatorNode(Node):
method __init__ (line 59) | def __init__(self):
method prediction (line 129) | def prediction(self, a_imu, W_imu):
method imu_correction (line 184) | def imu_correction(self, R_imu, V_R_imu):
method gps_correction (line 215) | def gps_correction(self, x_gps, v_gps, V_x_gps, V_v_gps):
method get_dt (line 255) | def get_dt(self):
method get_states (line 269) | def get_states(self):
method init_subscribers (line 282) | def init_subscribers(self):
method imu_callback (line 296) | def imu_callback(self, msg):
method gps_callback (line 326) | def gps_callback(self, msg):
method init_publishers (line 346) | def init_publishers(self):
method publish_states (line 350) | def publish_states(self):
function main (line 365) | def main(args=None):
FILE: src/fdcl_uav/fdcl_uav/freq_utils.py
class Freq (line 3) | class Freq():
method __init__ (line 4) | def __init__(self, name, freq):
method update (line 11) | def update(self, t_now):
method get_freq (line 24) | def get_freq(self):
method get_freq_str (line 28) | def get_freq_str(self):
FILE: src/fdcl_uav/fdcl_uav/gui.py
class GuiNode (line 22) | class GuiNode(Node, QMainWindow):
method __init__ (line 23) | def __init__(self):
method init_gui (line 91) | def init_gui(self):
method init_subscribers (line 222) | def init_subscribers(self):
method init_publishers (line 261) | def init_publishers(self):
method update (line 266) | def update(self):
method init_vbox (line 309) | def init_vbox(self, box, name, num):
method init_attitude_vbox (line 326) | def init_attitude_vbox(self, box, name):
method update_vbox (line 349) | def update_vbox(self, labels, data, data_size=3):
method update_attitude_vbox (line 357) | def update_attitude_vbox(self, labels, data):
method on_btn_close_clicked (line 363) | def on_btn_close_clicked(self):
method on_btn_motor_on_clicked (line 378) | def on_btn_motor_on_clicked(self):
method on_flight_mode_changed (line 394) | def on_flight_mode_changed(self):
method on_key_press (line 410) | def on_key_press(self, event):
method get_t_now (line 446) | def get_t_now(self):
method states_callback (line 452) | def states_callback(self, msg):
method desired_callback (line 471) | def desired_callback(self, msg):
method errors_callback (line 488) | def errors_callback(self, msg):
method imu_callback (line 504) | def imu_callback(self, msg):
method gps_callback (line 541) | def gps_callback(self, msg):
method fM_callback (line 558) | def fM_callback(self, msg):
function main (line 568) | def main(args=None):
FILE: src/fdcl_uav/fdcl_uav/integral_utils.py
class IntegralErrorVec3 (line 4) | class IntegralErrorVec3:
method __init__ (line 5) | def __init__(self):
method integrate (line 10) | def integrate(self, current_integrand, dt):
method set_zero (line 14) | def set_zero(self):
class IntegralError (line 19) | class IntegralError:
method __init__ (line 20) | def __init__(self):
method integrate (line 25) | def integrate(self, current_integrand, dt):
method set_zero (line 29) | def set_zero(self):
FILE: src/fdcl_uav/fdcl_uav/matrix_utils.py
function hat (line 5) | def hat(x):
function vee (line 25) | def vee(x):
function q_to_R (line 38) | def q_to_R(q):
function R_to_RPY (line 61) | def R_to_RPY(R):
function deriv_unit_vector (line 97) | def deriv_unit_vector(A, A_dot, A_2dot):
function saturate (line 136) | def saturate(x, x_min, x_max):
function expm_SO3 (line 157) | def expm_SO3(r):
function sinx_over_x (line 180) | def sinx_over_x(x):
function ensure_vector (line 201) | def ensure_vector(x, n):
function ensure_matrix (line 222) | def ensure_matrix(x, m, n):
function ensure_skew (line 244) | def ensure_skew(x, n):
function ensure_SO3 (line 263) | def ensure_SO3(x):
FILE: src/fdcl_uav/fdcl_uav/trajectory.py
class TrajectoryNode (line 9) | class TrajectoryNode(Node):
method __init__ (line 11) | def __init__(self):
method init_subscribers (line 78) | def init_subscribers(self):
method init_publishers (line 93) | def init_publishers(self):
method states_callback (line 101) | def states_callback(self, msg):
method mode_callback (line 116) | def mode_callback(self, msg):
method publish_trajectory (line 128) | def publish_trajectory(self):
method calculate_trajectory (line 148) | def calculate_trajectory(self):
method mark_traj_start (line 168) | def mark_traj_start(self):
method mark_traj_end (line 186) | def mark_traj_end(self, switch_to_manual=False):
method set_desired_states_to_zero (line 193) | def set_desired_states_to_zero(self):
method set_desired_states_to_current (line 205) | def set_desired_states_to_current(self):
method update_initial_state (line 217) | def update_initial_state(self):
method get_current_b1 (line 228) | def get_current_b1(self):
method waypoint_reached (line 234) | def waypoint_reached(self, waypoint, current, radius):
method update_current_time (line 243) | def update_current_time(self):
method manual (line 248) | def manual(self):
method takeoff (line 266) | def takeoff(self):
method land (line 314) | def land(self):
method stay (line 346) | def stay(self):
method circle (line 354) | def circle(self):
function main (line 409) | def main(args=None):
FILE: src/fdcl_uav/launch/fdcl_uav_launch.py
function generate_launch_description (line 4) | def generate_launch_description():
FILE: src/uav_gazebo/launch/uav_gazebo.launch.py
function generate_launch_description (line 16) | def generate_launch_description():
FILE: src/uav_plugins/include/fdcl/common_types.hpp
type fdcl (line 6) | namespace fdcl
FILE: src/uav_plugins/include/fdcl/matrix_utils.hpp
type fdcl (line 16) | namespace fdcl
FILE: src/uav_plugins/include/fdcl/ros_utils.hpp
type fdcl (line 12) | namespace fdcl
FILE: src/uav_plugins/src/uav_control_plugin.cpp
type gazebo (line 35) | namespace gazebo
class UavControlPlugin (line 38) | class UavControlPlugin : public ModelPlugin
method Load (line 42) | void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf)
method update (line 68) | void update(void)
method calculate_force (line 91) | void calculate_force(void)
method update_uav_rotation (line 109) | void update_uav_rotation(void)
method update_fm (line 122) | void update_fm(const geometry_msgs::msg::WrenchStamped &msg) const
method reset_uav (line 137) | void reset_uav(void)
method ignition_to_eigen (line 144) | void ignition_to_eigen(
method eigen_to_ignition (line 154) | void eigen_to_ignition(
FILE: tests/test_matrix_utils.py
class TestMatrixUtils (line 8) | class TestMatrixUtils(unittest.TestCase):
method setUp (line 10) | def setUp(self):
method test_ensure_vector (line 44) | def test_ensure_vector(self):
method test_ensure_vector_wrong_input_length (line 47) | def test_ensure_vector_wrong_input_length(self):
method test_ensure_vector_matrix_input (line 50) | def test_ensure_vector_matrix_input(self):
method test_ensure_vector_3d_matrix_input (line 53) | def test_ensure_vector_3d_matrix_input(self):
method test_ensure_vector_list_input (line 56) | def test_ensure_vector_list_input(self):
method test_ensure_matrix (line 60) | def test_ensure_matrix(self):
method test_ensure_matrix_2d_input (line 63) | def test_ensure_matrix_2d_input(self):
method test_ensure_matrix_3d_input (line 66) | def test_ensure_matrix_3d_input(self):
method test_ensure_skew (line 70) | def test_ensure_skew(self):
method test_ensure_skew_nonskew_input (line 73) | def test_ensure_skew_nonskew_input(self):
method test_ensure_skew_3d_matrix (line 76) | def test_ensure_skew_3d_matrix(self):
method test_ensure_SO3 (line 80) | def test_ensure_SO3(self):
method test_ensure_SO3_eye (line 83) | def test_ensure_SO3_eye(self):
method test_ensure_SO3_wrong_R (line 86) | def test_ensure_SO3_wrong_R(self):
method test_ensure_SO3_array_input (line 91) | def test_ensure_SO3_array_input(self):
method test_ensure_SO3_3d_matrix_input (line 94) | def test_ensure_SO3_3d_matrix_input(self):
method test_hat (line 98) | def test_hat(self):
method test_hat_short_input (line 101) | def test_hat_short_input(self):
method test_hat_long_input (line 104) | def test_hat_long_input(self):
method test_vee (line 108) | def test_vee(self):
method test_vee_non_skew (line 111) | def test_vee_non_skew(self):
method test_q_to_R_q1 (line 115) | def test_q_to_R_q1(self):
method test_q_to_R_q2 (line 118) | def test_q_to_R_q2(self):
method test_q_to_R_short_array (line 121) | def test_q_to_R_short_array(self):
method test_q_to_R_long_array (line 124) | def test_q_to_R_long_array(self):
method test_q_to_R_wrong_q (line 127) | def test_q_to_R_wrong_q(self):
method test_deriv_unit_vector (line 132) | def test_deriv_unit_vector(self):
method test_deriv_unit_vector_non_zero (line 141) | def test_deriv_unit_vector_non_zero(self):
method test_deriv_unit_vector_zero_norm_A (line 157) | def test_deriv_unit_vector_zero_norm_A(self):
method test_deriv_unit_vector_short_A (line 161) | def test_deriv_unit_vector_short_A(self):
method test_deriv_unit_vector_long_A (line 165) | def test_deriv_unit_vector_long_A(self):
method test_deriv_unit_vector_short_A_dot (line 169) | def test_deriv_unit_vector_short_A_dot(self):
method test_deriv_unit_vector_long_A_dot (line 173) | def test_deriv_unit_vector_long_A_dot(self):
method test_deriv_unit_vector_short_A_2dot (line 177) | def test_deriv_unit_vector_short_A_2dot(self):
method test_deriv_unit_vector_long_A_2dot (line 181) | def test_deriv_unit_vector_long_A_2dot(self):
method test_saturate (line 186) | def test_saturate(self):
method test_expm_SO3 (line 191) | def test_expm_SO3(self):
method test_expm_SO3_zero_inputs (line 202) | def test_expm_SO3_zero_inputs(self):
method test_expm_SO3_nonzero_inputs (line 205) | def test_expm_SO3_nonzero_inputs(self):
method test_expm_SO3_short_input_array (line 216) | def test_expm_SO3_short_input_array(self):
method test_expm_SO3_long_input_array (line 219) | def test_expm_SO3_long_input_array(self):
method test_sinx_over_x (line 223) | def test_sinx_over_x(self):
method test_sinx_over_x_pi (line 226) | def test_sinx_over_x_pi(self):
method test_sinx_over_x_zero (line 229) | def test_sinx_over_x_zero(self):
Condensed preview — 47 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (183K chars).
[
{
"path": ".catkin_workspace",
"chars": 98,
"preview": "# This file currently only serves to mark the location of a catkin workspace for tool integration\n"
},
{
"path": ".gitattributes",
"chars": 2856,
"preview": "# LaTeX\n*.tex text eol=lf\n*.cls text eol=lf\n*.sty text eol=lf\n*.bst text eol=lf\n*.bib text eol=lf\n\n#sources\n*.c text eol"
},
{
"path": ".gitignore",
"chars": 5946,
"preview": ".vscode/\n\n# Created by https://www.toptal.com/developers/gitignore/api/c++,jupyternotebooks,python,linux,windows,macos,v"
},
{
"path": ".gitmodules",
"chars": 99,
"preview": "[submodule \"libraries/eigen\"]\n\tpath = libraries/eigen\n\turl = https://github.com/fdcl-gwu/eigen.git\n"
},
{
"path": ".travis.yml",
"chars": 171,
"preview": "language: python\npython:\n - \"3.9\"\n\n# command to install dependencies\ninstall:\n - pip install -r requirements.txt\n\n# co"
},
{
"path": "Dockerfile",
"chars": 406,
"preview": "FROM ros:iron-ros-core-jammy\nRUN apt update\nRUN apt -y install python3-pip tmux\nRUN apt -y install python3-colcon-common"
},
{
"path": "License",
"chars": 1087,
"preview": "MIT License\n\nCopyright (c) 2020 Flight Dynamics and Control Lab\n\nPermission is hereby granted, free of charge, to any pe"
},
{
"path": "README.md",
"chars": 9666,
"preview": "| | !"
},
{
"path": "docker_release.sh",
"chars": 158,
"preview": "docker build --tag <tag-name> -t uav_simulator .\ndocker tag uav_simulator kanishgama/uav_simulator:<tag-name>\ndocker pu"
},
{
"path": "docker_run.sh",
"chars": 618,
"preview": "echo \"Starting docker image ..\"\n\nif [ \"$(docker ps -a -q -f name=uav-simulator-container)\" ]; \nthen\n echo \"Previously"
},
{
"path": "scripts/data_logs/.gitignore",
"chars": 5,
"preview": "*.txt"
},
{
"path": "scripts/plot_utils.py",
"chars": 3322,
"preview": "import itertools\nimport matplotlib.pyplot as plt\nimport numpy as np\nimport pandas as pd\nimport pdb\n\n# plt.style.use('sea"
},
{
"path": "scripts/thread_log.py",
"chars": 3046,
"preview": "from rover import rover\n\nimport datetime\nimport numpy as np\nimport rospy\n\n\ndef thread_log():\n print('LOG: thread star"
},
{
"path": "src/fdcl_uav/fdcl_uav/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "src/fdcl_uav/fdcl_uav/control.py",
"chars": 20441,
"preview": "from .matrix_utils import hat, vee, deriv_unit_vector, saturate\nfrom .integral_utils import IntegralError, IntegralError"
},
{
"path": "src/fdcl_uav/fdcl_uav/estimator.py",
"chars": 11045,
"preview": "from .matrix_utils import hat, vee, expm_SO3, q_to_R\n\nimport rclpy\nfrom rclpy.node import Node\n\nimport datetime\nimport n"
},
{
"path": "src/fdcl_uav/fdcl_uav/freq_utils.py",
"chars": 592,
"preview": "\n\nclass Freq():\n def __init__(self, name, freq):\n self.name = name\n self.t_prev = 0.0\n self.num_"
},
{
"path": "src/fdcl_uav/fdcl_uav/gui.py",
"chars": 18778,
"preview": "from .matrix_utils import q_to_R, R_to_RPY\nfrom .freq_utils import Freq\n\nimport numpy as np\nimport os\n\nfrom PyQt5.QtCore"
},
{
"path": "src/fdcl_uav/fdcl_uav/integral_utils.py",
"chars": 740,
"preview": "import numpy as np\n\n\nclass IntegralErrorVec3:\n def __init__(self):\n self.error = np.zeros(3)\n self.inte"
},
{
"path": "src/fdcl_uav/fdcl_uav/matrix_utils.py",
"chars": 6516,
"preview": "import numpy as np\n# import pdb\n\n\ndef hat(x):\n \"\"\"Returns the hat map of a given 3x1 vector.\n\n Args:\n x: (3"
},
{
"path": "src/fdcl_uav/fdcl_uav/trajectory.py",
"chars": 12566,
"preview": "import datetime\nimport numpy as np\n\nimport rclpy\nfrom rclpy.node import Node\n\nfrom uav_gazebo.msg import DesiredData, Mo"
},
{
"path": "src/fdcl_uav/launch/fdcl_uav_launch.py",
"chars": 735,
"preview": "from launch import LaunchDescription\nfrom launch_ros.actions import Node\n\ndef generate_launch_description():\n return "
},
{
"path": "src/fdcl_uav/package.xml",
"chars": 538,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "src/fdcl_uav/resource/fdcl_uav",
"chars": 0,
"preview": ""
},
{
"path": "src/fdcl_uav/setup.cfg",
"chars": 85,
"preview": "[develop]\nscript_dir=$base/lib/fdcl_uav\n[install]\ninstall_scripts=$base/lib/fdcl_uav\n"
},
{
"path": "src/fdcl_uav/setup.py",
"chars": 965,
"preview": "from setuptools import find_packages, setup\nfrom glob import glob\nimport os\n\npackage_name = 'fdcl_uav'\n\nsetup(\n name="
},
{
"path": "src/uav_gazebo/CMakeLists.txt",
"chars": 623,
"preview": "cmake_minimum_required(VERSION 3.22.1)\nproject(uav_gazebo)\n\nset(CMAKE_CXX_STANDARD 14)\n\nfind_package(ament_cmake REQUIRE"
},
{
"path": "src/uav_gazebo/launch/uav_gazebo.launch.py",
"chars": 1628,
"preview": "from ament_index_python.packages import get_package_share_directory\n\nfrom launch import LaunchDescription\nfrom launch.ac"
},
{
"path": "src/uav_gazebo/meshes/quadrotor_base.dae",
"chars": 42077,
"preview": "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n<COLLADA xmlns=\"http://www.collada.org/2005/11/COLLADASchema\" version=\"1.4.1\">\n "
},
{
"path": "src/uav_gazebo/model.config",
"chars": 260,
"preview": "<?xml version=\"1.0\"?>\n\n<model>\n <name>uav</name>\n <version>1.0</version>\n <sdf>urdf/uav.urdf</sdf>\n\n <author>\n <n"
},
{
"path": "src/uav_gazebo/msg/DesiredData.msg",
"chars": 416,
"preview": "std_msgs/Header header\n\nfloat64[3] position\nfloat64[3] velocity\nfloat64[3] acceleration\nfloat64[3] jerk\nfloat64[3] snap\n"
},
{
"path": "src/uav_gazebo/msg/ErrorData.msg",
"chars": 169,
"preview": "std_msgs/Header header\n\nfloat64[3] position\nfloat64[3] velocity\nfloat64[3] attitude\nfloat64[3] angular_velocity\nfloat64["
},
{
"path": "src/uav_gazebo/msg/ModeData.msg",
"chars": 73,
"preview": "std_msgs/Header header\n\nint32 mode\nfloat64[3] x_offset\nfloat64 yaw_offset"
},
{
"path": "src/uav_gazebo/msg/StateData.msg",
"chars": 135,
"preview": "std_msgs/Header header\n\nfloat64[3] position\nfloat64[3] velocity\nfloat64[3] acceleration\nfloat64[9] attitude\nfloat64[3] a"
},
{
"path": "src/uav_gazebo/package.xml",
"chars": 809,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format3.xsd\" schematypens=\"http://www.w3."
},
{
"path": "src/uav_gazebo/urdf/uav.urdf.xacro",
"chars": 1876,
"preview": "<?xml version=\"1.0\"?>\n\n<robot name=\"uav\" xmlns:xacro=\"http://www.ros.org/wiki/xacro\">\n <!-- <xacro:include filename=\"$("
},
{
"path": "src/uav_gazebo/worlds/simple.world",
"chars": 406,
"preview": "<?xml version=\"1.0\" ?>\n<sdf version=\"1.4\">\n <world name=\"default\">\n \n <plugin name=\"gazebo_ros_state\" filename=\"l"
},
{
"path": "src/uav_plugins/CMakeLists.txt",
"chars": 807,
"preview": "cmake_minimum_required(VERSION 3.22.1)\nproject(uav_control_plugin)\n\nset(CMAKE_CXX_STANDARD 14)\n\nfind_package(ament_cmake"
},
{
"path": "src/uav_plugins/include/fdcl/common_types.hpp",
"chars": 357,
"preview": "#ifndef FDCL_COMMON_TYPES\n#define FDCL_COMMON_TYPES\n\n#include \"Eigen/Dense\"\n\nnamespace fdcl\n{\n\ntypedef Eigen::Matrix<dou"
},
{
"path": "src/uav_plugins/include/fdcl/matrix_utils.hpp",
"chars": 4373,
"preview": "/** \\file matrix_utils.hpp\n* \\brief Miscellaneous math functions\n*\n* Miscellaneous math functions used in FDCL are def"
},
{
"path": "src/uav_plugins/include/fdcl/ros_utils.hpp",
"chars": 488,
"preview": "#ifndef FDCL_ROS_UTILS_HPP\n#define FDCL_ROS_UTILS_HPP\n\n\n#include \"fdcl/common_types.hpp\"\n\n\n#include \"Eigen/Dense\"\n#inclu"
},
{
"path": "src/uav_plugins/package.xml",
"chars": 1368,
"preview": "<?xml version=\"1.0\"?>\n<?xml-model href=\"http://download.ros.org/schema/package_format2.xsd\" schematypens=\"http://www.w3."
},
{
"path": "src/uav_plugins/src/fdcl_matrix_utils.cpp",
"chars": 4541,
"preview": "#include \"fdcl/matrix_utils.hpp\"\n\n\nfdcl::Matrix3 fdcl::hat(const Vector3 v)\n{\n Matrix3 V;\n V.setZero();\n\n V(2,1"
},
{
"path": "src/uav_plugins/src/fdcl_ros_utils.cpp",
"chars": 705,
"preview": "#include \"fdcl/ros_utils.hpp\"\n#include \"fdcl/matrix_utils.hpp\"\n\n\nvoid fdcl::eigen_to_ignition(\n const Vector3 input, "
},
{
"path": "src/uav_plugins/src/uav_control_plugin.cpp",
"chars": 4957,
"preview": "// #include <functional>\n// #include <gazebo/gazebo.hh>\n// #include <gazebo/physics/physics.hh>\n// #include <gazebo/comm"
},
{
"path": "tests/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "tests/test_matrix_utils.py",
"chars": 7760,
"preview": "import imp\nimport numpy as np\nimport unittest\n\nfrom scripts.matrix_utils import *\n\n\nclass TestMatrixUtils(unittest.TestC"
}
]
About this extraction
This page contains the full source code of the fdcl-gwu/uav_simulator GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 47 files (170.2 KB), approximately 61.2k tokens, and a symbol index with 177 extracted functions, classes, methods, constants, and types. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.