Showing preview only (655K chars total). Download the full file or copy to clipboard to get everything.
Repository: uzh-rpg/deep_drone_acrobatics
Branch: master
Commit: c533a6ea9f35
Files: 174
Total size: 608.2 KB
Directory structure:
gitextract_nr_44o_q/
├── .clang-format
├── .gitignore
├── LICENSE
├── README.md
├── acrobatic_trajectory_helper/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── acrobatic_trajectory_helper/
│ │ ├── acrobatic_sequence.h
│ │ ├── circle_trajectory_helper.h
│ │ ├── heading_trajectory_helper.h
│ │ └── polynomial_trajectory_helper.h
│ ├── package.xml
│ └── src/
│ ├── acrobatic_sequence.cpp
│ ├── circle_trajectory_helper.cpp
│ ├── heading_trajectory_helper.cpp
│ └── polynomial_trajectory_helper.cpp
├── conda_requirements.txt
├── controller_learning/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── ckpt/
│ │ └── loop/
│ │ ├── fts_and_imu/
│ │ │ ├── ckpt-7.data-00000-of-00002
│ │ │ ├── ckpt-7.data-00001-of-00002
│ │ │ └── ckpt-7.index
│ │ └── imu_only/
│ │ ├── ckpt-37.data-00000-of-00002
│ │ ├── ckpt-37.data-00001-of-00002
│ │ └── ckpt-37.index
│ ├── common.py
│ ├── config/
│ │ ├── dagger_settings.yaml
│ │ ├── settings.py
│ │ ├── test_settings.yaml
│ │ └── train_settings.yaml
│ ├── data/
│ │ └── .gitignore
│ ├── iterative_learning_trajectories.py
│ ├── launch/
│ │ └── simulation/
│ │ ├── base_quad_simulation_no_controller.launch
│ │ ├── controller.launch
│ │ ├── custom_rotors_interface.launch
│ │ ├── minisim.launch
│ │ ├── quadrotor_empty_world_no_controller.launch
│ │ ├── quadrotor_rgb_world_no_controller.launch
│ │ ├── rpg_mpc.launch
│ │ ├── rpg_pid.launch
│ │ ├── rpg_rotors_interface.launch
│ │ ├── rviz_only.launch
│ │ └── simulation.launch
│ ├── package.xml
│ ├── results/
│ │ └── .gitignore
│ ├── setup.py
│ ├── src/
│ │ └── ControllerLearning/
│ │ ├── TrajectoryBase.py
│ │ ├── TrajectoryLearning.py
│ │ ├── __init__.py
│ │ └── models/
│ │ ├── __init__.py
│ │ ├── body_dataset.py
│ │ ├── bodyrate_learner.py
│ │ ├── nets.py
│ │ └── tf_addons_normalizations.py
│ ├── test_trajectories.py
│ ├── train.py
│ └── train_logs/
│ ├── 20191015-164750/
│ │ └── train/
│ │ ├── checkpoint
│ │ ├── ckpt-78.data-00000-of-00002
│ │ ├── ckpt-78.data-00001-of-00002
│ │ ├── ckpt-78.index
│ │ ├── ckpt-79.data-00000-of-00002
│ │ ├── ckpt-79.data-00001-of-00002
│ │ ├── ckpt-79.index
│ │ ├── ckpt-80.data-00000-of-00002
│ │ ├── ckpt-80.data-00001-of-00002
│ │ ├── ckpt-80.index
│ │ └── events.out.tfevents.1571150870.elia-ThinkPad-P51.9892.82.v2
│ ├── 20191022-082718/
│ │ └── train/
│ │ ├── checkpoint
│ │ ├── ckpt-1078.data-00000-of-00002
│ │ ├── ckpt-1078.data-00001-of-00002
│ │ ├── ckpt-1078.index
│ │ ├── ckpt-1079.data-00000-of-00002
│ │ ├── ckpt-1079.data-00001-of-00002
│ │ ├── ckpt-1079.index
│ │ ├── ckpt-1080.data-00000-of-00002
│ │ ├── ckpt-1080.data-00001-of-00002
│ │ ├── ckpt-1080.index
│ │ └── events.out.tfevents.1571725638.elia-ThinkPad-P51.29578.82.v2
│ ├── 20191022-095948/
│ │ └── train/
│ │ ├── checkpoint
│ │ ├── ckpt-1000.data-00000-of-00002
│ │ ├── ckpt-1000.data-00001-of-00002
│ │ ├── ckpt-1000.index
│ │ ├── ckpt-998.data-00000-of-00002
│ │ ├── ckpt-998.data-00001-of-00002
│ │ ├── ckpt-998.index
│ │ ├── ckpt-999.data-00000-of-00002
│ │ ├── ckpt-999.data-00001-of-00002
│ │ ├── ckpt-999.index
│ │ └── events.out.tfevents.1571731188.elia-ThinkPad-P51.5931.82.v2
│ ├── 20191022-122145/
│ │ └── train/
│ │ └── events.out.tfevents.1571739705.elia-ThinkPad-P51.15206.54.v2
│ ├── 20191022-122316/
│ │ └── train/
│ │ └── events.out.tfevents.1571739796.elia-ThinkPad-P51.16327.54.v2
│ ├── 20191022-122551/
│ │ └── train/
│ │ └── events.out.tfevents.1571739951.elia-ThinkPad-P51.16978.54.v2
│ ├── 20191022-122626/
│ │ └── train/
│ │ └── events.out.tfevents.1571739986.elia-ThinkPad-P51.17446.54.v2
│ ├── 20191022-122842/
│ │ └── train/
│ │ └── events.out.tfevents.1571740122.elia-ThinkPad-P51.17988.54.v2
│ ├── 20191022-123011/
│ │ └── train/
│ │ └── events.out.tfevents.1571740211.elia-ThinkPad-P51.18467.54.v2
│ ├── 20191022-134158/
│ │ └── train/
│ │ └── events.out.tfevents.1571744518.elia-ThinkPad-P51.27889.54.v2
│ ├── 20191022-135227/
│ │ └── train/
│ │ └── events.out.tfevents.1571745147.elia-ThinkPad-P51.31014.54.v2
│ ├── 20191022-135514/
│ │ └── train/
│ │ └── events.out.tfevents.1571745314.elia-ThinkPad-P51.32244.54.v2
│ ├── 20191023-115641/
│ │ └── train/
│ │ └── events.out.tfevents.1571824601.elia-ThinkPad-P51.12466.54.v2
│ ├── 20191023-115844/
│ │ └── train/
│ │ └── events.out.tfevents.1571824724.elia-ThinkPad-P51.13447.54.v2
│ ├── 20191023-120327/
│ │ └── train/
│ │ └── events.out.tfevents.1571825007.elia-ThinkPad-P51.15260.54.v2
│ ├── 20191023-120523/
│ │ └── train/
│ │ └── events.out.tfevents.1571825123.elia-ThinkPad-P51.16206.54.v2
│ ├── 20191023-120641/
│ │ └── train/
│ │ └── events.out.tfevents.1571825201.elia-ThinkPad-P51.17007.54.v2
│ ├── 20191023-121605/
│ │ └── train/
│ │ └── events.out.tfevents.1571825765.elia-ThinkPad-P51.19388.54.v2
│ ├── 20191023-122039/
│ │ └── train/
│ │ └── events.out.tfevents.1571826039.elia-ThinkPad-P51.21241.54.v2
│ └── 20191023-122438/
│ └── train/
│ └── events.out.tfevents.1571826278.elia-ThinkPad-P51.23680.54.v2
├── custom_rotors_interface/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── custom_rotors_interface/
│ │ └── custom_rotors_interface.h
│ ├── package.xml
│ ├── parameters/
│ │ ├── autopilot.yaml
│ │ ├── custom_rotors_interface.yaml
│ │ ├── manual_flight_assistant.yaml
│ │ └── position_controller.yaml
│ └── src/
│ ├── custom_rotors_interface.cpp
│ └── custom_rotors_interface_node.cpp
├── dependencies.yaml
├── fpv_aggressive_trajectories/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── fpv_aggressive_trajectories/
│ │ ├── fpv_aggressive_trajectories.h
│ │ ├── odometry_republisher.h
│ │ └── visualize.h
│ ├── launch/
│ │ └── simulation/
│ │ ├── controller.launch
│ │ ├── custom_rotors_interface.launch
│ │ ├── rpg_mpc.launch
│ │ ├── rpg_pid.launch
│ │ └── simulation.launch
│ ├── package.xml
│ ├── parameters/
│ │ └── mpc_params.yaml
│ ├── resources/
│ │ ├── race_track/
│ │ │ ├── iros_materials/
│ │ │ │ └── materials/
│ │ │ │ └── scripts/
│ │ │ │ ├── asphalt.material
│ │ │ │ ├── carpet.material
│ │ │ │ ├── wall_1.material
│ │ │ │ ├── wall_2.material
│ │ │ │ ├── wall_3.material
│ │ │ │ └── wall_4.material
│ │ │ └── real_world/
│ │ │ └── gate/
│ │ │ └── meshes/
│ │ │ └── gate.dae
│ │ ├── rviz/
│ │ │ ├── race_track.rviz
│ │ │ └── real_world.rviz
│ │ ├── simulation/
│ │ │ ├── urdf/
│ │ │ │ ├── component_snippets.xacro
│ │ │ │ ├── hummingbird.xacro
│ │ │ │ ├── hummingbird_base.xacro
│ │ │ │ └── mav_generic_odometry_sensor.gazebo
│ │ │ └── vehicles/
│ │ │ ├── hummingbird.gazebo
│ │ │ └── hummingbird_rgbcamera300200.gazebo
│ │ ├── urdf/
│ │ │ ├── component_snippets.xacro
│ │ │ ├── hummingbird_base.xacro
│ │ │ └── mav_generic_odometry_sensor_rgb.gazebo
│ │ └── worlds/
│ │ ├── controller_learning.world
│ │ ├── random_gates.world
│ │ ├── single_gate.world
│ │ └── track.world
│ ├── rviz/
│ │ └── trajectory_comparison_looping_sim.rviz
│ └── src/
│ ├── fpv_aggressive_trajectories.cpp
│ ├── odometry_republisher.cpp
│ └── visualize.cpp
├── odometry_converter/
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── odometry_converter/
│ │ └── odometry_converter.h
│ ├── package.xml
│ └── src/
│ ├── odometry_converter.cpp
│ └── odometry_converter_node.cpp
├── pip_requirements.txt
├── test_performance/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── __init__.py
│ ├── launch/
│ │ └── test_performance.launch
│ ├── nodes/
│ │ └── test_performance.py
│ ├── package.xml
│ ├── setup.py
│ └── src/
│ └── TestPerformance/
│ ├── TestPerformance.py
│ └── __init__.py
└── trajectory_visualizer/
├── .gitignore
├── CMakeLists.txt
├── include/
│ └── trajectory_visualizer/
│ └── trajectory_visualizer.h
├── package.xml
└── src/
└── trajectory_visualizer.cpp
================================================
FILE CONTENTS
================================================
================================================
FILE: .clang-format
================================================
---
Language: Cpp
# BasedOnStyle: Google
AccessModifierOffset: -1
AlignAfterOpenBracket: Align
AlignConsecutiveAssignments: false
AlignConsecutiveDeclarations: false
AlignEscapedNewlines: Left
AlignOperands: true
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: true
AllowShortBlocksOnASingleLine: false
AllowShortCaseLabelsOnASingleLine: false
AllowShortFunctionsOnASingleLine: All
AllowShortIfStatementsOnASingleLine: true
AllowShortLoopsOnASingleLine: true
AlwaysBreakAfterDefinitionReturnType: None
AlwaysBreakAfterReturnType: None
AlwaysBreakBeforeMultilineStrings: true
AlwaysBreakTemplateDeclarations: true
BinPackArguments: true
BinPackParameters: true
BraceWrapping:
AfterClass: false
AfterControlStatement: false
AfterEnum: false
AfterFunction: false
AfterNamespace: false
AfterObjCDeclaration: false
AfterStruct: false
AfterUnion: false
AfterExternBlock: false
BeforeCatch: false
BeforeElse: false
IndentBraces: false
SplitEmptyFunction: true
SplitEmptyRecord: true
SplitEmptyNamespace: true
BreakBeforeBinaryOperators: None
BreakBeforeBraces: Attach
BreakBeforeInheritanceComma: false
BreakBeforeTernaryOperators: true
BreakConstructorInitializersBeforeComma: false
BreakConstructorInitializers: BeforeColon
BreakAfterJavaFieldAnnotations: false
BreakStringLiterals: true
ColumnLimit: 80
CommentPragmas: '^ IWYU pragma:'
CompactNamespaces: false
ConstructorInitializerAllOnOneLineOrOnePerLine: true
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 4
Cpp11BracedListStyle: true
DerivePointerAlignment: true
DisableFormat: false
ExperimentalAutoDetectBinPacking: false
FixNamespaceComments: true
ForEachMacros:
- foreach
- Q_FOREACH
- BOOST_FOREACH
IncludeBlocks: Preserve
IncludeCategories:
- Regex: '^<ext/.*\.h>'
Priority: 2
- Regex: '^<.*\.h>'
Priority: 1
- Regex: '^<.*'
Priority: 2
- Regex: '.*'
Priority: 3
IncludeIsMainRegex: '([-_](test|unittest))?$'
IndentCaseLabels: true
IndentPPDirectives: None
IndentWidth: 2
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: false
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000
PenaltyExcessCharacter: 1000000
PenaltyReturnTypeOnItsOwnLine: 200
PointerAlignment: Left
#RawStringFormats:
# - Delimiter: pb
# Language: TextProto
# BasedOnStyle: google
ReflowComments: true
SortIncludes: true
SortUsingDeclarations: true
SpaceAfterCStyleCast: false
SpaceAfterTemplateKeyword: true
SpaceBeforeAssignmentOperators: true
SpaceBeforeParens: ControlStatements
SpaceInEmptyParentheses: false
SpacesBeforeTrailingComments: 2
SpacesInAngles: false
SpacesInContainerLiterals: true
SpacesInCStyleCastParentheses: false
SpacesInParentheses: false
SpacesInSquareBrackets: false
Standard: Auto
TabWidth: 8
UseTab: Never
...
================================================
FILE: .gitignore
================================================
.idea/
cmake-build-debug/
__pycache__/
*.pyc
================================================
FILE: LICENSE
================================================
MIT License
Copyright (c) 2020 Robotics and Perception Group
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
================================================
# Deep Drone Acrobatics
This repo contains the code associated to our paper Deep Drone Acrobatics.
<p align="center">
<img src="./img/fma_powerloop.gif" alt="dda">
</p>
#### Citing
If you use this code in an academic context, please cite the following publication:
Paper: [Deep Drone Acrobatics](http://rpg.ifi.uzh.ch/docs/RSS20_Kaufmann.pdf)
RSS 2020 Video Pitch: [Youtube](https://youtu.be/r4zzdFw87CY)
Video: [YouTube](https://youtu.be/2N_wKXQ6MXA)
```
@inproceedings{kaufmann2020RSS,
title={Deep Drone Acrobatics},
author={Kaufmann, Elia and Loquercio, Antonio and Ranftl, Ren{\'e} and M{\"u}ller, Matthias and Koltun, Vladlen and Scaramuzza, Davide},
booktitle={Proceedings of Robotics: Science and Systems},
year={2020},
address={Corvalis, Oregon, USA},
month={July},
doi={10.15607/RSS.2020.XVI.040}
}
```
## Installation
### Requirements
The code was tested with Ubuntu 18.04, ROS Melodic, Anaconda v4.8.3.
Different OS and ROS versions are possible but not supported.
### Step-by-Step Procedure
Use the following commands to create a new catkin workspace and a virtual environment with all the required dependencies.
```bash
export ROS_VERSION=melodic
mkdir drone_acrobatics_ws
cd drone_acrobatics_ws
export CATKIN_WS=./catkin_dda
mkdir -p $CATKIN_WS/src
cd $CATKIN_WS
catkin init
catkin config --extend /opt/ros/$ROS_VERSION
catkin config --merge-devel
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS=-fdiagnostics-color
cd src
git clone https://github.com/uzh-rpg/deep_drone_acrobatics.git
vcs-import < deep_drone_acrobatics/dependencies.yaml
#install extra dependencies (might need more depending on your OS)
sudo apt-get install libqglviewer-dev-qt5
```
Before continuing, make sure that your protobuf compiler version is 3.0.0.
To check this out, type in a terminal ``protoc --version``.
If This is not the case, then check out [this guide](https://github.com/linux-on-ibm-z/docs/wiki/Building-ProtoBuf-3.0.0) on how to do it.
Note that building the VINS-Mono package requires to install the [Ceres Solver](http://ceres-solver.org/index.html). You can find installation instructions [here](https://github.com/HKUST-Aerial-Robotics/VINS-Mono) or [here](http://ceres-solver.org/installation.html).
```bash
# Build and re-source the workspace
catkin build
. ../devel/setup.bash
# Create your learning environment
cd deep_drone_acrobatics
conda create --name drone_flow python=3.6
conda activate drone_flow
# Install (in an hacky way) python requirements
pip install -r pip_requirements.txt
conda install --file conda_requirements.txt
```
## Let's do a Power Loop
Once you have installed the dependencies, you will be able to fly in simulation with our pre-trained checkpoint. You don't need necessarely need a GPU for execution. Note that if the network can't run at least at 15Hz, you won't be able to fly successfully.
Lauch a simulation! Open a terminal and type:
```bash
cd drone_acrobatics_ws
source catkin_dda/devel/setup.bash
roslaunch fpv_aggressive_trajectories simulation.launch
```
Run the Network in an other terminal:
```bash
cd
cd drone_acrobatics_ws
. ./catkin_dda/devel/setup.bash
conda activate drone_flow
python test_trajectories.py --settings_file=config/test_settings.yaml
```
## Train your own acrobatic controller
You can use the following commands to generate data in simulation and train your model on it. The trained checkpoint can then be used to control a physical platform (if you have one!).
### Generate data
Launch the simulation in one terminal
```bash
cd drone_acrobatics_ws
source catkin_dda/devel/setup.bash
roslaunch fpv_aggressive_trajectories simulation.launch
```
Launch data collection (with dagger) in an other terminal
```bash
cd
cd drone_acrobatics_ws
. ./catkin_dda/devel/setup.bash
conda activate drone_flow
python iterative_learning_trajectories.py --settings_file=config/dagger_settings.yaml
```
It is possible to change parameters (number of rollouts, history length, etc. ) in the file [dagger\_settings.yaml](./controller_learning/config/dagger_settings.yaml). Keep in mind that if you change the network (for example by changing the history length), you will need to adapt the file [test_settings.yaml](./controller_learning/config/test_settings.yaml) for compatibility.
### Train the Network
If you want to train the network on data already collected (for example to do some parameter tuning) use the following commands.
Make sure to adapt the settings file to your configuration.
```bash
cd
cd drone_acrobatics_ws
. ./catkin_dda/devel/setup.bash
conda activate drone_flow
python train.py --settings_file=config/train_settings.yaml
```
### Test the Network
To test the network you trained, adapt the [test_settings.yaml](./controller_learning/config/test_settings.yaml) with the new checkpoint path. Then follow the instruction to test!
Lauch a simulation! Open a terminal and type:
```bash
cd drone_acrobatics_ws
source catkin_dda/devel/setup.bash
roslaunch fpv_aggressive_trajectories simulation.launch
```
Run the Network in an other terminal:
```bash
cd
cd drone_acrobatics_ws
. ./catkin_dda/devel/setup.bash
conda activate drone_flow
python test_trajectories.py --settings_file=config/test_settings.yaml
```
================================================
FILE: acrobatic_trajectory_helper/.gitignore
================================================
.idea/
cmake-build-debug/
================================================
FILE: acrobatic_trajectory_helper/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(acrobatic_trajectory_helper)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
add_compile_options(-O3)
find_package(catkin_simple REQUIRED)
catkin_simple(ALL_DEPS_REQUIRED)
cs_add_library(${PROJECT_NAME} src/polynomial_trajectory_helper.cpp
src/heading_trajectory_helper.cpp src/circle_trajectory_helper.cpp)
cs_add_library(acrobatic_sequence src/acrobatic_sequence)
target_link_libraries(acrobatic_sequence ${PROJECT_NAME})
cs_install()
cs_export()
================================================
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/acrobatic_sequence.h
================================================
#pragma once
#include <quadrotor_common/trajectory.h>
#include <list>
namespace fpv_aggressive_trajectories {
class AcrobaticSequence {
public:
explicit AcrobaticSequence(
const quadrotor_common::TrajectoryPoint& start_state);
virtual ~AcrobaticSequence();
bool appendLoops(const int n_loops, const double& circle_velocity,
const double& radius,
const Eigen::Vector3d& circle_center_offset,
const Eigen::Vector3d& circle_center_offset_end,
const bool break_at_end, const double& traj_sampling_freq);
bool appendBarrelRoll(const int n_loops, const double& circle_velocity, const double& radius,
const Eigen::Vector3d& circle_center_offset,
const Eigen::Vector3d& circle_center_offset_end,
const bool break_at_end);
bool appendMattyLoop(const int n_loops, const double& circle_velocity, const double& radius,
const Eigen::Vector3d& circle_center_offset,
const Eigen::Vector3d& circle_center_offset_end);
std::list<quadrotor_common::Trajectory> getManeuverList();
private:
std::list<quadrotor_common::Trajectory> maneuver_list_;
};
} // namespace fpv_aggressive_trajectories
================================================
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/circle_trajectory_helper.h
================================================
#pragma once
#include <quadrotor_common/trajectory.h>
#include <Eigen/Dense>
namespace acrobatic_trajectory_helper {
namespace circles {
quadrotor_common::Trajectory computeHorizontalCircleTrajectory(
const Eigen::Vector3d center, const double radius, const double speed,
const double phi_start, const double phi_end,
const double sampling_frequency);
quadrotor_common::Trajectory computeVerticalCircleTrajectory(
const Eigen::Vector3d center, const double orientation, const double radius,
const double speed, const double phi_start, const double phi_end,
const double sampling_frequency);
quadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew(
const Eigen::Vector3d center, const double orientation, const double radius,
const double speed, const double phi_start, const double phi_end,
const double corkscrew_velocity, const double sampling_frequency);
} // namespace circles
} // namespace acrobatic_trajectory_helper
================================================
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/heading_trajectory_helper.h
================================================
#pragma once
#include <quadrotor_common/trajectory.h>
namespace acrobatic_trajectory_helper {
namespace heading {
void addConstantHeading(const double heading,
quadrotor_common::Trajectory* trajectory);
void addConstantHeadingRate(const double initial_heading,
const double final_heading,
quadrotor_common::Trajectory* trajectory);
} // namespace heading
} // namespace acrobatic_trajectory_helper
================================================
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/polynomial_trajectory_helper.h
================================================
#pragma once
#include <polynomial_trajectories/polynomial_trajectory.h>
#include <polynomial_trajectories/polynomial_trajectory_settings.h>
#include <quadrotor_common/trajectory.h>
#include <quadrotor_common/trajectory_point.h>
#include <Eigen/Dense>
namespace acrobatic_trajectory_helper {
namespace polynomials {
// Constrained Polynomials
quadrotor_common::Trajectory computeTimeOptimalTrajectory(
const quadrotor_common::TrajectoryPoint& s0,
const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency);
quadrotor_common::Trajectory computeFixedTimeTrajectory(
const quadrotor_common::TrajectoryPoint& s0,
const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,
const double execution_time, const double sampling_frequency);
// Minimum Snap Style Polynomials
quadrotor_common::Trajectory generateMinimumSnapTrajectory(
const Eigen::VectorXd& segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency);
quadrotor_common::Trajectory generateMinimumSnapTrajectory(
const Eigen::VectorXd& initial_segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency);
quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency);
quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency);
quadrotor_common::Trajectory generateMinimumSnapRingTrajectory(
const Eigen::VectorXd& segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency);
quadrotor_common::Trajectory generateMinimumSnapRingTrajectory(
const Eigen::VectorXd& initial_segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency);
quadrotor_common::Trajectory
generateMinimumSnapRingTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency);
quadrotor_common::Trajectory
generateMinimumSnapRingTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency);
// Sampling function
quadrotor_common::Trajectory samplePolynomial(
const polynomial_trajectories::PolynomialTrajectory& polynomial,
const double sampling_frequency);
} // namespace polynomials
} // namespace acrobatic_trajectory_helper
================================================
FILE: acrobatic_trajectory_helper/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>acrobatic_trajectory_helper</name>
<version>0.0.0</version>
<description>The acrobatic_trajectory_helper package</description>
<maintainer email="ekaufmann@ifi.uzh.ch">Elia Kaufmann</maintainer>
<license>MIT</license>
<author>Elia Kaufmann</author>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<depend>eigen_catkin</depend>
<depend>minimum_jerk_trajectories</depend>
<depend>polynomial_trajectories</depend>
<depend>quadrotor_common</depend>
<depend>roscpp</depend>
<export>
</export>
</package>
================================================
FILE: acrobatic_trajectory_helper/src/acrobatic_sequence.cpp
================================================
#include "acrobatic_trajectory_helper/acrobatic_sequence.h"
#include <acrobatic_trajectory_helper/circle_trajectory_helper.h>
#include <acrobatic_trajectory_helper/heading_trajectory_helper.h>
#include <acrobatic_trajectory_helper/polynomial_trajectory_helper.h>
#include <minimum_jerk_trajectories/RapidTrajectoryGenerator.h>
#include <quadrotor_common/geometry_eigen_conversions.h>
#include <quadrotor_common/math_common.h>
#include <quadrotor_common/parameter_helper.h>
#include <quadrotor_common/trajectory_point.h>
#include <fstream>
namespace fpv_aggressive_trajectories {
AcrobaticSequence::AcrobaticSequence(
const quadrotor_common::TrajectoryPoint& start_state) {
printf("Initiated acrobatic sequence\n");
quadrotor_common::Trajectory init_trajectory;
quadrotor_common::TrajectoryPoint init_point;
init_point = start_state;
init_trajectory.points.push_back(init_point);
maneuver_list_.push_back(init_trajectory);
}
AcrobaticSequence::~AcrobaticSequence() {}
bool AcrobaticSequence::appendLoops(
const int n_loops, const double& circle_velocity, const double& radius,
const Eigen::Vector3d& circle_center_offset,
const Eigen::Vector3d& circle_center_offset_end, const bool break_at_end,
const double& traj_sampling_freq) {
printf("appending loop\n");
// get start state
quadrotor_common::TrajectoryPoint init_state =
maneuver_list_.back().points.back();
printf(
"Enter state of loop maneuver: Pos: %.2f, %.2f, %.2f | Vel: %.2f, %.2f, "
"%.2f\n",
init_state.position.x(), init_state.position.y(), init_state.position.z(),
init_state.velocity.x(), init_state.velocity.y(),
init_state.velocity.z());
const double exec_loop_rate = traj_sampling_freq;
const double desired_heading = 0.0;
const double figure_z_rotation_angle = 0.0;
// const double figure_z_rotation_angle = 0.785398163;
const Eigen::Quaterniond q_W_P = Eigen::Quaterniond(
Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ()));
double desired_heading_loop = quadrotor_common::wrapMinusPiToPi(
desired_heading + figure_z_rotation_angle);
// cirlce center RELATIVE to start position
const Eigen::Vector3d circle_center =
init_state.position + q_W_P.inverse() * circle_center_offset;
printf("circle center: %.2f, %.2f, %.2f\n", circle_center.x(),
circle_center.y(), circle_center.z());
const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / radius;
const double max_roll_pitch_rate = 3.0;
// Compute Circle trajectory
printf("compute circle trajectory\n");
quadrotor_common::Trajectory circle_trajectory =
acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectory(
circle_center, figure_z_rotation_angle, radius, circle_velocity,
M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
desired_heading_loop, &circle_trajectory);
quadrotor_common::TrajectoryPoint circle_enter_state =
circle_trajectory.points.front();
// Start position relative to circle center
quadrotor_common::TrajectoryPoint start_state;
start_state = init_state;
// enter trajectory
printf("compute enter trajectory\n");
// printf("Maximum speed: %.3f, current speed: %.3f\n", 1.1*circle_velocity,
// start_state.velocity.norm());
quadrotor_common::Trajectory enter_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
start_state, circle_enter_state, 4,
1.1 * std::max(start_state.velocity.norm(), circle_velocity),
max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
desired_heading_loop, &enter_trajectory);
// End position RELATIVE to circle center
printf("compute exit trajectory\n");
const Eigen::Vector3d end_pos_P =
circle_center_offset_end; // Eigen::Vector3d(circle_center_offset.x(),
// scircle_center_offset.y(),-circle_center_offset.z()); // nice breaking
// forward flip
quadrotor_common::TrajectoryPoint end_state;
end_state.position = q_W_P * end_pos_P + circle_center;
end_state.velocity = q_W_P * Eigen::Vector3d(circle_velocity, 0.0, 0.0);
quadrotor_common::Trajectory exit_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
circle_enter_state, end_state, 4, 1.1 * circle_velocity, max_thrust,
2.0 * max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
desired_heading_loop, &exit_trajectory);
quadrotor_common::Trajectory breaking_trajectory;
breaking_trajectory.trajectory_type =
quadrotor_common::Trajectory::TrajectoryType::GENERAL;
maneuver_list_.push_back(enter_trajectory);
maneuver_list_.push_back(circle_trajectory);
maneuver_list_.push_back(exit_trajectory);
if (break_at_end) {
// append breaking trajectory at end
quadrotor_common::TrajectoryPoint end_state_hover;
end_state_hover.position =
(end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0));
end_state_hover.velocity = Eigen::Vector3d::Zero();
breaking_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0,
max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
0.0, &breaking_trajectory);
maneuver_list_.push_back(breaking_trajectory);
}
return !(enter_trajectory.trajectory_type ==
quadrotor_common::Trajectory::TrajectoryType::UNDEFINED ||
circle_trajectory.trajectory_type ==
quadrotor_common::Trajectory::TrajectoryType::UNDEFINED ||
exit_trajectory.trajectory_type ==
quadrotor_common::Trajectory::TrajectoryType::UNDEFINED ||
breaking_trajectory.trajectory_type ==
quadrotor_common::Trajectory::TrajectoryType::UNDEFINED);
}
bool AcrobaticSequence::appendMattyLoop(const int n_loops, const double& circle_velocity, const double& radius,
const Eigen::Vector3d& circle_center_offset,
const Eigen::Vector3d& circle_center_offset_end) {
printf("appending matty loop\n");
// get start state
quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back();
const double exec_loop_rate = 50.0;
const double desired_heading = M_PI;
const double figure_z_rotation_angle = 0.0;
const Eigen::Quaterniond q_W_P = Eigen::Quaterniond(
Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ()));
double desired_heading_loop = quadrotor_common::wrapMinusPiToPi(
desired_heading + figure_z_rotation_angle);
// cirlce center RELATIVE to start position
const Eigen::Vector3d circle_center = init_state.position + q_W_P.inverse() * circle_center_offset;
const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / radius;
const double max_roll_pitch_rate = 3.0;
quadrotor_common::Trajectory circle_trajectory =
acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectory(
circle_center, figure_z_rotation_angle, radius,
circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
desired_heading_loop, &circle_trajectory);
quadrotor_common::TrajectoryPoint circle_enter_state =
circle_trajectory.points.front();
// Start position relative to circle center
quadrotor_common::TrajectoryPoint start_state;
start_state = init_state;
// enter trajectory
quadrotor_common::Trajectory enter_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
start_state, circle_enter_state, 4, 1.1 * std::max(start_state.velocity.norm(), circle_velocity),
max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeadingRate(0.0,
M_PI,
&enter_trajectory);
const Eigen::Vector3d end_pos_P = circle_center_offset_end;
quadrotor_common::TrajectoryPoint end_state;
end_state.position = q_W_P * end_pos_P + circle_center;
end_state.velocity = q_W_P * Eigen::Vector3d(circle_velocity, 0.0, 0.0);
quadrotor_common::Trajectory exit_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
circle_enter_state, end_state, 4, 1.1 * circle_velocity, max_thrust,
2.0 * max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeadingRate(M_PI,
2 * M_PI,
&exit_trajectory);
// append breaking trajectory at end
quadrotor_common::TrajectoryPoint end_state_hover;
end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0));
end_state_hover.velocity = Eigen::Vector3d::Zero();
quadrotor_common::Trajectory breaking_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0,
max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
0.0,
&breaking_trajectory);
maneuver_list_.push_back(enter_trajectory);
maneuver_list_.push_back(circle_trajectory);
maneuver_list_.push_back(exit_trajectory);
maneuver_list_.push_back(breaking_trajectory);
return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED
|| circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED
|| exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED
|| breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED);
}
bool AcrobaticSequence::appendBarrelRoll(const int n_loops, const double& circle_velocity, const double& radius,
const Eigen::Vector3d& circle_center_offset,
const Eigen::Vector3d& circle_center_offset_end,
const bool break_at_end) {
printf("appending barell roll\n");
// get start state
quadrotor_common::TrajectoryPoint init_state = maneuver_list_.back().points.back();
const double exec_loop_rate = 50.0;
const double desired_heading = 0.0;
const double circle_radius = radius;
double corkscrew_velocity = 0.5;
const double max_thrust = 9.81 + 1.5 * pow(circle_velocity, 2.0) / circle_radius;
const double max_roll_pitch_rate = 3.0;
const double figure_z_rotation_angle = M_PI / 2.0;
double desired_heading_loop = quadrotor_common::wrapMinusPiToPi(
desired_heading + figure_z_rotation_angle);
quadrotor_common::TrajectoryPoint circle_enter_state;
circle_enter_state.position = init_state.position + circle_center_offset;
circle_enter_state.velocity = Eigen::Vector3d(corkscrew_velocity, circle_velocity, 0.0);
// compute enter trajectory
quadrotor_common::Trajectory enter_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
init_state, circle_enter_state, 4, 1.5 * std::max(init_state.velocity.norm(), circle_velocity),
max_thrust, 2.0 * max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(0.0,
&enter_trajectory);
// cirlce center RELATIVE to start position
const Eigen::Vector3d circle_center = circle_enter_state.position + Eigen::Vector3d(0.0, 0.0, circle_radius);
// Compute Circle trajectory
double rotate_loop = M_PI / 2.0;
quadrotor_common::Trajectory circle_trajectory =
acrobatic_trajectory_helper::circles::computeVerticalCircleTrajectoryCorkScrew(
circle_center, rotate_loop, circle_radius,
circle_velocity, M_PI / 2.0, -(3.0 / 2.0 + 2 * (n_loops - 1)) * M_PI, corkscrew_velocity, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
0, &circle_trajectory);
const Eigen::Quaterniond q_W_P = Eigen::Quaterniond(
Eigen::AngleAxisd(figure_z_rotation_angle, Eigen::Vector3d::UnitZ()));
quadrotor_common::TrajectoryPoint end_state;
end_state.position = circle_trajectory.points.back().position + circle_center_offset_end;
end_state.velocity = Eigen::Vector3d(circle_velocity, 0.0, 0.0);
quadrotor_common::TrajectoryPoint circle_exit_state = circle_trajectory.points.back();
quadrotor_common::Trajectory exit_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
circle_exit_state, end_state, 4, 1.5 * circle_velocity, max_thrust,
2.0 * max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(0.0,
&exit_trajectory);
maneuver_list_.push_back(enter_trajectory);
maneuver_list_.push_back(circle_trajectory);
maneuver_list_.push_back(exit_trajectory);
quadrotor_common::Trajectory breaking_trajectory;
breaking_trajectory.trajectory_type = quadrotor_common::Trajectory::TrajectoryType::GENERAL;
if (break_at_end) {
// append breaking trajectory at end
quadrotor_common::TrajectoryPoint end_state_hover;
end_state_hover.position = (end_state.position + Eigen::Vector3d(2.0, 0.0, 0.0));
end_state_hover.velocity = Eigen::Vector3d::Zero();
breaking_trajectory =
acrobatic_trajectory_helper::polynomials::computeTimeOptimalTrajectory(
end_state, end_state_hover, 4, 1.1 * circle_velocity, 15.0,
max_roll_pitch_rate, exec_loop_rate);
acrobatic_trajectory_helper::heading::addConstantHeading(
0.0,
&breaking_trajectory);
maneuver_list_.push_back(breaking_trajectory);
}
// Debug output
std::cout << static_cast<int>(enter_trajectory.trajectory_type) << std::endl;
std::cout << static_cast<int>(circle_trajectory.trajectory_type) << std::endl;
std::cout << static_cast<int>(exit_trajectory.trajectory_type) << std::endl;
return !(enter_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED
|| circle_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED
|| exit_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED
|| breaking_trajectory.trajectory_type == quadrotor_common::Trajectory::TrajectoryType::UNDEFINED);
}
std::list<quadrotor_common::Trajectory> AcrobaticSequence::getManeuverList() {
return maneuver_list_;
}
} // namespace fpv_aggressive_trajectories
================================================
FILE: acrobatic_trajectory_helper/src/circle_trajectory_helper.cpp
================================================
#include "acrobatic_trajectory_helper/circle_trajectory_helper.h"
#include <quadrotor_common/math_common.h>
#include <quadrotor_common/trajectory_point.h>
namespace acrobatic_trajectory_helper {
namespace circles {
quadrotor_common::Trajectory computeVerticalCircleTrajectory(
const Eigen::Vector3d center, const double orientation, const double radius,
const double speed, const double phi_start, const double phi_end,
const double sampling_frequency) {
/*
* We use a coordinate system with x to the front, y to the left, and z up.
* Orientation is the angle by which the circle is rotated about the z-axis
* If the orientation = 0.0 then the circle is in the x-z plane
* When setting phi_start = 0.0 and orientation = 0.0, the start point will
* be at (radius, 0, 0) + center
* If phi_end > phi_start the circle is going in positive direction around
* the y-axis rotated by orientation. Otherwise it is going in negative
* direction
*/
quadrotor_common::Trajectory trajectory;
trajectory.trajectory_type =
quadrotor_common::Trajectory::TrajectoryType::GENERAL;
const double phi_total = phi_end - phi_start;
const double direction = phi_total / fabs(phi_total);
const double omega = direction * fabs(speed / radius);
const double angle_step = fabs(omega / sampling_frequency);
const Eigen::Quaterniond q_ori = Eigen::Quaterniond(
Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation),
Eigen::Vector3d::UnitZ()));
for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step) {
const double phi = phi_start + direction * d_phi;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(d_phi / omega));
point.position =
q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center;
point.velocity =
q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi));
point.acceleration = q_ori * (radius * pow(omega, 2.0) *
Eigen::Vector3d(-cos_phi, 0.0, sin_phi));
point.jerk = q_ori * (radius * pow(omega, 3.0) *
Eigen::Vector3d(sin_phi, 0.0, cos_phi));
point.snap = q_ori * (radius * pow(omega, 4.0) *
Eigen::Vector3d(cos_phi, 0.0, -sin_phi));
Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);
Eigen::Quaterniond quatDes =
Eigen::Quaterniond::FromTwoVectors(
I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) *
Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0,
std::sin(0.5 * orientation));
point.orientation = quatDes;
// TODO: compute feedforward bodyrates
double bodyrate_y = -omega;
point.bodyrates = Eigen::Vector3d(0.0, bodyrate_y, 0.0);
trajectory.points.push_back(point);
}
// Add last point at phi_end
const double phi = phi_start + phi_total;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(phi_total / omega));
point.position =
q_ori * (radius * Eigen::Vector3d(cos_phi, 0.0, -sin_phi)) + center;
point.velocity =
q_ori * (radius * omega * Eigen::Vector3d(-sin_phi, 0.0, -cos_phi));
point.acceleration = q_ori * (radius * pow(omega, 2.0) *
Eigen::Vector3d(-cos_phi, 0.0, sin_phi));
point.jerk = q_ori * (radius * pow(omega, 3.0) *
Eigen::Vector3d(sin_phi, 0.0, cos_phi));
point.snap = q_ori * (radius * pow(omega, 4.0) *
Eigen::Vector3d(cos_phi, 0.0, -sin_phi));
Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);
Eigen::Quaterniond quatDes =
Eigen::Quaterniond::FromTwoVectors(
I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81)) *
Eigen::Quaterniond(std::cos(0.5 * orientation), 0.0, 0.0,
std::sin(0.5 * orientation));
point.orientation = quatDes;
trajectory.points.push_back(point);
return trajectory;
}
quadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew(
const Eigen::Vector3d center, const double orientation, const double radius,
const double speed, const double phi_start, const double phi_end, const double corkscrew_velocity,
const double sampling_frequency)
{
/*
* We use a coordinate system with x to the front, y to the left, and z up.
* Orientation is the angle by which the circle is rotated about the z-axis
* If the orientation = 0.0 then the circle is in the x-z plane
* When setting phi_start = 0.0 and orientation = 0.0, the start point will
* be at (radius, 0, 0) + center
* If phi_end > phi_start the circle is going in positive direction around
* the y-axis rotated by orientation. Otherwise it is going in negative
* direction
*/
quadrotor_common::Trajectory trajectory;
trajectory.trajectory_type =
quadrotor_common::Trajectory::TrajectoryType::GENERAL;
const double phi_total = phi_end - phi_start;
const double direction = phi_total / fabs(phi_total);
const double omega = direction * fabs(speed / radius);
const double angle_step = fabs(omega / sampling_frequency);
const double linear_step = fabs(corkscrew_velocity / sampling_frequency);
double y_pos = 0.0;
const Eigen::Quaterniond q_ori = Eigen::Quaterniond(
Eigen::AngleAxisd(quadrotor_common::wrapMinusPiToPi(orientation),
Eigen::Vector3d::UnitZ()));
for (double d_phi = 0.0; d_phi < fabs(phi_total); d_phi += angle_step)
{
const double phi = phi_start + direction * d_phi;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(d_phi / omega));
point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, y_pos, -sin_phi))
+ center;
point.velocity = q_ori
* (radius * omega * Eigen::Vector3d(-sin_phi, corkscrew_velocity, -cos_phi));
point.acceleration = q_ori
* (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi));
point.jerk = q_ori
* (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi));
point.snap = q_ori
* (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi));
Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);
Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81))
* Eigen::Quaterniond(std::cos(0.5 * orientation),
0.0,
0.0,
std::sin(0.5 * orientation));
point.orientation = quatDes;
// TODO: compute feedforward bodyrates
double bodyrate_y = -omega;
point.bodyrates = Eigen::Vector3d(0.0, bodyrate_y, 0.0);
y_pos -= linear_step;
trajectory.points.push_back(point);
}
// Add last point at phi_end
const double phi = phi_start + phi_total;
const double cos_phi = cos(phi);
const double sin_phi = sin(phi);
quadrotor_common::TrajectoryPoint point;
point.time_from_start = ros::Duration(fabs(phi_total / omega));
point.position = q_ori * (radius * Eigen::Vector3d(cos_phi, y_pos, -sin_phi))
+ center;
point.velocity = q_ori
* (radius * omega * Eigen::Vector3d(-sin_phi, corkscrew_velocity, -cos_phi));
point.acceleration = q_ori
* (radius * pow(omega, 2.0) * Eigen::Vector3d(-cos_phi, 0.0, sin_phi));
point.jerk = q_ori
* (radius * pow(omega, 3.0) * Eigen::Vector3d(sin_phi, 0.0, cos_phi));
point.snap = q_ori
* (radius * pow(omega, 4.0) * Eigen::Vector3d(cos_phi, 0.0, -sin_phi));
Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);
Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(I_eZ_I, point.acceleration + Eigen::Vector3d(0.0, 0.0, 9.81))
* Eigen::Quaterniond(std::cos(0.5 * orientation),
0.0,
0.0,
std::sin(0.5 * orientation));
point.orientation = quatDes;
trajectory.points.push_back(point);
return trajectory;
}
} // namespace circles
} // namespace acrobatic_trajectory_helper
================================================
FILE: acrobatic_trajectory_helper/src/heading_trajectory_helper.cpp
================================================
#include "acrobatic_trajectory_helper/heading_trajectory_helper.h"
#include <quadrotor_common/math_common.h>
namespace acrobatic_trajectory_helper {
namespace heading {
void addConstantHeading(const double heading,
quadrotor_common::Trajectory* trajectory) {
auto iterator(trajectory->points.begin());
auto iterator_prev(trajectory->points.begin());
iterator_prev = std::prev(iterator_prev);
auto iterator_next(trajectory->points.begin());
iterator_next = std::next(iterator_next);
auto last_element = trajectory->points.end();
last_element = std::prev(last_element);
double time_step;
for (int i = 0; i < trajectory->points.size(); i++) {
// do orientation first, since bodyrate conversion will depend on it
Eigen::Vector3d I_eZ_I(0.0, 0.0, 1.0);
Eigen::Quaterniond quatDes = Eigen::Quaterniond::FromTwoVectors(
I_eZ_I, iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81));
// set full orientation and heading to zero
Eigen::Quaternion<double> q_heading =
Eigen::Quaternion<double>(Eigen::AngleAxis<double>(
heading, Eigen::Matrix<double, 3, 1>::UnitZ()));
Eigen::Quaternion<double> q_orientation = quatDes * q_heading;
iterator->orientation = q_orientation;
iterator->heading = 0.0; // heading is now absorbed in orientation
iterator->heading_rate = 0.0;
iterator->heading_acceleration = 0.0;
Eigen::Vector3d thrust_1;
Eigen::Vector3d thrust_2;
// catch case of first and last element
if (i == 0) {
thrust_1 = iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);
time_step =
(iterator_next->time_from_start - iterator->time_from_start).toSec();
thrust_2 = iterator_next->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);
} else if (i < trajectory->points.size() - 1) {
thrust_1 = iterator_prev->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);
time_step =
(iterator_next->time_from_start - iterator_prev->time_from_start)
.toSec();
thrust_2 = iterator_next->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);
} else {
// at the last point, we extrapolate the acceleration
thrust_1 = iterator_prev->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81);
thrust_2 = iterator->acceleration + Eigen::Vector3d(0.0, 0.0, 9.81) +
time_step / 2.0 * iterator->jerk;
}
thrust_1.normalize();
thrust_2.normalize();
Eigen::Vector3d crossProd =
thrust_1.cross(thrust_2); // direction of omega, in inertial axes
Eigen::Vector3d angular_rates_wf = Eigen::Vector3d(0, 0, 0);
if (crossProd.norm() > 0.0) {
angular_rates_wf = std::acos(thrust_1.dot(thrust_2)) / time_step *
crossProd / crossProd.norm();
}
// rotate bodyrates to bodyframe
iterator->bodyrates = q_orientation.inverse() * angular_rates_wf;
iterator_prev++;
iterator++;
iterator_next++;
}
}
void addConstantHeadingRate(const double initial_heading,
const double final_heading,
quadrotor_common::Trajectory* trajectory) {
if (trajectory->points.size() < 2) {
return;
}
const double delta_angle =
quadrotor_common::wrapAngleDifference(initial_heading, final_heading);
const double trajectory_duration =
(trajectory->points.back().time_from_start -
trajectory->points.front().time_from_start)
.toSec();
const double heading_rate = delta_angle / trajectory_duration;
std::list<quadrotor_common::TrajectoryPoint>::iterator it;
for (it = trajectory->points.begin(); it != trajectory->points.end(); it++) {
const double duration_ratio =
(it->time_from_start - trajectory->points.front().time_from_start)
.toSec() /
trajectory_duration;
it->heading = initial_heading + duration_ratio * delta_angle;
it->heading_rate = heading_rate;
it->heading_acceleration = 0.0;
}
}
} // namespace heading
} // namespace acrobatic_trajectory_helper
================================================
FILE: acrobatic_trajectory_helper/src/polynomial_trajectory_helper.cpp
================================================
#include "acrobatic_trajectory_helper/polynomial_trajectory_helper.h"
#include <polynomial_trajectories/constrained_polynomial_trajectories.h>
#include <polynomial_trajectories/minimum_snap_trajectories.h>
#include <polynomial_trajectories/polynomial_trajectories_common.h>
namespace acrobatic_trajectory_helper {
namespace polynomials {
// Constrained Polynomials
quadrotor_common::Trajectory computeTimeOptimalTrajectory(
const quadrotor_common::TrajectoryPoint& s0,
const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::constrained_polynomial_trajectories::
computeTimeOptimalTrajectory(s0, s1, order_of_continuity,
max_velocity, max_normalized_thrust,
max_roll_pitch_rate);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory computeFixedTimeTrajectory(
const quadrotor_common::TrajectoryPoint& s0,
const quadrotor_common::TrajectoryPoint& s1, const int order_of_continuity,
const double execution_time, const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::constrained_polynomial_trajectories::
computeFixedTimeTrajectory(s0, s1, order_of_continuity,
execution_time);
return samplePolynomial(polynomial, sampling_frequency);
}
// Minimum Snap Style Polynomials
quadrotor_common::Trajectory generateMinimumSnapTrajectory(
const Eigen::VectorXd& segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapTrajectory(segment_times, start_state, end_state,
trajectory_settings);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory generateMinimumSnapTrajectory(
const Eigen::VectorXd& initial_segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapTrajectory(initial_segment_times, start_state,
end_state, trajectory_settings,
max_velocity, max_normalized_thrust,
max_roll_pitch_rate);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapTrajectoryWithSegmentRefinement(
initial_segment_times, start_state, end_state,
trajectory_settings);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const quadrotor_common::TrajectoryPoint& start_state,
const quadrotor_common::TrajectoryPoint& end_state,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapTrajectoryWithSegmentRefinement(
initial_segment_times, start_state, end_state,
trajectory_settings, max_velocity, max_normalized_thrust,
max_roll_pitch_rate);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory generateMinimumSnapRingTrajectory(
const Eigen::VectorXd& segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapRingTrajectory(segment_times, trajectory_settings);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory generateMinimumSnapRingTrajectory(
const Eigen::VectorXd& initial_segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapRingTrajectory(
initial_segment_times, trajectory_settings, max_velocity,
max_normalized_thrust, max_roll_pitch_rate);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory
generateMinimumSnapRingTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapRingTrajectoryWithSegmentRefinement(
initial_segment_times, trajectory_settings);
return samplePolynomial(polynomial, sampling_frequency);
}
quadrotor_common::Trajectory
generateMinimumSnapRingTrajectoryWithSegmentRefinement(
const Eigen::VectorXd& initial_segment_times,
const polynomial_trajectories::PolynomialTrajectorySettings&
trajectory_settings,
const double max_velocity, const double max_normalized_thrust,
const double max_roll_pitch_rate, const double sampling_frequency) {
polynomial_trajectories::PolynomialTrajectory polynomial =
polynomial_trajectories::minimum_snap_trajectories::
generateMinimumSnapRingTrajectoryWithSegmentRefinement(
initial_segment_times, trajectory_settings, max_velocity,
max_normalized_thrust, max_roll_pitch_rate);
return samplePolynomial(polynomial, sampling_frequency);
}
// Sampling function
quadrotor_common::Trajectory samplePolynomial(
const polynomial_trajectories::PolynomialTrajectory& polynomial,
const double sampling_frequency) {
if (polynomial.trajectory_type ==
polynomial_trajectories::TrajectoryType::UNDEFINED) {
return quadrotor_common::Trajectory();
}
quadrotor_common::Trajectory trajectory;
trajectory.points.push_back(polynomial.start_state);
const ros::Duration dt(1.0 / sampling_frequency);
ros::Duration time_from_start = polynomial.start_state.time_from_start + dt;
while (time_from_start < polynomial.T) {
trajectory.points.push_back(polynomial_trajectories::getPointFromTrajectory(
polynomial, time_from_start));
time_from_start += dt;
}
trajectory.points.push_back(polynomial.end_state);
trajectory.trajectory_type =
quadrotor_common::Trajectory::TrajectoryType::GENERAL;
return trajectory;
}
} // namespace polynomials
} // namespace acrobatic_trajectory_helper
================================================
FILE: conda_requirements.txt
================================================
defusedxml==0.6.0
docutils==0.15.2
google-auth-oauthlib==0.4.1
google-pasta==0.1.7
Keras-Applications==1.0.8
Keras-Preprocessing==1.1.0
oauthlib==3.1.0
pyasn1==0.4.8
pydot==1.4.1
requests-oauthlib==1.3.0
rsa==4.7
scipy==1.4.1
tensorflow-estimator==2.1.0
tensorflow-gpu==2.4.1
wrapt==1.11.2
================================================
FILE: controller_learning/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(controller_learning)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
add_compile_options(-O3)
#find_package(catkin_simple REQUIRED)
#catkin_simple(ALL_DEPS_REQUIRED)
#cs_add_library(acrobatic_sequence src/acrobatic_sequence.cpp)
#cs_add_executable(fpv_aggressive_trajectories src/fpv_aggressive_trajectories.cpp)
#target_link_libraries(fpv_aggressive_trajectories acrobatic_sequence)
#cs_add_executable(odometry_republisher src/odometry_republisher.cpp)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
catkin_simple REQUIRED
)
catkin_python_setup()
catkin_simple()
cs_install()
cs_export()
================================================
FILE: controller_learning/README.md
================================================
================================================
FILE: controller_learning/common.py
================================================
import os
import time
def update_mpc_params():
params = {}
params['Q_pos_xy'] = 200.0
params['Q_pos_z'] = 500.0
params['Q_attitude'] = 50.0
params['Q_velocity'] = 10.0
params['R_thrust'] = 0.1
params['R_pitchroll'] = 0.1
params['R_yaw'] = 0.1
params['state_cost_exp'] = 0.0
params['input_cost_exp'] = 0.0
params['max_bodyrate_xy'] = 20.0
params['max_bodyrate_z'] = 5.0
params['min_thrust'] = 1.0
params['max_thrust'] = 40.0
print("Setting control parameters of MPC to:")
print(params)
os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_pos_xy " + str(params['Q_pos_xy']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_pos_z " + str(params['Q_pos_z']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_attitude " + str(params['Q_attitude']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/Q_velocity " + str(params['Q_velocity']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/R_thrust " + str(params['R_thrust']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/R_pitchroll " + str(params['R_pitchroll']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/R_yaw " + str(params['R_yaw']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/state_cost_exponential " + str(params['state_cost_exp']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/input_cost_exponential " + str(params['input_cost_exp']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_xy " + str(params['max_bodyrate_xy']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/max_bodyrate_z " + str(params['max_bodyrate_z']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/min_thrust " + str(params['min_thrust']))
os.system("timeout 1s rosparam set /hummingbird/autopilot/max_thrust " + str(params['max_thrust']))
os.system("timeout 1s rostopic pub /hummingbird/autopilot/reload_parameters std_msgs/Empty '{}'")
def setup_sim():
print("==========================")
print(" RESET SIMULATION ")
print("==========================")
# set odometry converter back to ground truth state estimate
# make sure fpv_aggressive_trajectories is not publishing anything!
# turn off network
os.system("timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Bool 'data: false'")
os.system("timeout 1s rostopic pub /hummingbird/switch_to_network std_msgs/Bool 'data: false'")
# after this message, autopilot will automatically go to 'BREAKING' and 'HOVER' state since
# no control_command_inputs are published any more
os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'")
os.system("rosservice call /gazebo/pause_physics")
print("Unpausing Physics...")
os.system("rosservice call /gazebo/unpause_physics")
print("Placing quadrotor...")
os.system("timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty")
os.system("rosservice call /gazebo/set_model_state "
"'{model_state: { model_name: hummingbird, pose: { position: { x: 0.0, y: 0.0 ,z: 0.2 }, "
"orientation: {x: 0, y: 0, z: 0.0, w: 1.0 }}, "
"twist:{ linear: {x: 0.0 , y: 0 ,z: 0 } , angular: { x: 0.0 , y: 0 , z: 0.0 }}, "
"reference_frame: world } }'")
time.sleep(2)
# start quadrotor
# os.system("timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'")
# os.system("timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty")
def random_replace():
reset_success_str = 'rostopic pub /success_reset std_msgs/Empty "{}" -1'
os.system(reset_success_str)
def initialize_vio():
# Make sure to use GT odometry in this step
os.system("timeout 1s rostopic pub /hummingbird/autopilot/off std_msgs/Empty")
os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'")
# reset quad to initial position
os.system("timeout 1s rostopic pub /hummingbird/bridge/arm std_msgs/Bool 'True'")
print("Start quadrotor")
os.system("timeout 1s rostopic pub /hummingbird/autopilot/start std_msgs/Empty")
time.sleep(10)
# Restart VIO
os.system("timeout 1s rostopic pub /feature_tracker/restart std_msgs/Bool 'data: true' ")
os.system(
"timeout 1s rostopic pub /hummingbird/autopilot/pose_command geometry_msgs/PoseStamped '{header: {seq: 0, stamp: {secs: 0, nsecs: 0} , frame_id: world}, pose:{position: { x: 0.0, y: 5.0, z: 4.0}, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0} } }'")
time.sleep(9)
# TODO: maybe remove this
# os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Bool 'data: true'")
return
================================================
FILE: controller_learning/config/dagger_settings.yaml
================================================
log_dir: "results/loop"
quad_name: "hummingbird"
verbose: False
seq_len: 1 # History Lenght. When increasing to more than one, the model becomes machine dependent due to data saving latency.
checkpoint:
# Put to true and add a path for loading a ckpt
resume_training: False
resume_file: ""
data_generation:
max_rollouts: 150
train_every_n_rollouts: 30
double_th_every_n_rollouts: 30
train:
max_training_epochs: 20
max_allowed_error: 0.6 # Collision rollouts are eliminated from the training
min_number_fts: 40 # Number of feature tracks per image
batch_size: 32
summary_freq: 400
train_dir: "data/loop/train" # Where data will be generated and where the network will train
val_dir: "data/loop/test" # Validation Data, change to a new data folder!
save_every_n_epochs: 5
use_imu: True
use_fts_tracks: True
test_time:
execute_nw_predictions: True
# Dagger constants and random controller constants for exploration
fallback_threshold_rates: 1.0
rand_thrust_mag: 6
rand_rate_mag: 3.5
rand_controller_prob: 0.05
================================================
FILE: controller_learning/config/settings.py
================================================
import math
import os
import shutil
import sys
import time
import datetime
import yaml
def create_settings(settings_yaml, mode='test'):
setting_dict = {'train': TrainSetting,
'test': TestSetting,
'dagger': DaggerSetting}
settings = setting_dict.get(mode, None)
if settings is None:
raise IOError("Unidentified Settings")
settings = settings(settings_yaml)
return settings
class Settings:
def __init__(self, settings_yaml, generate_log=True):
assert os.path.isfile(settings_yaml), settings_yaml
with open(settings_yaml, 'r') as stream:
settings = yaml.safe_load(stream)
self.quad_name = settings['quad_name']
self.seq_len = settings['seq_len']
# --- checkpoint ---
checkpoint = settings['checkpoint']
self.resume_training = checkpoint['resume_training']
assert isinstance(self.resume_training, bool)
self.resume_ckpt_file = checkpoint['resume_file']
# Save a copy of the parameters for reproducibility
log_root = settings['log_dir']
if not log_root == '' and generate_log:
current_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S")
self.log_dir = os.path.join(log_root, current_time)
os.makedirs(self.log_dir)
net_file = "./src/ControllerLearning/models/nets.py"
assert os.path.isfile(net_file)
shutil.copy(net_file, self.log_dir)
shutil.copy(settings_yaml, self.log_dir)
def add_flags(self):
self._add_flags()
def _add_flags(self):
raise NotImplementedError
class TrainSetting(Settings):
def __init__(self, settings_yaml):
super(TrainSetting, self).__init__(settings_yaml, generate_log=True)
self.settings_yaml = settings_yaml
self.add_flags()
def _add_flags(self):
with open(self.settings_yaml, 'r') as stream:
settings = yaml.safe_load(stream)
# --- Train Time --- #
train_conf = settings['train']
self.max_training_epochs = train_conf['max_training_epochs']
self.max_allowed_error = train_conf['max_allowed_error']
self.batch_size = train_conf['batch_size']
self.summary_freq = train_conf['summary_freq']
self.train_dir = train_conf['train_dir']
self.use_fts_tracks = train_conf['use_fts_tracks']
self.use_imu = train_conf['use_imu']
self.val_dir = train_conf['val_dir']
self.min_number_fts = train_conf['min_number_fts']
self.save_every_n_epochs = train_conf['save_every_n_epochs']
class TestSetting(Settings):
def __init__(self, settings_yaml):
super(TestSetting, self).__init__(settings_yaml, generate_log=True)
self.settings_yaml = settings_yaml
self.add_flags()
def _add_flags(self):
with open(self.settings_yaml, 'r') as stream:
settings = yaml.safe_load(stream)
test_time = settings['test_time']
self.execute_nw_predictions = test_time['execute_nw_predictions']
assert isinstance(self.execute_nw_predictions, bool)
self.max_rollouts = test_time['max_rollouts']
self.fallback_threshold_rates = test_time['fallback_threshold_rates']
self.fallback_threshold_thrust = test_time['fallback_threshold_thrust']
self.min_number_fts = test_time['min_number_fts']
self.use_imu = test_time['use_imu']
self.use_fts_tracks = test_time['use_fts_tracks']
self.verbose = settings['verbose']
class DaggerSetting(Settings):
def __init__(self, settings_yaml):
super(DaggerSetting, self).__init__(settings_yaml, generate_log=True)
self.settings_yaml = settings_yaml
self.add_flags()
def _add_flags(self):
with open(self.settings_yaml, 'r') as stream:
settings = yaml.safe_load(stream)
# --- Data Generation --- #
data_gen = settings['data_generation']
self.max_rollouts = data_gen['max_rollouts']
self.double_th_every_n_rollouts = data_gen['double_th_every_n_rollouts']
self.train_every_n_rollouts = data_gen['train_every_n_rollouts']
# --- Test Time --- #
test_time = settings['test_time']
self.execute_nw_predictions = test_time['execute_nw_predictions']
assert isinstance(self.execute_nw_predictions, bool)
self.fallback_threshold_rates = test_time['fallback_threshold_rates']
self.rand_thrust_mag = test_time['rand_thrust_mag']
self.rand_rate_mag = test_time['rand_rate_mag']
self.rand_controller_prob = test_time['rand_controller_prob']
# --- Train Time --- #
train_conf = settings['train']
self.max_training_epochs = train_conf['max_training_epochs']
self.max_allowed_error = train_conf['max_allowed_error']
self.batch_size = train_conf['batch_size']
self.min_number_fts = train_conf['min_number_fts']
self.summary_freq = train_conf['summary_freq']
self.train_dir = train_conf['train_dir']
self.val_dir = train_conf['val_dir']
self.use_imu = train_conf['use_imu']
self.use_fts_tracks = train_conf['use_fts_tracks']
self.val_dir = train_conf['val_dir']
self.save_every_n_epochs = train_conf['save_every_n_epochs']
self.verbose = settings['verbose']
assert isinstance(self.verbose, bool)
================================================
FILE: controller_learning/config/test_settings.yaml
================================================
quad_name: 'hummingbird'
log_dir: '/tmp/test'
ideal_inputs: False
seq_len: 1
verbose: False
throw_params:
vlin_range: 5
vang_range: 5
pos_range: 5
checkpoint:
resume_training: True
resume_file: "ckpt/loop/fts_and_imu/ckpt-7"
test_time:
max_rollouts: 5
min_number_fts: 40
execute_nw_predictions: True
fallback_threshold_thrust: 100
fallback_threshold_rates: 100
use_imu: True
use_fts_tracks: True
================================================
FILE: controller_learning/config/train_settings.yaml
================================================
log_dir: 'results/train_fma_cork_seq_1_fts_tracks_newcam_new_end'
quad_name: 'none'
seq_len: 1 # History Lenght. When increasing to more than one, the model becomes machine dependent due to data saving latency.
checkpoint:
# Put to true and add a path for loading a ckpt
resume_training: False
resume_file: ""
train:
max_training_epochs: 150
max_allowed_error: 0.7
batch_size: 32
summary_freq: 400
min_number_fts: 40
train_dir: "data/loop/train"
val_dir: "data/loop/val"
save_every_n_epochs: 5
use_imu: True
use_fts_tracks: True
================================================
FILE: controller_learning/data/.gitignore
================================================
*
*/
!.gitignore
================================================
FILE: controller_learning/iterative_learning_trajectories.py
================================================
#!/usr/bin/env python3
import argparse
import os
import sys
import time
import numpy as np
import rospy
from ControllerLearning import TrajectoryLearning
from std_msgs.msg import Bool
from common import update_mpc_params, setup_sim, random_replace, initialize_vio
from config.settings import create_settings
class Trainer():
def __init__(self, settings):
rospy.init_node('iterative_learning_node', anonymous=False)
self.settings = settings
self.trajectory_done = False
self.traj_done_sub = rospy.Subscriber("/hummingbird/switch_to_network", Bool,
self.callback_traj_done, queue_size=1)
def callback_traj_done(self, data):
self.trajectory_done = data.data
def start_experiment(self, learner):
reset_success_str = 'rostopic pub /success_reset std_msgs/Empty "{}" -1'
os.system(reset_success_str)
initialize_vio()
learner.latest_thrust_factor = 1.0 # Could be changed to adapt for different quadrotors.
print("Doing experiment {}, with such factor {}".format(learner.rollout_idx, learner.latest_thrust_factor))
# if True, we will still use the VIO-orientation, even when initialization is poor.
# If set to False, we will fall back to GT.
# Set to True only if you are sure your VIO is well calibrated.
use_chimaera = False
# check if initialization was good. If not, we will perform rollout with ground truth to not waste time!
vio_init_good = learner.vio_init_good
if vio_init_good:
rospy.loginfo("VINS-Mono initialization is good, switching to vision-based state estimate!")
os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 1'")
else:
if use_chimaera:
rospy.logerr("VINS-Mono initialization is poor, use orientation, bodyrates from VIO and linear velocity estimate from GT!")
os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 2'")
else:
rospy.logerr("VINS-Mono initialization is poor, keeping ground truth estimate!")
os.system("timeout 1s rostopic pub /switch_odometry std_msgs/Int8 'data: 0'")
# Start Flying!
os.system("timeout 1s rostopic pub /hummingbird/fpv_quad_looping/execute_trajectory std_msgs/Bool 'data: true'")
return vio_init_good
def perform_training(self):
learner = TrajectoryLearning.TrajectoryLearning(self.settings, mode="iterative")
shutdown_requested = False
train_every_n_rollouts = self.settings.train_every_n_rollouts
if self.settings.execute_nw_predictions:
print("-------------------------------------------")
print("Running Dagger with the following params")
print("Rates threshold: {}; Rand Controller Th {}".format(
self.settings.fallback_threshold_rates,
self.settings.rand_controller_prob))
print("-------------------------------------------")
else:
print("---------------------------")
print("Collecting Data with Expert")
print("---------------------------")
while (not shutdown_requested) and (learner.rollout_idx < self.settings.max_rollouts):
self.trajectory_done = False
setup_sim()
learner.start_data_recording()
self.start_experiment(learner)
print("Starting Experiment {}".format(learner.rollout_idx))
start_time = time.time()
time_run = 0
ref_log = []
gt_pos_log = []
error_log = []
while (not self.trajectory_done) and (time_run < 100):
time.sleep(0.1)
time_run = time.time() - start_time
if learner.use_network and learner.reference_updated:
pos_ref_dict = learner.compute_trajectory_error()
gt_pos_log.append(pos_ref_dict["gt_pos"])
ref_log.append(pos_ref_dict["gt_ref"])
error_log.append(np.linalg.norm(pos_ref_dict["gt_pos"] - pos_ref_dict["gt_ref"]))
# final logging
tracking_error = np.mean(error_log)
median_traj_error = np.median(error_log)
t_log = np.stack((ref_log, gt_pos_log), axis=0)
expert_usage = learner.stop_data_recording()
shutdown_requested = learner.shutdown_requested()
print("Expert used {:.03f}% of the times".format(100.0 * expert_usage))
print("Mean Tracking Error is {:.03f}".format(tracking_error))
print("Median Tracking Error is {:.03f}".format(median_traj_error))
if learner.rollout_idx % train_every_n_rollouts == 0:
os.system("rosservice call /gazebo/pause_physics")
learner.train()
os.system("rosservice call /gazebo/unpause_physics")
if (learner.rollout_idx % self.settings.double_th_every_n_rollouts) == 0:
self.settings.fallback_threshold_rates += 0.5
print("Setting Rate Threshold to {}".format(self.settings.fallback_threshold_rates))
self.settings.rand_controller_prob = np.minimum(0.3, self.settings.rand_controller_prob * 2)
print("Setting Rand Controller Prob to {}".format(self.settings.rand_controller_prob))
if self.settings.verbose:
t_log_fname = os.path.join(self.settings.log_dir, "traj_log_{:5d}.npy".format(learner.rollout_idx))
np.save(t_log_fname, t_log)
def perform_testing(self):
learner = TrajectoryLearning.TrajectoryLearning(self.settings, mode="testing")
shutdown_requested = False
rollout_idx = 0
while (not shutdown_requested) and (rollout_idx < self.settings.max_rollouts):
self.trajectory_done = False
setup_sim()
if self.settings.verbose:
# Will save data for debugging reasons
learner.start_data_recording()
self.start_experiment(learner)
start_time = time.time()
time_run = 0
ref_log = []
gt_pos_log = []
error_log = []
while (not self.trajectory_done) and (time_run < 100):
time.sleep(0.1)
time_run = time.time() - start_time
if learner.use_network and learner.reference_updated:
pos_ref_dict = learner.compute_trajectory_error()
gt_pos_log.append(pos_ref_dict["gt_pos"])
ref_log.append(pos_ref_dict["gt_ref"])
error_log.append(np.linalg.norm(pos_ref_dict["gt_pos"] - pos_ref_dict["gt_ref"]))
# final logging
tracking_error = np.mean(error_log)
median_traj_error = np.median(error_log)
t_log = np.stack((ref_log, gt_pos_log), axis=0)
expert_usage = learner.stop_data_recording()
shutdown_requested = learner.shutdown_requested()
print("{} Rollout: Expert used {:.03f}% of the times".format(rollout_idx+1, 100.0 * expert_usage))
print("Mean Tracking Error is {:.03f}".format(tracking_error))
print("Median Tracking Error is {:.03f}".format(median_traj_error))
rollout_idx +=1
if self.settings.verbose:
t_log_fname = os.path.join(self.settings.log_dir, "traj_log_{:05d}.npy".format(rollout_idx))
np.save(t_log_fname, t_log)
def main():
parser = argparse.ArgumentParser(description='Train RAF network.')
parser.add_argument('--settings_file', help='Path to settings yaml', required=True)
args = parser.parse_args()
settings_filepath = args.settings_file
settings = create_settings(settings_filepath, mode='dagger')
update_mpc_params()
setup_sim()
trainer = Trainer(settings)
trainer.perform_training()
if __name__ == "__main__":
main()
================================================
FILE: controller_learning/launch/simulation/base_quad_simulation_no_controller.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="world_name"/>
<arg name="paused"/>
<arg name="gui"/>
<arg name="use_ground_truth"/>
<arg name="custom_models" default=""/>
<arg name="mav_name"/>
<arg name="model"/>
<arg name="enable_logging"/>
<arg name="enable_ground_truth"/>
<arg name="log_file"/>
<arg name="x_init"/>
<arg name="y_init"/>
<arg name="debug"/>
<arg name="verbose"/>
<!-- Gazebo stuff to spawn the world !-->
<env name="GAZEBO_MODEL_PATH"
value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models:$(arg custom_models)"/>
<env name="GAZEBO_RESOURCE_PATH"
value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(arg world_name)" />
<arg name="debug" value="$(arg debug)" />
<arg name="paused" value="$(arg paused)" />
<arg name="gui" value="$(arg gui)" />
<arg name="verbose" value="$(arg verbose)"/>
</include>
<!-- RotorS stuff to spawn the quadrotor !-->
<group ns="$(arg mav_name)">
<include file="$(find rotors_gazebo)/launch/spawn_mav.launch">
<arg name="mav_name" value="$(arg mav_name)" />
<arg name="model" value="$(arg model)" />
<arg name="enable_logging" value="$(arg enable_logging)" />
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
<arg name="log_file" value="$(arg log_file)"/>
<arg name="x" value="$(arg x_init)" />
<arg name="y" value="$(arg y_init)" />
</include>
</group>
<!-- RPG stuff !-->
<group ns="$(arg quad_name)" >
<node pkg="joy" type="joy_node" name="joy_node">
<param name="autorepeat_rate" value="10"/>
</node>
<node pkg="manual_flight_assistant" type="manual_flight_assistant"
name="manual_flight_assistant" output="screen">
<rosparam file="$(find rpg_rotors_interface)/parameters/manual_flight_assistant.yaml"/>
</node>
<node name="rqt_quad_gui" pkg="rqt_gui" type="rqt_gui"
args="-s rqt_quad_gui.basic_flight.BasicFlight --args
--quad_name $(arg quad_name)" output="screen"/>
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/controller.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="controller"/>
<arg name="use_ground_truth"/>
<arg name="enable_command_feedthrough"/>
<arg name="velocity_estimate_in_world_frame"/>
<!-- Autopilot -->
<include file="$(find controller_learning)/launch/simulation/$(arg controller).launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
<arg name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<arg name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
</include>
</launch>
================================================
FILE: controller_learning/launch/simulation/custom_rotors_interface.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="use_ground_truth" default="true"/>
<group ns="$(arg quad_name)">
<!-- To debug the rpg_rotors_interface -->
<group if="$(arg use_ground_truth)">
<node pkg="custom_rotors_interface" type="custom_rotors_interface_node"
name="custom_rotors_interface" output="screen" >
<rosparam file="$(find custom_rotors_interface)/parameters/custom_rotors_interface.yaml" />
<remap from="odometry" to="ground_truth/odometry" />
<remap from="arm" to="bridge/arm" />
</node>
</group>
<group unless="$(arg use_ground_truth)">
<node pkg="custom_rotors_interface" type="custom_rotors_interface_node"
name="custom_rotors_interface" output="screen" >
<rosparam file="$(find custom_rotors_interface)/parameters/custom_rotors_interface.yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
<remap from="arm" to="bridge/arm" />
</node>
</group>
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/minisim.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="use_ground_truth" default="true"/>
<arg name="controller" default="rpg_mpc"/> <!-- "rpg_mpc", "rpg_pid", "asl_mpc" -->
<!-- Quad Minisim -->
<include file="$(find quad_minisim)/launch/minisim.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
</include>
<!-- Quadrotor simulation -->
<!-- include file="$(find rpg_rotors_interface)/launch/quadrotor_empty_world_no_controller.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
</include -->
<!-- custom RotorS interface -->
<!-- include file="$(find fpv_aggressive_trajectories)/launch/custom_rotors_interface.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
</include -->
<!-- High Level Controller -->
<include file="$(find fpv_aggressive_trajectories)/launch/simulation/controller.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
<arg name="controller" value="$(arg controller)"/>
<arg name="enable_command_feedthrough" value="false"/>
<arg name="velocity_estimate_in_world_frame" value="true"/>
</include>
<group ns="$(arg quad_name)">
<!-- Trajectory Generation -->
<node pkg="fpv_aggressive_trajectories" name="fpv_aggressive_trajectories"
type="fpv_aggressive_trajectories" output="screen">
<param name="loop_rate" value="55.0"/>
<param name="desired_yaw_P" value="0.0"/>
<param name="circle_velocity" value="3.5"/> <!-- 3.5 for nice looping; 1.0-2.0 for testing -->
<param name="n_loops" value="1"/>
</node>
<!-- Visualization -->
<node pkg="trajectory_visualizer" name="trajectory_visualizer"
type="trajectory_visualizer" output="screen">
<param name="n_points_to_visualize" value="300"/>
</node>
<node pkg="rviz" type="rviz" name="viz_face"
args="-d $(find fpv_aggressive_trajectories)/rviz/trajectory_comparison_looping_sim.rviz"/>
<!-- node pkg="rqt_gui" name="rqt_gui" type="rqt_gui"
args="- -clear-config - -perspective-file $(find fpv_aggressive_trajectories)/rviz/simulation_looping.perspective"/ -->
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/quadrotor_empty_world_no_controller.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="mav_name" default="$(arg quad_name)"/>
<arg name="model" value="$(find rotors_description)/urdf/mav_generic_odometry_sensor.gazebo"/>
<arg name="world_name" default="$(find rotors_gazebo)/worlds/basic.world"/>
<arg name="x_init" default="0"/>
<arg name="y_init" default="0"/>
<arg name="enable_logging" default="false" />
<arg name="enable_ground_truth" default="true" />
<arg name="log_file" default="$(arg mav_name)" />
<arg name="paused" value="true"/>
<arg name="gui" value="false"/>
<arg name="use_ground_truth" value="true"/>
<arg name="custom_models" default=""/>
<arg name="verbose" default="false"/>
<arg name="debug" default="false"/>
<!-- Basic simulation environment !-->
<include file="$(find rpg_rotors_interface)/launch/basics/base_quad_simulation_no_controller.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
<arg name="custom_models" value="$(arg custom_models)"/>
<arg name="mav_name" value="$(arg mav_name)"/>
<arg name="model" value="$(arg model)"/>
<arg name="enable_logging" value="$(arg enable_logging)"/>
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)"/>
<arg name="log_file" value="$(arg log_file)"/>
<arg name="verbose" default="$(arg verbose)"/>
<arg name="debug" default="$(arg debug)"/>
<arg name="x_init" value="$(arg x_init)"/>
<arg name="y_init" value="$(arg y_init)"/>
</include>
</launch>
================================================
FILE: controller_learning/launch/simulation/quadrotor_rgb_world_no_controller.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="mav_name" default="$(arg quad_name)"/>
<arg name="model" value="$(find fpv_aggressive_trajectories)/resources/urdf/mav_generic_odometry_sensor_rgb.gazebo"/>
<arg name="world_name" default="$(find fpv_aggressive_trajectories)/resources/worlds/controller_learning.world"/>
<arg name="x_init" default="0"/>
<arg name="y_init" default="0"/>
<arg name="enable_logging" default="false"/>
<arg name="enable_ground_truth" default="true"/>
<arg name="log_file" default="$(arg mav_name)"/>
<arg name="paused" value="true"/>
<arg name="gui" default="true"/>
<arg name="use_ground_truth" value="true"/>
<arg name="custom_models" default="$(find fpv_aggressive_trajectories)/resources"/>
<arg name="verbose" default="false"/>
<arg name="debug" default="false"/>
<!-- Basic simulation environment !-->
<include file="$(find controller_learning)/launch/simulation/base_quad_simulation_no_controller.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="world_name" value="$(arg world_name)"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
<arg name="custom_models" value="$(arg custom_models)"/>
<arg name="mav_name" value="$(arg mav_name)"/>
<arg name="model" value="$(arg model)"/>
<arg name="enable_logging" value="$(arg enable_logging)"/>
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)"/>
<arg name="log_file" value="$(arg log_file)"/>
<arg name="verbose" default="$(arg verbose)"/>
<arg name="debug" default="$(arg debug)"/>
<arg name="x_init" value="$(arg x_init)"/>
<arg name="y_init" value="$(arg y_init)"/>
</include>
</launch>
================================================
FILE: controller_learning/launch/simulation/rpg_mpc.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="use_ground_truth"/>
<arg name="enable_command_feedthrough"/>
<arg name="velocity_estimate_in_world_frame"/>
<group ns="$(arg quad_name)">
<group if="$(arg use_ground_truth)">
<node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml"/>
<rosparam file="$(find rpg_mpc)/parameters/default.yaml"/>
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml"/>
<param name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
<param name="state_estimate_timeout" value="0.1"/>
<param name="control_command_delay" value="0.05"/>
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<param name="min_control_period" value="0.02"/>
<remap from="control_command" to="control_command_label"/>
<remap from="autopilot/state_estimate" to="ground_truth/odometry"/>
</node>
</group>
<group unless="$(arg use_ground_truth)">
<node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml"/>
<rosparam file="$(find rpg_mpc)/parameters/default.yaml"/>
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml"/>
<param name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
<param name="state_estimate_timeout" value="0.1"/>
<param name="control_command_delay" value="0.05"/>
<param name="min_control_period" value="0.02"/>
<remap from="control_command" to="control_command_label"/>
<remap from="autopilot/state_estimate" to="odometry_sensor1/odometry"/>
</node>
</group>
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/rpg_pid.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="use_ground_truth"/>
<arg name="enable_command_feedthrough"/>
<arg name="velocity_estimate_in_world_frame"/>
<group ns="$(arg quad_name)">
<group if="$(arg use_ground_truth)">
<node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml"/>
<rosparam file="$(find custom_rotors_interface)/parameters/position_controller.yaml"/>
<rosparam file="$(find custom_rotors_interface)/parameters/autopilot.yaml"/>
<param name="position_controller/use_rate_mode" value="True"/>
<param name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
<param name="state_estimate_timeout" value="0.1"/>
<param name="control_command_delay" value="0.05"/>
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<param name="min_control_period" value="0.02"/>
<remap from="control_command" to="control_command_label"/>
<remap from="autopilot/state_estimate" to="ground_truth/odometry"/>
</node>
</group>
<group unless="$(arg use_ground_truth)">
<node pkg="autopilot" type="autopilot" name="autopilot" output="screen">
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml"/>
<rosparam file="$(find custom_rotors_interface)/parameters/position_controller.yaml"/>
<rosparam file="$(find custom_rotors_interface)/parameters/autopilot.yaml"/>
<param name="position_controller/use_rate_mode" value="True"/>
<param name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
<param name="state_estimate_timeout" value="0.1"/>
<param name="control_command_delay" value="0.05"/>
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<param name="min_control_period" value="0.02"/>
<remap from="control_command" to="control_command_label"/>
<remap from="autopilot/state_estimate" to="odometry_sensor1/odometry"/>
</node>
</group>
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/rpg_rotors_interface.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="use_ground_truth" default="true"/>
<group ns="$(arg quad_name)">
<!-- To debug the rpg_rotors_interface -->
<group if="$(arg use_ground_truth)">
<node pkg="rpg_rotors_interface" type="rpg_rotors_interface"
name="rpg_rotors_interface" clear_params="true" output="screen" launch-prefix="gdb -ex run --args" >
<rosparam file="$(find rpg_rotors_interface)/parameters/rpg_rotors_interface.yaml" />
<remap from="odometry" to="ground_truth/odometry" />
<remap from="rpg_rotors_interface/arm" to="bridge/arm" />
</node>
</group>
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/rviz_only.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="use_ground_truth" default="false"/>
<arg name="controller" default="rpg_mpc"/> <!-- "rpg_mpc", "rpg_pid", "asl_mpc" -->
<group ns="$(arg quad_name)">
<node pkg="rviz" type="rviz" name="viz_face"
args="-d $(find fpv_aggressive_trajectories)/rviz/trajectory_comparison_looping_sim.rviz"/>
</group>
</launch>
================================================
FILE: controller_learning/launch/simulation/simulation.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="use_ground_truth" default="false"/>
<arg name="enable_command_feedthrough" default="true"/>
<arg name="controller" default="rpg_pid"/> <!-- "rpg_mpc", "rpg_pid", "asl_mpc" -->
<!-- Quadrotor simulation -->
<include file="$(find controller_learning)/launch/simulation/quadrotor_rgb_world_no_controller.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="gui" value="false"/>
</include>
<!-- custom RotorS interface -->
<include file="$(find controller_learning)/launch/simulation/custom_rotors_interface.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
</include>
<!-- High Level Controller -->
<include file="$(find controller_learning)/launch/simulation/controller.launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
<arg name="controller" value="$(arg controller)"/>
<arg name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<arg name="velocity_estimate_in_world_frame" value="false"/>
</include>
<group ns="$(arg quad_name)">
<!-- Trajectory Generation -->
<node pkg="fpv_aggressive_trajectories" name="fpv_aggressive_trajectories"
type="fpv_aggressive_trajectories" output="screen">
<param name="loop_rate" value="55.0"/>
<param name="desired_yaw_P" value="0.0"/>
<param name="circle_velocity" value="3.5"/> <!-- 3.5 for nice looping; 1.0-2.0 for testing -->
<param name="n_loops" value="1"/>
</node>
<!-- Visualization -->
<node pkg="trajectory_visualizer" name="trajectory_visualizer"
type="trajectory_visualizer" output="screen">
<param name="n_points_to_visualize" value="300"/>
</node>
<node pkg="rviz" type="rviz" name="viz_face"
args="-d $(find fpv_aggressive_trajectories)/rviz/trajectory_comparison_looping_sim.rviz"/>
<!-- node pkg="rqt_gui" name="rqt_gui" type="rqt_gui"
args="- -clear-config - -perspective-file $(find fpv_aggressive_trajectories)/rviz/simulation_looping.perspective"/ -->
</group>
</launch>
================================================
FILE: controller_learning/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>controller_learning</name>
<version>0.0.0</version>
<description>The controller_learning package</description>
<maintainer email="ekaufmann@ifi.uzh.ch">Elia Kaufmann</maintainer>
<license>TODO</license>
<author>Elia Kaufmann</author>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<depend>autopilot</depend>
<depend>eigen_catkin</depend>
<depend>minimum_jerk_trajectories</depend>
<depend>quadrotor_common</depend>
<depend>quadrotor_msgs</depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>nav_msgs</depend>
<export>
</export>
</package>
================================================
FILE: controller_learning/results/.gitignore
================================================
*
*/
!.gitignore
================================================
FILE: controller_learning/setup.py
================================================
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['ControllerLearning'],
package_dir={'': 'src'}
)
setup(**d)
================================================
FILE: controller_learning/src/ControllerLearning/TrajectoryBase.py
================================================
#!/usr/bin/env python3
import collections
import copy
import csv
import datetime
import os
import random
import numpy as np
import rospy
from nav_msgs.msg import Odometry
from quadrotor_msgs.msg import ControlCommand
from quadrotor_msgs.msg import TrajectoryPoint
from sensor_msgs.msg import PointCloud
from std_msgs.msg import Bool
from std_msgs.msg import Empty
from scipy.spatial.transform import Rotation as R
from .models.bodyrate_learner import BodyrateLearner
TRACK_NUM_NORMALIZE = 10 # Normalization factor for feature length
class TrajectoryBase(object):
def __init__(self, config, mode):
self.config = config
self.odometry = Odometry()
self.gt_odometry = Odometry()
self.rows_buffer = []
self.ref_state = TrajectoryPoint()
self.vins_odometry = None
self.gt_control_command = ControlCommand()
self.counter = 0
self.features = None
self.image = None
self.images_input = None
self.maneuver_complete = False
self.shutdown_node = False
self.record_data = False
self.is_training = False
self.use_network = False
self.net_initialized = False
self.reference_updated = False
self.rollout_idx = 0
self.n_times_net = 0.001 # Due to cope against weird gazebo behaviour
self.n_times_expert = 0
self.mode = mode
self.fts_queue = collections.deque([], maxlen=self.config.seq_len)
self.state_queue = collections.deque([], maxlen=self.config.seq_len)
self.reset_queue()
self.learner = BodyrateLearner(settings=config)
if self.mode == 'training':
return # Nothing to initialize
self.pub_actions = rospy.Publisher("/" + self.config.quad_name + "/control_command",
ControlCommand, queue_size=1)
self.odometry_sub = rospy.Subscriber("/" + self.config.quad_name + "/state_estimate",
Odometry,
self.callback_odometry,
queue_size=1,
tcp_nodelay=True)
self.shutdown_sub = rospy.Subscriber("shutdown_learner", Empty,
self.callback_shutdown,
queue_size=1)
self.ref_sub = rospy.Subscriber("/" + self.config.quad_name + "/vio_reference",
TrajectoryPoint,
self.callback_ref,
queue_size=1,
tcp_nodelay=True)
self.control_command_sub = rospy.Subscriber("/" + self.config.quad_name + "/control_command_label",
ControlCommand,
self.callback_control_command, queue_size=1,
tcp_nodelay=True)
if self.config.use_fts_tracks or self.mode == 'iterative':
self.fts_sub = rospy.Subscriber("/feature_tracker/feature", PointCloud,
self.callback_fts, queue_size=1)
self.traj_done_sub = rospy.Subscriber("/" + self.config.quad_name + "/switch_to_network", Bool,
self.callback_nw_switch, queue_size=1)
self.trajectory_start = rospy.Subscriber("/" + self.config.quad_name + "/trajectory_computation_finish", Bool,
self.callback_start_trajectory, queue_size=10)
if self.mode == "testing":
self.success = 1
def start_data_recording(self):
print("Collecting data")
self.record_data = True
def stop_data_recording(self):
print("Stop data collection")
self.record_data = False
expert_usage = self.n_times_expert / (self.n_times_net + self.n_times_expert)
return expert_usage
def reset_queue(self):
self.fts_queue.clear()
self.state_queue.clear()
self.ref_rot = [0 for _ in range(9)]
self.odom_rot = [0 for _ in range(9)]
init_dict = {}
if self.config.use_imu:
n_init_states = 30
else:
n_init_states = 15
for i in range(self.config.min_number_fts):
init_dict[i] = np.zeros((5,), dtype=np.float32)
for _ in range(self.config.seq_len):
self.fts_queue.append(init_dict)
self.state_queue.append(np.zeros((n_init_states,)))
self.features_input = np.stack([np.stack([v for v in self.fts_queue[j].values()]) \
for j in range(self.config.seq_len)])
def publish_control_command(self, control_command):
self.pub_actions.publish(control_command)
def preprocess_fts(self, data):
features_dict = {}
for i in range(len(data.points)):
ft_id = data.channels[0].values[i]
x = data.points[i].x
y = data.points[i].y
z = data.points[i].z
velocity_x = data.channels[3].values[i]
velocity_y = data.channels[4].values[i]
track_count = 2 * (data.channels[5].values[i] / TRACK_NUM_NORMALIZE) - 1
assert z == 1
feat = np.array([x, y, velocity_x, velocity_y, track_count])
features_dict[ft_id] = feat
return features_dict
def add_missing_fts(self, features_dict):
processed_dict = copy.copy(features_dict)
# Could be both positive or negative
missing_fts = self.config.min_number_fts - len(features_dict.keys())
if missing_fts > 0:
# Features are missing
if missing_fts != self.config.min_number_fts:
# There is something, we can sample
new_features_keys = random.choices(list(features_dict.keys()), k=int(missing_fts))
for j in range(missing_fts):
processed_dict[-j - 1] = features_dict[new_features_keys[j]]
else:
raise IOError("There should not be zero features!")
elif missing_fts < 0:
# There are more features than we need, so sample
del_features_keys = random.sample(features_dict.keys(), int(-missing_fts))
for k in del_features_keys:
del processed_dict[k]
return processed_dict
def callback_fts(self, data):
if (not self.config.use_fts_tracks) and (self.mode == 'testing'):
return
features = self.preprocess_fts(data)
if len(features.keys()) != 0:
# Update features only if something is available
self.features = features
preprocessed_fts = self.add_missing_fts(self.features)
self.fts_queue.append(preprocessed_fts)
self.features_input = np.stack([np.stack([v for v in self.fts_queue[j].values()]) \
for j in range(self.config.seq_len)])
def callback_shutdown(self, data):
self.shutdown_node = True
def callback_nw_switch(self, msg):
self.use_network = False
if msg.data:
# Trajectory is done, stop everything
self.maneuver_complete = True
print("Maneuver is finished")
def callback_start_trajectory(self, data):
# VIO is ready, can fly new trajectory
if data.data:
print("Ready to start trajectory, network on")
self.use_network = True
self.counter = 0
def callback_start(self, data):
print("Callback START")
self.pipeline_off = False
def callback_off(self, data):
print("Callback OFF")
self.pipeline_off = True
def maneuver_finished(self):
return self.maneuver_complete
def callback_odometry(self, data):
self.odometry = data
self.odom_rot = R.from_quat([self.odometry.pose.pose.orientation.x,
self.odometry.pose.pose.orientation.y,
self.odometry.pose.pose.orientation.z,
self.odometry.pose.pose.orientation.w]).as_matrix().reshape((9,)).tolist()
def callback_gt_odometry(self, data):
self.gt_odometry = data
def callback_ref(self, data):
self.ref_state = data
self.ref_rot = R.from_quat([self.ref_state.pose.orientation.x,
self.ref_state.pose.orientation.y,
self.ref_state.pose.orientation.z,
self.ref_state.pose.orientation.w]).as_matrix().reshape((9,)).tolist()
if not self.reference_updated:
self.reference_updated = True
def callback_control_command(self, data):
self.control_command = data
self._generate_control_command()
def shutdown_requested(self):
return self.shutdown_node
def _prepare_net_inputs(self):
if not self.net_initialized:
# return fake input for init
# TODO: change to features
if self.config.use_imu:
n_init_states = 30
else:
n_init_states = 15
inputs = {'fts': np.zeros((1, self.config.seq_len, 40, 5), dtype=np.float64),
'state': np.zeros((1, self.config.seq_len, n_init_states),
dtype=np.float64)}
return inputs
# Reference
state_inputs = self.ref_rot + [ self.ref_state.velocity.linear.x,
self.ref_state.velocity.linear.y,
self.ref_state.velocity.linear.z,
self.ref_state.velocity.angular.x,
self.ref_state.velocity.angular.y,
self.ref_state.velocity.angular.z]
if self.config.use_imu:
imu_states = self.odom_rot + [
self.odometry.twist.twist.linear.x,
self.odometry.twist.twist.linear.y,
self.odometry.twist.twist.linear.z,
self.odometry.twist.twist.angular.x,
self.odometry.twist.twist.angular.y,
self.odometry.twist.twist.angular.z]
state_inputs = imu_states + state_inputs
state_inputs = np.array(state_inputs)
self.state_queue.append(state_inputs)
state_inputs = np.stack(self.state_queue, axis=0)
inputs = {'fts': np.expand_dims(self.features_input, axis=0),
'state': np.expand_dims(state_inputs, axis=0)}
return inputs
def _generate_control_command(self):
pass # Implemented in derived class
def write_csv_header(self):
pass # Implemented in derived class
================================================
FILE: controller_learning/src/ControllerLearning/TrajectoryLearning.py
================================================
#!/usr/bin/env python3
import csv
import datetime
import os
import random
import numpy as np
import rospy
from nav_msgs.msg import Odometry
from quadrotor_msgs.msg import ControlCommand
from std_msgs.msg import Bool
from std_msgs.msg import Empty
from .TrajectoryBase import TrajectoryBase, TRACK_NUM_NORMALIZE
class TrajectoryLearning(TrajectoryBase):
def __init__(self, config, mode):
super(TrajectoryLearning, self).__init__(config, mode)
self.gt_odometry = Odometry()
self.latest_thrust_factor = 1.0 # Init, can be changed for different drones
self.recorded_samples = 0
if self.mode == 'training':
return # Nothing to initialize
self.pub_reset_vio = rospy.Publisher("/feature_tracker/restart",
Bool, queue_size=1)
self.success_subs = rospy.Subscriber("success_reset", Empty,
self.callback_success_reset,
queue_size=1)
self.state_estimate_sub = rospy.Subscriber("/hummingbird/odometry_converted_vio",
Odometry,
self.callback_odometry,
queue_size=1,
tcp_nodelay=True)
self.ground_truth_odom = rospy.Subscriber("/hummingbird/ground_truth/odometry",
Odometry,
self.callback_gt_odometry,
queue_size=1)
self.vins_mono_sub = rospy.Subscriber("/vins_estimator/imu_propagate", Odometry,
self.callback_vins_mono, queue_size=1)
if mode == "iterative" or self.config.verbose:
self.write_csv_header()
if self.mode == "testing":
self.success = 1
def train(self):
self.is_training = True
self.learner.train()
self.is_training = False
self.use_network = False
def callback_success_reset(self, data):
print("Received call to Clear Buffer and Restart Experiment")
os.system("rosservice call /gazebo/pause_physics")
self.rollout_idx += 1
self.reset_queue()
print('Buffer Cleared')
if self.mode == 'testing':
self.success = 1 # We are positive, default is pass
# Init is hacky, but gazebo is very bad!
self.n_times_expert = 0.000
self.n_times_net = 0.001
self.reference_updated = False
os.system("rosservice call /gazebo/unpause_physics")
print('Done Reset')
def callback_gt_odometry(self, data):
self.gt_odometry = data
def callback_vins_mono(self, data):
self.vins_odometry = data
@property
def vio_init_good(self):
max_allowed_velocity = 0.1
if abs(self.vins_odometry.twist.twist.linear.x) < max_allowed_velocity and \
abs(self.vins_odometry.twist.twist.linear.y) < max_allowed_velocity and \
abs(self.vins_odometry.twist.twist.linear.z) < max_allowed_velocity:
return True
else:
return False
def save_data(self):
row = [self.rollout_idx,
self.odometry.header.stamp.to_sec(),
# GT Positon
self.gt_odometry.pose.pose.position.x,
self.gt_odometry.pose.pose.position.y,
self.gt_odometry.pose.pose.position.z,
self.ref_state.pose.position.z - self.gt_odometry.pose.pose.position.z,
self.gt_odometry.pose.pose.orientation.w,
self.gt_odometry.pose.pose.orientation.x,
self.gt_odometry.pose.pose.orientation.y,
self.gt_odometry.pose.pose.orientation.z,
self.gt_odometry.twist.twist.linear.x,
self.gt_odometry.twist.twist.linear.y,
self.gt_odometry.twist.twist.linear.z,
self.gt_odometry.twist.twist.angular.x,
self.gt_odometry.twist.twist.angular.y,
self.gt_odometry.twist.twist.angular.z,
# VIO Estimate
self.odometry.pose.pose.position.x,
self.odometry.pose.pose.position.y,
self.odometry.pose.pose.position.z,
self.ref_state.pose.position.z - self.odometry.pose.pose.position.z,
self.odometry.pose.pose.orientation.w,
self.odometry.pose.pose.orientation.x,
self.odometry.pose.pose.orientation.y,
self.odometry.pose.pose.orientation.z,
self.odometry.twist.twist.linear.x,
self.odometry.twist.twist.linear.y,
self.odometry.twist.twist.linear.z,
self.odometry.twist.twist.angular.x,
self.odometry.twist.twist.angular.y,
self.odometry.twist.twist.angular.z,
# Reference state
self.ref_state.pose.position.x,
self.ref_state.pose.position.y,
self.ref_state.pose.position.z,
self.ref_state.pose.orientation.w,
self.ref_state.pose.orientation.x,
self.ref_state.pose.orientation.y,
self.ref_state.pose.orientation.z,
self.ref_state.velocity.linear.x,
self.ref_state.velocity.linear.y,
self.ref_state.velocity.linear.z,
self.ref_state.velocity.angular.x,
self.ref_state.velocity.angular.y,
self.ref_state.velocity.angular.z,
# MPC output with GT Position
self.control_command.collective_thrust,
self.control_command.bodyrates.x,
self.control_command.bodyrates.y,
self.control_command.bodyrates.z,
# NET output with GT Position
self.net_control.collective_thrust,
self.net_control.bodyrates.x,
self.net_control.bodyrates.y,
self.net_control.bodyrates.z,
# Maneuver type
0]
if self.record_data and self.gt_odometry.pose.pose.position.z > 0.3 and \
self.control_command.collective_thrust > 0.2:
with open(self.csv_filename, 'a') as writeFile:
writer = csv.writer(writeFile)
writer.writerows([row])
fts_name = '{:08d}.npy'
fts_filename = os.path.join(self.image_save_dir,
fts_name.format(self.recorded_samples))
np.save(fts_filename, self.features)
self.recorded_samples += 1
def callback_control_command(self, data):
self.control_command = data
self._generate_control_command()
if self.mode == 'testing' and self.gt_odometry.pose.pose.position.z < 0.3:
self.success = 0
def compute_trajectory_error(self):
gt_ref = np.array([self.ref_state.pose.position.x,
self.ref_state.pose.position.y,
self.ref_state.pose.position.z])
gt_pos = np.array([self.gt_odometry.pose.pose.position.x,
self.gt_odometry.pose.pose.position.y,
self.gt_odometry.pose.pose.position.z])
results = {"gt_ref": gt_ref, "gt_pos": gt_pos}
return results
def publish_control_command(self, control_command):
control_command.collective_thrust = self.latest_thrust_factor * control_command.collective_thrust
self.pub_actions.publish(control_command)
def _generate_control_command(self):
inputs = self._prepare_net_inputs()
if not self.net_initialized:
# Apply Network to init
results = self.learner.inference(inputs)
print("Net initialized")
self.net_initialized = True
self.publish_control_command(self.control_command)
if not self.use_network or not self.reference_updated \
or (len(inputs['fts'].shape) != 4):
# Will be in here if:
# - starting and VIO init
# - Image queue is not ready, can only run expert
self.publish_control_command(self.control_command)
if self.use_network:
print("Using expert wait for ref")
return
# Use always expert at the beginning (approximately 0.2s) to avoid syncronization problems
if self.counter < 10:
self.counter += 1
self.publish_control_command(self.control_command)
return
# Apply Network
results = self.learner.inference(inputs)
control_command = ControlCommand()
control_command.armed = True
control_command.expected_execution_time = rospy.Time.now()
control_command.control_mode = 2
control_command.collective_thrust = results[0][0].numpy()
control_command.bodyrates.x = results[0][1].numpy()
control_command.bodyrates.y = results[0][2].numpy()
control_command.bodyrates.z = results[0][3].numpy()
self.net_control = control_command
# Log immediately everything to avoid surprises (if required)
if self.record_data:
self.save_data()
# Apply random controller now and then to facilitate exploration
if (self.mode != 'testing') and random.random() < self.config.rand_controller_prob:
self.control_command.collective_thrust += self.config.rand_thrust_mag * (random.random() - 0.5) * 2
self.control_command.bodyrates.x += self.config.rand_rate_mag * (random.random() - 0.5) * 2
self.control_command.bodyrates.y += self.config.rand_rate_mag * (random.random() - 0.5) * 2
self.control_command.bodyrates.z += self.config.rand_rate_mag * (random.random() - 0.5) * 2
self.publish_control_command(self.control_command)
return
# Dagger (on control command label).
d_thrust = control_command.collective_thrust - self.control_command.collective_thrust
d_br_x = control_command.bodyrates.x - self.control_command.bodyrates.x
d_br_y = control_command.bodyrates.y - self.control_command.bodyrates.y
d_br_z = control_command.bodyrates.z - self.control_command.bodyrates.z
if self.config.execute_nw_predictions \
and abs(d_thrust) < self.config.fallback_threshold_rates \
and abs(d_br_x) < self.config.fallback_threshold_rates \
and abs(d_br_y) < self.config.fallback_threshold_rates \
and abs(d_br_z) < self.config.fallback_threshold_rates:
self.n_times_net += 1
self.publish_control_command(control_command)
else:
self.n_times_expert += 1
self.publish_control_command(self.control_command)
def write_csv_header(self):
row = ["Rollout_idx",
"Odometry_stamp",
# GT Position
"gt_Position_x",
"gt_Position_y",
"gt_Position_z",
"gt_Position_z_error",
"gt_Orientation_w",
"gt_Orientation_x",
"gt_Orientation_y",
"gt_Orientation_z",
"gt_V_linear_x",
"gt_V_linear_y",
"gt_V_linear_z",
"gt_V_angular_x",
"gt_V_angular_y",
"gt_V_angular_z",
# VIO Estimate
"Position_x",
"Position_y",
"Position_z",
"Position_z_error",
"Orientation_w",
"Orientation_x",
"Orientation_y",
"Orientation_z",
"V_linear_x",
"V_linear_y",
"V_linear_z",
"V_angular_x",
"V_angular_y",
"V_angular_z",
# Reference state
"Reference_position_x",
"Reference_position_y",
"Reference_position_z",
"Reference_orientation_w",
"Reference_orientation_x",
"Reference_orientation_y",
"Reference_orientation_z",
"Reference_v_linear_x",
"Reference_v_linear_y",
"Reference_v_linear_z",
"Reference_v_angular_x",
"Reference_v_angular_y",
"Reference_v_angular_z",
# MPC output with GT Postion
"Gt_control_command_collective_thrust",
"Gt_control_command_bodyrates_x",
"Gt_control_command_bodyrates_y",
"Gt_control_command_bodyrates_z",
# Net output
"Net_control_command_collective_thrust",
"Net_control_command_bodyrates_x",
"Net_control_command_bodyrates_y",
"Net_control_command_bodyrates_z",
"Maneuver_type"]
current_time = datetime.datetime.now().strftime("%Y%m%d-%H%M%S")
if self.mode == 'iterative':
root_save_dir = self.config.train_dir
else:
root_save_dir = self.config.log_dir
self.csv_filename = os.path.join(root_save_dir, "data_" + current_time + ".csv")
self.image_save_dir = os.path.join(root_save_dir, "img_data_" + current_time)
if not os.path.exists(self.image_save_dir):
os.makedirs(self.image_save_dir)
with open(self.csv_filename, 'w') as writeFile:
writer = csv.writer(writeFile)
writer.writerows([row])
================================================
FILE: controller_learning/src/ControllerLearning/__init__.py
================================================
================================================
FILE: controller_learning/src/ControllerLearning/models/__init__.py
================================================
================================================
FILE: controller_learning/src/ControllerLearning/models/body_dataset.py
================================================
import numpy as np
import glob
import os
import pandas as pd
import tensorflow as tf
import fnmatch
import cv2
import random
from scipy.spatial.transform import Rotation as R
def create_dataset(directory, settings, training=True):
dataset = SafeDataset(directory, settings, training)
return dataset
class BodyDataset:
"""
Base Dataset Class
"""
def __init__(self, directory, config, training=True):
self.config = config
self.directory = directory
self.training = training
self.samples = 0
self.experiments = []
self.features = []
self.labels = []
self.filenames = []
self.stacked_filenames = [] # Will be used for passing stacked fnames
img_rootname = 'img_data'
for root, dirs, files in os.walk(directory, topdown=True, followlinks=True):
for name in dirs:
if name.startswith(img_rootname):
exp_dir = os.path.join(root, name)
self.experiments.append(os.path.abspath(exp_dir))
self.num_experiments = len(self.experiments)
self.img_format = 'npy'
self.data_format = 'csv'
for exp_dir in self.experiments:
try:
self._decode_experiment_dir(exp_dir)
except:
raise ImportWarning("Image reading in {} failed".format(
exp_dir))
if self.samples == 0:
raise IOError("Did not find any file in the dataset folder")
print('Found {} images belonging to {} experiments:'.format(
self.samples, self.num_experiments))
def _recursive_list(self, subpath, fmt='jpg'):
return fnmatch.filter(os.listdir(subpath), '*.{}'.format(fmt))
def build_dataset(self):
self._build_dataset()
def _build_dataset(self):
raise NotImplementedError
def _decode_experiment_dir(self, directory):
raise NotImplementedError
class SafeDataset(BodyDataset):
def __init__(self, directory, config, training=True):
super(SafeDataset, self).__init__(directory, config, training)
self.build_dataset()
def _decode_experiment_dir(self, dir_subpath):
base_path = os.path.basename(dir_subpath)
parent_dict = os.path.dirname(dir_subpath)
data_name = 'data' + base_path[8:] + ".csv"
data_name = os.path.join(parent_dict, data_name)
assert os.path.isfile(data_name), "Not Found data file"
df = pd.read_csv(data_name, delimiter=',')
num_files = df.shape[0]
num_images = len(self._recursive_list(dir_subpath, fmt=self.img_format))
assert num_files == num_images, "Number of features and images does not match"
features_imu = [# VIO Estimate
"Orientation_x",
"Orientation_y",
"Orientation_z",
"Orientation_w",
"V_linear_x",
"V_linear_y",
"V_linear_z",
"V_angular_x",
"V_angular_y",
"V_angular_z"]
features = [# Reference state
"Reference_orientation_x",
"Reference_orientation_y",
"Reference_orientation_z",
"Reference_orientation_w",
"Reference_v_linear_x",
"Reference_v_linear_y",
"Reference_v_linear_z",
"Reference_v_angular_x",
"Reference_v_angular_y",
"Reference_v_angular_z"]
# Preprocessing: we select the good rollouts (no crash in training data)
rollout_fts = ["Rollout_idx"]
rollout_fts_v = df[rollout_fts].values
position_gt = ["gt_Position_x",
"gt_Position_y",
"gt_Position_z"]
position_gt_v = df[position_gt].values
position_ref = ["Reference_position_x",
"Reference_position_y",
"Reference_position_z"]
position_ref_v = df[position_ref].values
good_rollouts = []
if rollout_fts_v.shape[0] == 0:
return
for r in np.arange(1,np.max(rollout_fts_v)+1):
rollout_positions = rollout_fts_v == r
roll_gt = position_gt_v[np.squeeze(rollout_positions),:]
roll_ref = position_ref_v[np.squeeze(rollout_positions),:]
if roll_gt.shape[0] == 0:
continue
assert roll_ref.shape == roll_gt.shape
error = np.mean(np.linalg.norm(roll_gt - roll_ref, axis=1))
if error < self.config.max_allowed_error:
good_rollouts.append(r)
if self.config.use_imu:
features = ["Rollout_idx"] + features_imu + features
else:
features = ["Rollout_idx"] + features
labels = ["Gt_control_command_collective_thrust",
"Gt_control_command_bodyrates_x",
"Gt_control_command_bodyrates_y",
"Gt_control_command_bodyrates_z"]
features_v = df[features].values
labels_v = df[labels].values
for frame_number in range(num_files):
is_valid = False
img_fname = os.path.join(dir_subpath,
"{:08d}.{}".format(frame_number, self.img_format))
if os.path.isfile(img_fname) and (rollout_fts_v[frame_number] in good_rollouts):
is_valid = True
if is_valid:
self.features.append(self.preprocess_fts(features_v[frame_number]))
self.labels.append(labels_v[frame_number])
if self.config.use_fts_tracks:
self.filenames.append(img_fname)
self.samples += 1
def preprocess_fts(self, fts):
"""
Converts rotations from quadrans to rotation matrix.
Fts have the following indexing.
rollout_idx, qx,qy,qz,qw, vx, vy, vz, ax, ay, az, rqx, rqy, rqz, rqw, ...
"""
fts = fts.tolist()
ref_rot = R.from_quat(fts[11:15]).as_matrix().reshape((9,)).tolist()
if self.config.use_imu:
odom_rot = R.from_quat(fts[1:5]).as_matrix().reshape((9,)).tolist()
processed_fts = [fts[0]] + odom_rot + fts[5:11] + ref_rot + fts[15:]
else:
processed_fts = [fts[0]] + ref_rot + fts[5:11]
return np.array(processed_fts)
def add_missing_fts(self, features_dict):
processed_dict = features_dict
# Could be both positive or negative
missing_fts = self.config.min_number_fts - len(features_dict.keys())
if missing_fts > 0:
# Features are missing
if missing_fts != self.config.min_number_fts:
# There is something, we can sample
new_features_keys = random.choices(list(features_dict.keys()), k=int(missing_fts))
for j in range(missing_fts):
processed_dict[-j-1] = features_dict[new_features_keys[j]]
else:
# Zero features, this is a transient
for j in range(missing_fts):
processed_dict[-j-1] = np.zeros((5,))
elif missing_fts < 0:
# There are more features than we need, so sample
del_features_keys = random.sample(features_dict.keys(), int(-missing_fts))
for k in del_features_keys:
del processed_dict[k]
return processed_dict
def load_fts_sequence(self, sample_num):
fts_seq = []
sample_num_np = sample_num.numpy()
filenames_num = self.stacked_filenames[sample_num_np]
for idx in range(self.config.seq_len):
fname_idx = filenames_num[idx]
if fname_idx < 0:
fts = {}
else:
fname = self.filenames[fname_idx]
fts = np.load(fname, allow_pickle=True).item()
fts_seq.append(fts)
# Reverse list to have it ordered in time (t-seq_len, ..., t)
fts_seq = reversed(fts_seq)
# Crop to the required lenght
fts_seq = [self.add_missing_fts(ft) for ft in fts_seq]
# Stack
features_input = np.stack([np.stack([v for v in fts_seq[j].values()]) \
for j in range(self.config.seq_len)])
return features_input
def _dataset_map(self, sample_num):
# First is rollout idx
label = tf.gather(self.labels, sample_num)
state_seq = []
# For states is easy: nothing to do.
for idx in reversed(range(self.config.seq_len)):
state = tf.gather(self.features, sample_num - idx)[1:]
state_seq.append(state)
state_seq = tf.stack(state_seq)
# For images, take care they do not overlap
if self.config.use_fts_tracks:
fts_seq = tf.py_function(func=self.load_fts_sequence,
inp=[sample_num],
Tout=tf.float32)
return (state_seq, fts_seq), label
else:
return state_seq, label
def check_equal_dict(self, d1, d2):
for k_1, v_1 in d1.items():
try:
v_2 = d2[k_1]
except:
return False
if not np.array_equal(v_1,v_2):
return False
return True
def _preprocess_fnames(self):
# Append filenames up to seq_len for fast loading.
# A bit ugly and inefficent, can be improved
self.last_init_fts = None
for k in range(len(self.filenames)):
if k % 3000 == 0:
print("Built {:.2f}% of the dataset".format(
k/len(self.filenames)*100), end='\r')
# Check if you can copy the things before
kth_fts = self.filenames[k]
kth_fts = np.load(kth_fts, allow_pickle=True).item()
if k > 0:
if self.check_equal_dict(self.last_init_fts, kth_fts):
self.stacked_filenames.append(self.stacked_filenames[-1])
continue
# This is the lastest observed feature track different from others
self.last_init_fts = kth_fts
idx = 0
rollout_idxes = []
fname_seq = []
fts_seq = []
while len(fts_seq) < self.config.seq_len:
if k - idx < 0:
#this is transient, can only append zeros
fname_seq.append(-1)
fts_seq.append(0.)
continue
current_idx = k - idx
rollout_idx = self.features[current_idx][0]
if idx == 0:
fname_seq.append(current_idx)
fts_seq.append(kth_fts)
rollout_idxes.append(rollout_idx)
else:
if rollout_idx != rollout_idxes[-1]:
# it is a transient! Can only append zeros.
fname_seq.append(-1)
fts_seq.append(0.)
else:
# Check the features are different
fname = self.filenames[current_idx]
fts = np.load(fname, allow_pickle=True).item()
if not self.check_equal_dict(fts, fts_seq[-1]):
# Objects are not equal, can append
fname_seq.append(current_idx)
fts_seq.append(fts)
rollout_idxes.append(rollout_idx)
idx += 1
assert len(fts_seq) == len(fname_seq)
self.stacked_filenames.append(fname_seq)
# EndFor
assert len(self.filenames) == len(self.stacked_filenames)
def _build_dataset(self):
# Need to take care that rollout_idxs are consistent
self.features = np.stack(self.features)
self.features = self.features.astype(np.float32)
self.labels = np.stack(self.labels)
self.labels = self.labels.astype(np.float32)
last_fname_numbers = []
if self.config.use_fts_tracks:
self._preprocess_fnames()
# Preprocess filenames to assess consistency of experiment
for idx in range(self.config.seq_len-1, self.samples):
if self.features[idx,0] == self.features[idx-self.config.seq_len+1,0]:
last_fname_numbers.append(np.int32(idx))
if self.training:
np.random.shuffle(last_fname_numbers)
# Form training batches
dataset = tf.data.Dataset.from_tensor_slices(last_fname_numbers)
if self.training:
dataset = dataset.shuffle(buffer_size=len(last_fname_numbers))
dataset = dataset.map(self._dataset_map,
num_parallel_calls=10 if self.training else 1)
dataset = dataset.batch(self.config.batch_size,
drop_remainder=not self.training)
dataset = dataset.prefetch(buffer_size=10*self.config.batch_size)
self.batched_dataset = dataset
================================================
FILE: controller_learning/src/ControllerLearning/models/bodyrate_learner.py
================================================
import datetime
import os
import numpy as np
import tensorflow as tf
from tqdm import tqdm
# Required for catkin import strategy
try:
from .nets import create_network
from .body_dataset import create_dataset
except:
from nets import create_network
from body_dataset import create_dataset
os.environ['TF_CPP_MIN_LOG_LEVEL'] = '2'
class BodyrateLearner(object):
def __init__(self, settings):
self.config = settings
physical_devices = tf.config.experimental.list_physical_devices('GPU')
if len(physical_devices) > 0:
tf.config.experimental.set_memory_growth(physical_devices[0], True)
self.min_val_loss = tf.Variable(np.inf,
name='min_val_loss',
trainable=False)
self.network = create_network(self.config)
self.loss = tf.keras.losses.MeanSquaredError()
self.optimizer = tf.keras.optimizers.Adam(learning_rate=1e-4, clipvalue=.2)
self.train_loss = tf.keras.metrics.Mean(name='train_loss')
self.val_loss = tf.keras.metrics.Mean(name='validation_loss')
self.global_epoch = tf.Variable(0)
self.ckpt = tf.train.Checkpoint(step=self.global_epoch,
optimizer=self.optimizer,
net=self.network)
if self.config.resume_training:
if self.ckpt.restore(self.config.resume_ckpt_file):
print("------------------------------------------")
print("Restored from {}".format(self.config.resume_ckpt_file))
print("------------------------------------------")
return
print("------------------------------------------")
print("Initializing from scratch.")
print("------------------------------------------")
@tf.function
def train_step(self, inputs, labels):
with tf.GradientTape() as tape:
predictions = self.network(inputs)
loss = self.loss(labels, predictions)
gradients = tape.gradient(loss, self.network.trainable_variables)
self.optimizer.apply_gradients(zip(gradients, self.network.trainable_variables))
self.train_loss.update_state(loss)
return gradients
@tf.function
def val_step(self, inputs, labels):
predictions = self.network(inputs)
loss = self.loss(labels, predictions)
self.val_loss.update_state(loss)
def adapt_input_data(self, features):
if self.config.use_fts_tracks:
inputs = {"fts": features[1],
"state": features[0]}
else:
inputs = {"state": features}
return inputs
def write_train_summaries(self, features, gradients):
with self.summary_writer.as_default():
tf.summary.scalar('Train Loss', self.train_loss.result(),
step=self.optimizer.iterations)
for g, v in zip(gradients, self.network.trainable_variables):
tf.summary.histogram(v.name, g, step=self.optimizer.iterations)
def train(self):
print("Training Network")
if not hasattr(self, 'train_log_dir'):
# This should be done only once
self.train_log_dir = os.path.join(self.config.log_dir, 'train')
self.summary_writer = tf.summary.create_file_writer(self.train_log_dir)
self.ckpt_manager = tf.train.CheckpointManager(self.ckpt, self.train_log_dir, max_to_keep=10)
else:
# We are in dagger mode, so let us reset the best loss
self.min_val_loss = np.inf
self.train_loss.reset_states()
self.val_loss.reset_states()
dataset_train = create_dataset(self.config.train_dir,
self.config, training=True)
dataset_val = create_dataset(self.config.val_dir,
self.config, training=False)
for epoch in range(self.config.max_training_epochs):
# Train
for k, (features, label) in enumerate(tqdm(dataset_train.batched_dataset)):
features = self.adapt_input_data(features)
gradients = self.train_step(features, label)
if tf.equal(k % self.config.summary_freq, 0):
self.write_train_summaries(features, gradients)
self.train_loss.reset_states()
# Eval
for features, label in tqdm(dataset_val.batched_dataset):
features = self.adapt_input_data(features)
self.val_step(features, label)
validation_loss = self.val_loss.result()
with self.summary_writer.as_default():
tf.summary.scalar("Validation Loss", validation_loss, step=tf.cast(self.global_epoch, dtype=tf.int64))
self.val_loss.reset_states()
self.global_epoch = self.global_epoch + 1
self.ckpt.step.assign_add(1)
print("Epoch: {:2d}, Validation Loss: {:.4f}".format(self.global_epoch, validation_loss))
if validation_loss < self.min_val_loss or ((epoch + 1) % self.config.save_every_n_epochs) == 0:
if validation_loss < self.min_val_loss:
self.min_val_loss = validation_loss
save_path = self.ckpt_manager.save()
print("Saved checkpoint for epoch {}: {}".format(int(self.ckpt.step), save_path))
# Reset the metrics for the next epoch
print("------------------------------")
print("Training finished successfully")
print("------------------------------")
def test(self):
print("Testing Network")
self.train_log_dir = os.path.join(self.config.log_dir, 'test')
dataset_val = create_dataset(self.config.test_dir,
self.config, training=False)
for features, label in tqdm(dataset_val.batched_dataset):
features = self.adapt_input_data(features)
self.val_step(features, label)
validation_loss = self.val_loss.result()
self.val_loss.reset_states()
print("Testing Loss: {:.4f}".format(validation_loss))
@tf.function
def inference(self, inputs):
predictions = self.network(inputs)
return predictions
================================================
FILE: controller_learning/src/ControllerLearning/models/nets.py
================================================
import tensorflow as tf
from tensorflow.keras import Model
from tensorflow.keras.layers import Dense, Conv2D, LeakyReLU, Conv1D
from tensorflow.keras.layers import Flatten, GlobalAveragePooling2D
try:
from .tf_addons_normalizations import InstanceNormalization
except:
from tf_addons_normalizations import InstanceNormalization
def create_network(settings):
net = AggressiveNet(settings)
return net
class Network(Model):
def __init__(self):
super(Network, self).__init__()
def create(self):
self._create()
def call(self, x):
return self._internal_call(x)
def _create(self):
raise NotImplementedError
def _internal_call(self):
raise NotImplementedError
class AggressiveNet(Network):
def __init__(self, config):
super(AggressiveNet, self).__init__()
self.config = config
self._create(input_size=(config.seq_len, config.min_number_fts, 5))
def _create(self, input_size, has_bias=True, learn_affine=True):
"""Init.
Args:
input_size (float): size of input
has_bias (bool, optional): Defaults to True. Conv1d bias?
learn_affine (bool, optional): Defaults to True. InstanceNorm1d affine?
"""
if self.config.use_fts_tracks:
f = 2.0
self.pointnet = [Conv2D(int(16 * f), kernel_size=1, strides=1, padding='valid',
dilation_rate=1, use_bias=has_bias, input_shape=input_size),
InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine),
LeakyReLU(alpha=1e-2),
Conv2D(int(32 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1,
use_bias=has_bias),
InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine),
LeakyReLU(alpha=1e-2),
Conv2D(int(64 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1,
use_bias=has_bias),
InstanceNormalization(axis=3, epsilon=1e-5, center=learn_affine, scale=learn_affine),
LeakyReLU(alpha=1e-2),
Conv2D(int(64 * f), kernel_size=1, strides=1, padding='valid', dilation_rate=1,
use_bias=has_bias),
GlobalAveragePooling2D()]
input_size = (self.config.seq_len, int(64*f))
self.fts_mergenet = [Conv1D(int(64 * f), kernel_size=2, strides=1, padding='same',
dilation_rate=1, input_shape=input_size),
LeakyReLU(alpha=1e-2),
Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1),
LeakyReLU(alpha=1e-2),
Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1),
LeakyReLU(alpha=1e-2),
Conv1D(int(32 * f), kernel_size=2, strides=1, padding='same', dilation_rate=1),
LeakyReLU(alpha=1e-2),
Flatten(),
Dense(int(64*f))]
g = 2.0
self.states_conv = [Conv1D(int(64 * g), kernel_size=2, strides=1, padding='same',
dilation_rate=1),
LeakyReLU(alpha=1e-2),
Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1),
LeakyReLU(alpha=1e-2),
Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1),
LeakyReLU(alpha=1e-2),
Conv1D(int(32 * g), kernel_size=2, strides=1, padding='same', dilation_rate=1),
Flatten(),
Dense(int(64*g))]
self.control_module = [Dense(64*g),
LeakyReLU(alpha=1e-2),
Dense(32*g),
LeakyReLU(alpha=1e-2),
Dense(16*g),
LeakyReLU(alpha=1e-2),
Dense(4)]
def _pointnet_branch(self, single_t_features):
x = tf.expand_dims(single_t_features, axis=1)
for f in self.pointnet:
x = f(x)
return x
def _features_branch(self, input_features):
preprocessed_fts = tf.map_fn(self._pointnet_branch,
elems=input_features,
parallel_iterations=self.config.seq_len) # (seq_len, batch_size, 64)
preprocessed_fts = tf.transpose(preprocessed_fts, (1,0,2)) # (batch_size, seq_len, 64)
x = preprocessed_fts
for f in self.fts_mergenet:
x = f(x)
return x
def _states_branch(self, embeddings):
x = embeddings
for f in self.states_conv:
x = f(x)
return x
def _control_branch(self, embeddings):
x = embeddings
for f in self.control_module:
x = f(x)
return x
def _internal_call(self, inputs):
states = inputs['state']
if self.config.use_fts_tracks:
fts_stack = inputs['fts'] # (batch_size, seq_len, min_numb_features, 5)
fts_stack = tf.transpose(fts_stack, (1,0,2,3)) # (seq_len, batch_size, min_numb_features, 5)
# Execute PointNet Part
fts_embeddings = self._features_branch(fts_stack)
states_embeddings = self._states_branch(states)
if self.config.use_fts_tracks:
total_embeddings = tf.concat((fts_embeddings, states_embeddings), axis=1)
else:
total_embeddings = states_embeddings
output = self._control_branch(total_embeddings)
return output
================================================
FILE: controller_learning/src/ControllerLearning/models/tf_addons_normalizations.py
================================================
# Copyright 2019 The TensorFlow Authors. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# Orginal implementation from keras_contrib/layer/normalization
# The Authors of the drone_acrobatics repository do not own any right on this file.
# If you have problems with this file, install the tf_addons repository directly.
# =============================================================================
from __future__ import absolute_import
from __future__ import division
from __future__ import print_function
import tensorflow as tf
class GroupNormalization(tf.keras.layers.Layer):
"""Group normalization layer.
Group Normalization divides the channels into groups and computes
within each group the mean and variance for normalization.
Empirically, its accuracy is more stable than batch norm in a wide
range of small batch sizes, if learning rate is adjusted linearly
with batch sizes.
Relation to Layer Normalization:
If the number of groups is set to 1, then this operation becomes identical
to Layer Normalization.
Relation to Instance Normalization:
If the number of groups is set to the
input dimension (number of groups is equal
to number of channels), then this operation becomes
identical to Instance Normalization.
Arguments
groups: Integer, the number of groups for Group Normalization.
Can be in the range [1, N] where N is the input dimension.
The input dimension must be divisible by the number of groups.
axis: Integer, the axis that should be normalized.
epsilon: Small float added to variance to avoid dividing by zero.
center: If True, add offset of `beta` to normalized tensor.
If False, `beta` is ignored.
scale: If True, multiply by `gamma`.
If False, `gamma` is not used.
beta_initializer: Initializer for the beta weight.
gamma_initializer: Initializer for the gamma weight.
beta_regularizer: Optional regularizer for the beta weight.
gamma_regularizer: Optional regularizer for the gamma weight.
beta_constraint: Optional constraint for the beta weight.
gamma_constraint: Optional constraint for the gamma weight.
Input shape
Arbitrary. Use the keyword argument `input_shape`
(tuple of integers, does not include the samples axis)
when using this layer as the first layer in a model.
Output shape
Same shape as input.
References
- [Group Normalization](https://arxiv.org/abs/1803.08494)
"""
def __init__(self,
groups=2,
axis=-1,
epsilon=1e-3,
center=True,
scale=True,
beta_initializer='zeros',
gamma_initializer='ones',
beta_regularizer=None,
gamma_regularizer=None,
beta_constraint=None,
gamma_constraint=None,
**kwargs):
super(GroupNormalization, self).__init__(**kwargs)
self.supports_masking = True
self.groups = groups
self.axis = axis
self.epsilon = epsilon
self.center = center
self.scale = scale
self.beta_initializer = tf.keras.initializers.get(beta_initializer)
self.gamma_initializer = tf.keras.initializers.get(gamma_initializer)
self.beta_regularizer = tf.keras.regularizers.get(beta_regularizer)
self.gamma_regularizer = tf.keras.regularizers.get(gamma_regularizer)
self.beta_constraint = tf.keras.constraints.get(beta_constraint)
self.gamma_constraint = tf.keras.constraints.get(gamma_constraint)
self._check_axis()
def build(self, input_shape):
self._check_if_input_shape_is_none(input_shape)
self._set_number_of_groups_for_instance_norm(input_shape)
self._check_size_of_dimensions(input_shape)
self._create_input_spec(input_shape)
self._add_gamma_weight(input_shape)
self._add_beta_weight(input_shape)
self.built = True
super(GroupNormalization, self).build(input_shape)
def call(self, inputs):
input_shape = tf.keras.backend.int_shape(inputs)
tensor_input_shape = tf.shape(inputs)
reshaped_inputs, group_shape = self._reshape_into_groups(
inputs, input_shape, tensor_input_shape)
normalized_inputs = self._apply_normalization(reshaped_inputs,
input_shape)
outputs = tf.reshape(normalized_inputs, tensor_input_shape)
return outputs
def get_config(self):
config = {
'groups':
self.groups,
'axis':
self.axis,
'epsilon':
self.epsilon,
'center':
self.center,
'scale':
self.scale,
'beta_initializer':
tf.keras.initializers.serialize(self.beta_initializer),
'gamma_initializer':
tf.keras.initializers.serialize(self.gamma_initializer),
'beta_regularizer':
tf.keras.regularizers.serialize(self.beta_regularizer),
'gamma_regularizer':
tf.keras.regularizers.serialize(self.gamma_regularizer),
'beta_constraint':
tf.keras.constraints.serialize(self.beta_constraint),
'gamma_constraint':
tf.keras.constraints.serialize(self.gamma_constraint)
}
base_config = super(GroupNormalization, self).get_config()
return dict(list(base_config.items()) + list(config.items()))
def compute_output_shape(self, input_shape):
return input_shape
def _reshape_into_groups(self, inputs, input_shape, tensor_input_shape):
group_shape = [tensor_input_shape[i] for i in range(len(input_shape))]
group_shape[self.axis] = input_shape[self.axis] // self.groups
group_shape.insert(1, self.groups)
group_shape = tf.stack(group_shape)
reshaped_inputs = tf.reshape(inputs, group_shape)
return reshaped_inputs, group_shape
def _apply_normalization(self, reshaped_inputs, input_shape):
group_shape = tf.keras.backend.int_shape(reshaped_inputs)
group_reduction_axes = list(range(len(group_shape)))
# Remember the ordering of the tensor is [batch, group , steps]. Jump
# the first 2 to calculate the variance and the mean
mean, variance = tf.nn.moments(
reshaped_inputs, group_reduction_axes[2:], keepdims=True)
gamma, beta = self._get_reshaped_weights(input_shape)
normalized_inputs = tf.nn.batch_normalization(
reshaped_inputs,
mean=mean,
variance=variance,
scale=gamma,
offset=beta,
variance_epsilon=self.epsilon)
return normalized_inputs
def _get_reshaped_weights(self, input_shape):
broadcast_shape = self._create_broadcast_shape(input_shape)
gamma = None
beta = None
if self.scale:
gamma = tf.reshape(self.gamma, broadcast_shape)
if self.center:
beta = tf.reshape(self.beta, broadcast_shape)
return gamma, beta
def _check_if_input_shape_is_none(self, input_shape):
dim = input_shape[self.axis]
if dim is None:
raise ValueError('Axis ' + str(self.axis) + ' of '
'input tensor should have a defined dimension '
'but the layer received an input with shape ' +
str(input_shape) + '.')
def _set_number_of_groups_for_instance_norm(self, input_shape):
dim = input_shape[self.axis]
if self.groups == -1:
self.groups = dim
def _check_size_of_dimensions(self, input_shape):
dim = input_shape[self.axis]
if dim < self.groups:
raise ValueError(
'Number of groups (' + str(self.groups) + ') cannot be '
'more than the number of channels (' + str(dim) + ').')
if dim % self.groups != 0:
raise ValueError(
'Number of groups (' + str(self.groups) + ') must be a '
'multiple of the number of channels (' + str(dim) + ').')
def _check_axis(self):
if self.axis == 0:
raise ValueError(
"You are trying to normalize your batch axis. Do you want to "
"use tf.layer.batch_normalization instead")
def _create_input_spec(self, input_shape):
dim = input_shape[self.axis]
self.input_spec = tf.keras.layers.InputSpec(
ndim=len(input_shape), axes={self.axis: dim})
def _add_gamma_weight(self, input_shape):
dim = input_shape[self.axis]
shape = (dim,)
if self.scale:
self.gamma = self.add_weight(
shape=shape,
name='gamma',
initializer=self.gamma_initializer,
regularizer=self.gamma_regularizer,
constraint=self.gamma_constraint)
else:
self.gamma = None
def _add_beta_weight(self, input_shape):
dim = input_shape[self.axis]
shape = (dim,)
if self.center:
self.beta = self.add_weight(
shape=shape,
name='beta',
initializer=self.beta_initializer,
regularizer=self.beta_regularizer,
constraint=self.beta_constraint)
else:
self.beta = None
def _create_broadcast_shape(self, input_shape):
broadcast_shape = [1] * len(input_shape)
broadcast_shape[self.axis] = input_shape[self.axis] // self.groups
broadcast_shape.insert(1, self.groups)
return broadcast_shape
# @keras_utils.register_keras_custom_object
class InstanceNormalization(GroupNormalization):
"""Instance normalization layer.
Instance Normalization is an specific case of ```GroupNormalization```since
it normalizes all features of one channel. The Groupsize is equal to the
channel size. Empirically, its accuracy is more stable than batch norm in a
wide range of small batch sizes, if learning rate is adjusted linearly
with batch sizes.
Arguments
axis: Integer, the axis that should be normalized.
epsilon: Small float added to variance to avoid dividing by zero.
center: If True, add offset of `beta` to normalized tensor.
If False, `beta` is ignored.
scale: If True, multiply by `gamma`.
If False, `gamma` is not used.
beta_initializer: Initializer for the beta weight.
gamma_initializer: Initializer for the gamma weight.
beta_regularizer: Optional regularizer for the beta weight.
gamma_regularizer: Optional regularizer for the gamma weight.
beta_constraint: Optional constraint for the beta weight.
gamma_constraint: Optional constraint for the gamma weight.
Input shape
Arbitrary. Use the keyword argument `input_shape`
(tuple of integers, does not include the samples axis)
when using this layer as the first layer in a model.
Output shape
Same shape as input.
References
- [Instance Normalization: The Missing Ingredient for Fast Stylization]
(https://arxiv.org/abs/1607.08022)
"""
def __init__(self, **kwargs):
kwargs["groups"] = -1
super(InstanceNormalization, self).__init__(**kwargs)
================================================
FILE: controller_learning/test_trajectories.py
================================================
import argparse
from common import setup_sim, update_mpc_params
from config.settings import create_settings
from iterative_learning_trajectories import Trainer
def main():
parser = argparse.ArgumentParser(description='Evaluate Trajectory tracker.')
parser.add_argument('--settings_file', help='Path to settings yaml', required=True)
args = parser.parse_args()
settings_filepath = args.settings_file
settings = create_settings(settings_filepath, mode='test')
update_mpc_params()
setup_sim()
trainer = Trainer(settings)
trainer.perform_testing()
if __name__ == "__main__":
main()
================================================
FILE: controller_learning/train.py
================================================
#!/usr/bin/env python3
import argparse
import os
import sys
sys.path.append("./src/ControllerLearning/models/")
import time
#import rospy
from bodyrate_learner import BodyrateLearner
from config.settings import create_settings
def main():
parser = argparse.ArgumentParser(description='Train Network')
parser.add_argument('--settings_file', help='Path to settings yaml', required=True)
args = parser.parse_args()
settings_filepath = args.settings_file
settings = create_settings(settings_filepath, mode='train')
learner = BodyrateLearner(settings=settings)
learner.train()
if __name__ == "__main__":
main()
================================================
FILE: controller_learning/train_logs/20191015-164750/train/checkpoint
================================================
model_checkpoint_path: "ckpt-80"
all_model_checkpoint_paths: "ckpt-78"
all_model_checkpoint_paths: "ckpt-79"
all_model_checkpoint_paths: "ckpt-80"
all_model_checkpoint_timestamps: 1571151095.56399
all_model_checkpoint_timestamps: 1571151096.0123613
all_model_checkpoint_timestamps: 1571151096.455976
last_preserved_timestamp: 1571150869.450073
================================================
FILE: controller_learning/train_logs/20191022-082718/train/checkpoint
================================================
model_checkpoint_path: "ckpt-1080"
all_model_checkpoint_paths: "ckpt-1078"
all_model_checkpoint_paths: "ckpt-1079"
all_model_checkpoint_paths: "ckpt-1080"
all_model_checkpoint_timestamps: 1571726594.0011396
all_model_checkpoint_timestamps: 1571726594.9023852
all_model_checkpoint_timestamps: 1571726595.8179815
last_preserved_timestamp: 1571725637.457743
================================================
FILE: controller_learning/train_logs/20191022-095948/train/checkpoint
================================================
model_checkpoint_path: "ckpt-1000"
all_model_checkpoint_paths: "ckpt-998"
all_model_checkpoint_paths: "ckpt-999"
all_model_checkpoint_paths: "ckpt-1000"
all_model_checkpoint_timestamps: 1571733868.2468255
all_model_checkpoint_timestamps: 1571733870.8290527
all_model_checkpoint_timestamps: 1571733873.409515
last_preserved_timestamp: 1571731187.6806133
================================================
FILE: custom_rotors_interface/.gitignore
================================================
cmake-build-debug/
================================================
FILE: custom_rotors_interface/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(custom_rotors_interface)
# Add plain cmake packages
find_package(catkin_simple REQUIRED)
catkin_simple(ALL_DEPS_REQUIRED)
cs_add_library(custom_rotors_interface src/custom_rotors_interface.cpp)
cs_add_executable(custom_rotors_interface_node src/custom_rotors_interface_node.cpp)
target_link_libraries(custom_rotors_interface_node custom_rotors_interface)
cs_install()
cs_export()
# CMake Indexing
FILE(GLOB_RECURSE LibFiles "include/*")
add_custom_target(headers SOURCES ${LibFiles})
FILE(GLOB_RECURSE ProtoFiles "proto/*")
add_custom_target(protos SOURCES ${ProtoFiles})
================================================
FILE: custom_rotors_interface/include/custom_rotors_interface/custom_rotors_interface.h
================================================
#pragma once
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <geometry_msgs/PoseStamped.h>
#include <quadrotor_common/control_command.h>
#include <quadrotor_msgs/ControlCommand.h>
#include "mav_msgs/Actuators.h"
#include "nav_msgs/Odometry.h"
#include "quadrotor_msgs/AutopilotFeedback.h"
#include "pose_utils/pose.h"
#include "std_msgs/Bool.h"
namespace custom_rotors_interface {
struct TorquesAndThrust {
Eigen::Vector3d body_torques;
double collective_thrust;
};
class CustomRotorsInterface {
public:
CustomRotorsInterface(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
CustomRotorsInterface()
: CustomRotorsInterface(ros::NodeHandle(), ros::NodeHandle("~")) {}
private:
void armCallback(const std_msgs::BoolConstPtr& msg);
void controlCommandCallback(
const quadrotor_msgs::ControlCommandConstPtr& msg);
void odometryCallback(const nav_msgs::OdometryConstPtr& msg);
void autopilotCallback(const quadrotor_msgs::AutopilotFeedbackConstPtr& msg);
void motorSpeedCallback(const mav_msgs::Actuators::ConstPtr& msg);
void lowLevelControlLoop();
TorquesAndThrust bodyRateControl(
const quadrotor_common::ControlCommand& rate_cmd,
const Eigen::Vector3d& body_rate_estimate);
mav_msgs::Actuators mixer(const TorquesAndThrust& torques_and_thrust);
bool loadParameters();
ros::NodeHandle nh_;
ros::NodeHandle pnh_;
ros::Subscriber arm_sub_;
ros::Subscriber motor_speed_sub_;
ros::Subscriber ctrl_cmd_sub_;
ros::Subscriber imu_sub_;
ros::Subscriber odometry_sub_;
ros::Subscriber controller_sub_;
ros::Subscriber network_sub_;
ros::Subscriber autopilot_sub_;
ros::Publisher desired_motor_speed_pub_;
ros::WallTime start_time_;
std::string data_dir_;
Eigen::Vector3d prev_v_lin_, prev_v_ang_;
Eigen::Vector3d incr_v_lin_, incr_v_ang_;
ros::Time last_hover_time_ = ros::Time::now();
int maneuver_idx_ = 0;
int action_idx_ = 0;
bool velocity_estimate_in_world_frame_;
bool enable_navigation_ = false;
bool reference_frame_set_ = false;
bool interface_armed_ = false;
TorquesAndThrust torques_and_thrust_estimate_;
double inertia_x_;
double inertia_y_;
double inertia_z_;
double body_rates_p_xy_;
double body_rates_d_xy_;
double body_rates_p_z_;
double body_rates_d_z_;
// Parameters
Eigen::Matrix3d inertia_;
Eigen::MatrixXd K_lqr_;
double low_level_control_frequency_;
double roll_pitch_cont_gain_;
double arm_length_;
double rotor_drag_coeff_;
double rotor_thrust_coeff_;
double mass_;
double max_rotor_speed_;
quadrotor_msgs::ControlCommand control_command_;
nav_msgs::Odometry odometry_;
quadrotor_msgs::AutopilotFeedback autopilot_feedback_;
double rel_time_;
};
} // namespace custom_rotors_interface
================================================
FILE: custom_rotors_interface/package.xml
================================================
<?xml version="1.0"?>
<package format="2">
<name>custom_rotors_interface</name>
<version>0.0.0</version>
<description>A package containing utils for simulation</description>
<maintainer email="ekaufmann@ifi.uzh.ch">Elia Kaufmann</maintainer>
<license>GNU GPL</license>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend>catkin_simple</buildtool_depend>
<depend>roscpp</depend>
<depend>gazebo</depend>
<depend>gazebo_msgs</depend>
<depend>mav_msgs</depend>
<depend>quadrotor_common</depend>
<depend>pose_utils</depend>
<depend>rotors_gazebo</depend>
<depend>rotors_description</depend>
<depend>manual_flight_assistant</depend>
<depend>rqt_quad_gui</depend>
</package>
================================================
FILE: custom_rotors_interface/parameters/autopilot.yaml
================================================
state_estimate_timeout: 5.0 # [s]
velocity_estimate_in_world_frame: false
control_command_delay: 0.05 # [s]
start_land_velocity: 0.5 # [m/s]
start_land_acceleration: 1 # [m/s^2]
start_idle_duration: 2.0 # [s]
idle_thrust: 2.0 # [m/s]
optitrack_start_height: 3.0 # [m]
optitrack_start_land_timeout: 10 # [s]
optitrack_land_drop_height: 0.3 # [m]
propeller_ramp_down_timeout: 1.5 # [s]
breaking_velocity_threshold: 0.2 # [m/s]
breaking_timeout: 0.5 # [s]
go_to_pose_max_velocity: 3.5 # [m/s]
go_to_pose_max_normalized_thrust: 15.0 # [m/s^2]
go_to_pose_max_roll_pitch_rate: 1.0 # [rad/s]
velocity_command_input_timeout: 0.1 # [s]
tau_velocity_command: 0.8 # []
reference_state_input_timeout: 0.1 # [s]
emergency_land_duration: 4 # [s]
emergency_land_thrust: 9.0 # [m/s^2]
control_command_input_timeout: 1.0 # [s]
enable_command_feedthrough: true
predictive_control_lookahead: 2.0 # [s]
min_control_period: 0.0 # [s]
================================================
FILE: custom_rotors_interface/parameters/custom_rotors_interface.yaml
================================================
# Vehicle parameters
inertia_x: 0.007
inertia_y: 0.007
inertia_z: 0.012
arm_length: 0.17
mass: 0.73
# Motors parameters
rotor_drag_coeff: 0.016
rotor_thrust_coeff: 8.54858e-06
max_rotor_speed: 838.0
# Controller
low_level_control_frequency: 200.0
body_rates_p_xy: 0.1
body_rates_d_xy: 0.5
body_rates_p_z: 0.03
body_rates_d_z: 0.1
roll_pitch_cont_gain: 6.0
================================================
FILE: custom_rotors_interface/parameters/manual_flight_assistant.yaml
================================================
joypad_timeout: 0.5
sbus_timeout: 0.5
joypad_axes_zero_tolerance: 0.05
sbus_axes_zero_tolerance: 10
vmax_xy: 1.5
vmax_z: 0.7
rmax_yaw: 1.5
================================================
FILE: custom_rotors_interface/parameters/position_controller.yaml
================================================
position_controller:
use_rate_mode: true
kpxy: 10.0
kdxy: 4.0
kpz: 15.0
kdz: 6.0
krp: 12.0
kyaw: 5.0
pxy_error_max: 0.6
vxy_error_max: 1.0
pz_error_max: 0.3
vz_error_max: 0.75
yaw_error_max: 0.7
perform_aerodynamics_compensation: false
k_drag_x: 0.0
k_drag_y: 0.0
k_drag_z: 0.0
k_thrust_horz: 0.0
================================================
FILE: custom_rotors_interface/src/custom_rotors_interface.cpp
================================================
#include "custom_rotors_interface/custom_rotors_interface.h"
#include <quadrotor_common/math_common.h>
#include "quadrotor_common/geometry_eigen_conversions.h"
namespace custom_rotors_interface {
CustomRotorsInterface::CustomRotorsInterface(const ros::NodeHandle& nh,
const ros::NodeHandle& pnh)
: nh_(nh), pnh_(pnh) {
if (!loadParameters()) {
ROS_ERROR("[%s] Could not load parameters.", pnh_.getNamespace().c_str());
}
arm_sub_ =
nh_.subscribe("bridge/arm", 1, &CustomRotorsInterface::armCallback, this);
ctrl_cmd_sub_ = nh_.subscribe("control_command", 1,
&CustomRotorsInterface::controlCommandCallback,
this, ros::TransportHints().tcpNoDelay());
autopilot_sub_ = nh_.subscribe("autopilot/feedback", 1,
&CustomRotorsInterface::autopilotCallback,
this, ros::TransportHints().tcpNoDelay());
odometry_sub_ =
nh_.subscribe("odometry", 1, &CustomRotorsInterface::odometryCallback,
this, ros::TransportHints().tcpNoDelay());
motor_speed_sub_ = nh_.subscribe("motor_speed", 1,
&CustomRotorsInterface::motorSpeedCallback,
this, ros::TransportHints().tcpNoDelay());
desired_motor_speed_pub_ =
nh_.advertise<mav_msgs::Actuators>("command/motor_speed", 1);
}
void CustomRotorsInterface::armCallback(const std_msgs::BoolConstPtr& msg) {
if (msg->data) {
interface_armed_ = true;
ROS_INFO("[%s] Interface armed", pnh_.getNamespace().c_str());
} else {
interface_armed_ = false;
ROS_INFO("[%s] Interface disarmed", pnh_.getNamespace().c_str());
}
}
void CustomRotorsInterface::controlCommandCallback(
const quadrotor_msgs::ControlCommandConstPtr& msg) {
control_command_ = *msg;
}
void CustomRotorsInterface::autopilotCallback(
const quadrotor_msgs::AutopilotFeedbackConstPtr& msg) {
autopilot_feedback_ = *msg;
}
void CustomRotorsInterface::odometryCallback(
const nav_msgs::OdometryConstPtr& msg) {
odometry_ = *msg;
}
void CustomRotorsInterface::motorSpeedCallback(
const mav_msgs::Actuators::ConstPtr& msg) {
// The hummingbird that we use in our simulations has the following rotor
// configuration Rotor 0 spins clockwise
// x
// 0 ^
// | |
// 1--+--3 y <--+
// |
// 2
const double f0 = rotor_thrust_coeff_ * pow(msg->angular_velocities[0], 2.0);
const double f1 = rotor_thrust_coeff_ * pow(msg->angular_velocities[1], 2.0);
const double f2 = rotor_thrust_coeff_ * pow(msg->angular_velocities[2], 2.0);
const double f3 = rotor_thrust_coeff_ * pow(msg->angular_velocities[3], 2.0);
torques_and_thrust_estimate_.body_torques.x() = arm_length_ * (f1 - f3);
torques_and_thrust_estimate_.body_torques.y() = arm_length_ * (f2 - f0);
torques_and_thrust_estimate_.body_torques.z() =
rotor_drag_coeff_ * (f0 - f1 + f2 - f3);
torques_and_thrust_estimate_.collective_thrust = f0 + f1 + f2 + f3;
lowLevelControlLoop();
}
void CustomRotorsInterface::lowLevelControlLoop() {
mav_msgs::Actuators desired_motor_speed;
quadrotor_msgs::ControlCommand control_command = control_command_;
nav_msgs::Odometry quad_state = odometry_;
if (!interface_armed_ || !control_command.armed) {
for (int i = 0; i < 4; i++) {
desired_motor_speed.angular_velocities.push_back(0.0);
}
} else {
if (control_command.control_mode == control_command.BODY_RATES) {
const quadrotor_common::ControlCommand rate_cmd =
quadrotor_common::ControlCommand(control_command);
const TorquesAndThrust torques_and_thrust = bodyRateControl(
rate_cmd,
quadrotor_common::geometryToEigen(quad_state.twist.twist.angular));
desired_motor_speed = mixer(torques_and_thrust);
} else {
ROS_ERROR_THROTTLE(1,
"[%s] Undefined control mode, will not apply command.",
ros::this_node::getName().c_str());
return;
}
}
desired_motor_speed_pub_.publish(desired_motor_speed);
}
TorquesAndThrust CustomRotorsInterface::bodyRateControl(
const quadrotor_common::ControlCommand& rate_cmd,
const Eigen::Vector3d& body_rate_estimate) {
Eigen::MatrixXd H = Eigen::MatrixXd::Zero(3, 3);
Eigen::VectorXd control_error = Eigen::VectorXd::Zero(6);
// control_error.segment(0, 3) = rate_cmd.bodyrates - body_rate_estimate;
// control_error.segment(3, 3) = rate_cmd.bodyrates.cross(inertia_ *
// rate_cmd.bodyrates)
// + inertia_ * rate_cmd.angular_accelerations
// - torques_and_thrust_estimate_.body_torques;
control_error.segment(0, 3) = rate_cmd.bodyrates - body_rate_estimate;
control_error.segment(3, 3) =
rate_cmd.bodyrates.cross(inertia_ * rate_cmd.bodyrates) +
inertia_ * rate_cmd.angular_accelerations -
torques_and_thrust_estimate_.body_torques;
TorquesAndThrust torques_and_thrust;
// torques_and_thrust.body_torques = K_lqr_ * control_error +
// body_rate_estimate.cross(inertia_ * body_rate_estimate)
// + inertia_ *
// rate_cmd.angular_accelerations;
// new version
torques_and_thrust.body_torques =
K_lqr_ * control_error +
body_rate_estimate.cross(inertia_ * body_rate_estimate) +
inertia_ * rate_cmd.angular_accelerations;
torques_and_thrust.collective_thrust = rate_cmd.collective_thrust;
return torques_and_thrust;
}
mav_msgs::Actuators CustomRotorsInterface::mixer(
const TorquesAndThrust& torques_and_thrust) {
// Using Rotor Convention of the Hummingbird
mav_msgs::Actuators rotor_speed_cmds;
for (int i = 0; i < 4; i++) {
rotor_speed_cmds.angular_velocities.push_back(0.0);
}
// Compute square of single rotor speed commands
if (torques_and_thrust.collective_thrust < 0.05) {
return rotor_speed_cmds;
}
// Compute the square of the single rotor speeds
rotor_speed_cmds.angular_velocities[0] =
((arm_length_ * torques_and_thrust.body_torques.z() -
2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.y() +
rotor_drag_coeff_ * arm_length_ * mass_ *
torques_and_thrust.collective_thrust) /
(4.0 * rotor_drag_coeff_ * arm_length_)) /
rotor_thrust_coeff_;
rotor_speed_cmds.angular_velocities[1] =
((2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.x() -
arm_length_ * torques_and_thrust.body_torques.z() +
rotor_drag_coeff_ * arm_length_ * mass_ *
torques_and_thrust.collective_thrust) /
(4.0 * rotor_drag_coeff_ * arm_length_)) /
rotor_thrust_coeff_;
rotor_speed_cmds.angular_velocities[2] =
((2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.y() +
arm_length_ * torques_and_thrust.body_torques.z() +
rotor_drag_coeff_ * arm_length_ * mass_ *
torques_and_thrust.collective_thrust) /
(4.0 * rotor_drag_coeff_ * arm_length_)) /
rotor_thrust_coeff_;
rotor_speed_cmds.angular_velocities[3] =
(-(2.0 * rotor_drag_coeff_ * torques_and_thrust.body_torques.x() +
arm_length_ * torques_and_thrust.body_torques.z() -
rotor_drag_coeff_ * arm_length_ * mass_ *
torques_and_thrust.collective_thrust) /
(4.0 * rotor_drag_coeff_ * arm_length_)) /
rotor_thrust_coeff_;
// Apply limits and take square root
for (int i = 0; i < 4; i++) {
quadrotor_common::limit(&rotor_speed_cmds.angular_velocities[i], 0.0,
pow(max_rotor_speed_, 2.0));
rotor_speed_cmds.angular_velocities[i] =
sqrt(rotor_speed_cmds.angular_velocities[i]);
}
rotor_speed_cmds.header.stamp = ros::Time::now();
return rotor_speed_cmds;
}
bool CustomRotorsInterface::loadParameters() {
bool check = true;
check &= pnh_.getParam("inertia_x", inertia_x_);
check &= pnh_.getParam("inertia_y", inertia_y_);
check &= pnh_.getParam("inertia_z", inertia_z_);
inertia_ = Eigen::Matrix3d::Zero();
inertia_(0, 0) = inertia_x_;
inertia_(1, 1) = inertia_y_;
inertia_(2, 2) = inertia_z_;
check &= pnh_.getParam("body_rates_p_xy", body_rates_p_xy_);
check &= pnh_.getParam("body_rates_d_xy", body_rates_d_xy_);
check &= pnh_.getParam("body_rates_p_z", body_rates_p_z_);
check &= pnh_.getParam("body_rates_d_z", body_rates_d_z_);
// nothing to do with lqr, just a PD controller
K_lqr_ = Eigen::MatrixXd::Zero(3, 6);
K_lqr_(0, 0) = body_rates_p_xy_;
K_lqr_(1, 1) = body_rates_p_xy_;
K_lqr_(2, 2) = body_rates_p_z_;
K_lqr_(0, 3) = body_rates_d_xy_;
K_lqr_(1, 4) = body_rates_d_xy_;
K_lqr_(2, 5) = body_rates_d_z_;
check &= pnh_.getParam("roll_pitch_cont_gain", roll_pitch_cont_gain_);
check &= pnh_.getParam("arm_length", arm_length_);
check &= pnh_.getParam("rotor_drag_coeff", rotor_drag_coeff_);
check &= pnh_.getParam("rotor_thrust_coeff", rotor_thrust_coeff_);
check &= pnh_.getParam("mass", mass_);
check &= pnh_.getParam("max_rotor_speed", max_rotor_speed_);
return check;
}
} // namespace custom_rotors_interface
================================================
FILE: custom_rotors_interface/src/custom_rotors_interface_node.cpp
================================================
#include <custom_rotors_interface/custom_rotors_interface.h>
#include <memory>
int main(int argc, char **argv) {
ros::init(argc, argv, "custom_rotors_interface");
custom_rotors_interface::CustomRotorsInterface custom_rotors_interface;
ros::spin();
return 0;
}
================================================
FILE: dependencies.yaml
================================================
repositories:
catkin_boost_python_buildtool:
type: git
url: git@github.com:ethz-asl/catkin_boost_python_buildtool.git
version: master
catkin_simple:
type: git
url: git@github.com:catkin/catkin_simple.git
version: master
eigen_catkin:
type: git
url: git@github.com:ethz-asl/eigen_catkin.git
version: master
eigen_checks:
type: git
url: git@github.com:ethz-asl/eigen_checks.git
version: master
gflags_catkin:
type: git
url: git@github.com:ethz-asl/gflags_catkin.git
version: master
glog_catkin:
type: git
url: git@github.com:ethz-asl/glog_catkin.git
version: master
minkindr:
type: git
url: git@github.com:ethz-asl/minkindr.git
version: master
minkindr_ros:
type: git
url: git@github.com:ethz-asl/minkindr_ros.git
version: master
numpy_eigen:
type: git
url: git@github.com:ethz-asl/numpy_eigen.git
version: master
pose_utils:
type: git
url: git@github.com:antonilo/pose_utils.git
version: master
minimum_jerk_trajectories:
type: git
url: git@github.com:uzh-rpg/minimum_jerk_trajectories.git
version: master
octomap:
type: git
url: git@github.com:OctoMap/octomap.git
version: master
octomap_msgs:
type: git
url: git@github.com:OctoMap/octomap_msgs.git
version: master
octomap_ros:
type: git
url: git@github.com:OctoMap/octomap_ros.git
version: melodic-devel
rpg_mpc:
type: git
url: git@github.com:uzh-rpg/rpg_mpc.git
version: master
rotors_simulator:
type: git
url: git@github.com:ethz-asl/rotors_simulator.git
version: master
rpg_quadrotor_control:
type: git
url: git@github.com:uzh-rpg/rpg_quadrotor_control.git
version: master
rpg_quadrotor_common:
type: git
url: git@github.com:uzh-rpg/rpg_quadrotor_common.git
version: master
mav_comm:
type: git
url: git@github.com:ethz-asl/mav_comm.git
version: master
VINS-Mono:
type: git
url: git@github.com:kelia/VINS-Mono.git
version: imu_constant_integration
================================================
FILE: fpv_aggressive_trajectories/.gitignore
================================================
cmake-build-debug/
.idea/
================================================
FILE: fpv_aggressive_trajectories/CMakeLists.txt
================================================
cmake_minimum_required(VERSION 2.8.3)
project(fpv_aggressive_trajectories)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
add_compile_options(-O3)
find_package(catkin_simple REQUIRED)
catkin_simple(ALL_DEPS_REQUIRED)
cs_add_library(visualize src/visualize.cpp)
cs_add_executable(fpv_aggressive_trajectories src/fpv_aggressive_trajectories.cpp)
target_include_directories(fpv_aggressive_trajectories PUBLIC
../../rpg_mpc/model/quadrotor_mpc_codegen/
../../rpg_mpc/externals/qpoases
../../rpg_mpc/externals/qpoases/INCLUDE
../../rpg_mpc/externals/qpoases/SRC)
target_link_libraries(fpv_aggressive_trajectories visualize)
cs_add_executable(odometry_republisher src/odometry_republisher.cpp)
cs_install()
cs_export()
================================================
FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/fpv_aggressive_trajectories.h
================================================
#pragma once
#include <ros/ros.h>
#include <Eigen/Core>
#include <autopilot/autopilot_helper.h>
#include <nav_msgs/Odometry.h>
#include <position_controller/position_controller.h>
#include <quadrotor_common/control_command.h>
#include <quadrotor_common/trajectory.h>
#include <quadrotor_msgs/ControlCommand.h>
#include <rpg_mpc/mpc_controller.h>
#include <state_predictor/state_predictor.h>
#include <std_msgs/Empty.h>
#include <visualization_msgs/MarkerArray.h>
#include "fpv_aggressive_trajectories/visualize.h"
namespace fpv_aggressive_trajectories {
class FPVAggressiveTrajectories {
public:
FPVAggressiveTrajectories(const ros::NodeHandle& nh,
const ros::NodeHandle& pnh);
FPVAggressiveTrajectories()
: FPVAggressiveTrajectories(ros::NodeHandle(), ros::NodeHandle("~")) {}
virtual ~FPVAggressiveTrajectories();
private:
enum STATES {
kOff = 0,
kComputeTrajectory = 1,
kExecuteTrajectory = 2,
kNetworkControl = 3,
kAutopilot = 4
};
ros::NodeHandle nh_;
ros::NodeHandle pnh_;
// Subscribers
ros::Subscriber toggle_experiment_sub_;
ros::Subscriber odometry_sub_;
ros::Subscriber odometry_gt_sub_;
// Publishers
ros::Publisher control_command_pub_;
ros::Publisher control_command_gt_pub_;
ros::Publisher aggressive_end_pub_;
ros::Publisher marker_pub_ref_;
ros::Publisher vio_ref_pub_;
ros::Publisher traj_comp_finish_pub_;
quadrotor_common::QuadStateEstimate getPredictedStateEstimate(
const ros::Time& time, const state_predictor::StatePredictor* predictor);
void computeManeuver();
quadrotor_common::ControlCommand computeControlCommand(
const quadrotor_common::QuadStateEstimate& state_estimate,
ros::Duration* trajectory_execution_left_duration,
int* trajectories_left_in_queue);
void publishControlCommand(
const quadrotor_common::ControlCommand& control_command,
const quadrotor_common::ControlCommand& control_command_gt);
void startExecutionCallback(const std_msgs::BoolConstPtr& msg);
void odometryCallback(const nav_msgs::OdometryConstPtr& msg);
void odometryGTCallback(const nav_msgs::OdometryConstPtr& msg);
bool loadParameters();
std::list<quadrotor_common::Trajectory> trajectory_queue_;
// will be the same trajectories, but transformed to GT frame
std::list<quadrotor_common::Trajectory> trajectory_queue_gt_;
// Parameters
double desired_heading_;
double exec_loop_rate_;
double circle_velocity_;
int n_loops_;
nav_msgs::Odometry odometry_gt_;
STATES state_machine_{kComputeTrajectory};
ros::Time time_start_trajectory_execution_;
quadrotor_common::QuadStateEstimate received_state_est_;
quadrotor_common::QuadStateEstimate received_state_est_gt_;
bool first_time_in_new_state_;
double predictive_control_lookahead_ = 2.0;
bool sent_maneuver_end_msg_ = false;
int maneuver_counter_ = 0;
double traj_sampling_freq_ = 50.0;
// MPC controller variant
rpg_mpc::MpcController<double> base_controller_ =
rpg_mpc::MpcController<double>(ros::NodeHandle(), ros::NodeHandle("~"),
"vio_mpc_path");
rpg_mpc::MpcParams<double> base_controller_params_;
state_predictor::StatePredictor state_predictor_;
// Second controller instance that operates on ground truth data
rpg_mpc::MpcParams<double> base_controller_params_gt_;
state_predictor::StatePredictor state_predictor_gt_;
std::shared_ptr<fpv_aggressive_trajectories::Visualizer> visualizer_;
};
} // namespace fpv_aggressive_trajectories
================================================
FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/odometry_republisher.h
================================================
#pragma once
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <pose_utils/pose.h>
namespace odometry_republisher {
class OdometryRepublisher {
public:
OdometryRepublisher();
virtual ~OdometryRepublisher();
private:
ros::NodeHandle nh_;
ros::Subscriber state_estimate_sub_;
ros::Publisher odometry_out_pub_;
rpg::Pose T_B_S_;
void odometryInCallback(const nav_msgs::OdometryConstPtr& msg);
void transformOdometry(const nav_msgs::OdometryConstPtr& odom_in,
nav_msgs::Odometry* odom_out);
};
} // namespace odometry_republisher
================================================
FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/visualize.h
================================================
#pragma once
#include <ros/ros.h>
#include <visualization_msgs/MarkerArray.h>
#include "quadrotor_common/trajectory.h"
namespace fpv_aggressive_trajectories {
class Visualizer {
public:
Visualizer(const ros::NodeHandle& nh, const ros::NodeHandle& pnh);
Visualizer() : Visualizer(ros::NodeHandle(), ros::NodeHandle("~")) {}
virtual ~Visualizer();
void visualizeReferenceTrajectory(
const quadrotor_common::Trajectory* trajectory);
void visualizeTrajectories(
const std::list<quadrotor_common::Trajectory>& maneuver_list);
void create_vehicle_markers(int num_rotors, float arm_len, float body_width,
float body_height);
void displayQuadrotor();
private:
ros::NodeHandle nh_;
ros::NodeHandle pnh_;
ros::Publisher bodyrates_viz_pub_;
ros::Publisher marker_ref_pub_;
ros::Publisher marker_pub_;
ros::Publisher vehicle_marker_pub_;
std::shared_ptr<visualization_msgs::MarkerArray> vehicle_marker_;
};
} // namespace fpv_aggressive_trajectories
================================================
FILE: fpv_aggressive_trajectories/launch/simulation/controller.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="controller"/>
<arg name="use_ground_truth"/>
<arg name="enable_command_feedthrough"/>
<arg name="velocity_estimate_in_world_frame"/>
<!-- Autopilot -->
<include file="$(find fpv_aggressive_trajectories)/launch/simulation/$(arg controller).launch">
<arg name="quad_name" value="$(arg quad_name)"/>
<arg name="use_ground_truth" value="$(arg use_ground_truth)"/>
<arg name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<arg name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
</include>
</launch>
================================================
FILE: fpv_aggressive_trajectories/launch/simulation/custom_rotors_interface.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name" default="hummingbird"/>
<arg name="use_ground_truth" default="true"/>
<group ns="$(arg quad_name)">
<!-- To debug the rpg_rotors_interface -->
<group if="$(arg use_ground_truth)">
<node pkg="custom_rotors_interface" type="custom_rotors_interface_node"
name="custom_rotors_interface" output="screen" >
<rosparam file="$(find custom_rotors_interface)/parameters/custom_rotors_interface.yaml" />
<remap from="odometry" to="ground_truth/odometry" />
<remap from="arm" to="bridge/arm" />
</node>
</group>
<group unless="$(arg use_ground_truth)">
<node pkg="custom_rotors_interface" type="custom_rotors_interface_node"
name="custom_rotors_interface" output="screen" >
<rosparam file="$(find custom_rotors_interface)/parameters/custom_rotors_interface.yaml" />
<remap from="odometry" to="odometry_sensor1/odometry" />
<remap from="arm" to="bridge/arm" />
</node>
</group>
</group>
</launch>
================================================
FILE: fpv_aggressive_trajectories/launch/simulation/rpg_mpc.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="use_ground_truth"/>
<arg name="enable_command_feedthrough"/>
<arg name="velocity_estimate_in_world_frame"/>
<group ns="$(arg quad_name)">
<group if="$(arg use_ground_truth)">
<node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml"/>
<rosparam file="$(find fpv_aggressive_trajectories)/parameters/mpc_params.yaml"/>
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml"/>
<param name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
<param name="state_estimate_timeout" value="0.1"/>
<param name="control_command_delay" value="0.05"/>
<param name="enable_command_feedthrough" value="$(arg enable_command_feedthrough)"/>
<!-- <remap from="control_command" to="control_command"/>-->
<!-- <remap from="autopilot/state_estimate" to="ground_truth/odometry"/>-->
<remap from="control_command" to="control_command_label"/>
<remap from="autopilot/state_estimate" to="odometry_converted"/>
</node>
</group>
<group unless="$(arg use_ground_truth)">
<node pkg="rpg_mpc" type="autopilot_mpc_instance" name="autopilot" output="screen">
<rosparam file="$(find state_predictor)/parameters/hummingbird.yaml"/>
<rosparam file="$(find fpv_aggressive_trajectories)/parameters/mpc_params.yaml"/>
<rosparam file="$(find rpg_rotors_interface)/parameters/autopilot.yaml"/>
<param name="velocity_estimate_in_world_frame" value="$(arg velocity_estimate_in_world_frame)"/>
<param name="state_estimate_timeout" value="0.1"/>
<param name="control_command_delay" value="0.05"/>
<!-- <remap from="control_command" to="control_command"/>-->
<!-- <remap from="autopilot/state_estimate" to="odometry_sensor1/odometry"/>-->
<remap from="control_command" to="control_command_label"/>
<remap from="autopilot/state_estimate" to="odometry_converted"/>
</node>
</group>
</group>
</launch>
================================================
FILE: fpv_aggressive_trajectories/launch/simulation/rpg_pid.launch
================================================
<?xml version="1.0"?>
<launch>
<arg name="quad_name"/>
<arg name="use_ground_truth"/>
<arg name="enable_command_feedthrough"/>
<arg name="velocity_estimate_in_world_frame"/>
<group ns="$(arg quad_name)">
<group if="$(arg use_ground_truth)">
<node pkg="autopilot" type="autopilot" name="au
gitextract_nr_44o_q/
├── .clang-format
├── .gitignore
├── LICENSE
├── README.md
├── acrobatic_trajectory_helper/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── acrobatic_trajectory_helper/
│ │ ├── acrobatic_sequence.h
│ │ ├── circle_trajectory_helper.h
│ │ ├── heading_trajectory_helper.h
│ │ └── polynomial_trajectory_helper.h
│ ├── package.xml
│ └── src/
│ ├── acrobatic_sequence.cpp
│ ├── circle_trajectory_helper.cpp
│ ├── heading_trajectory_helper.cpp
│ └── polynomial_trajectory_helper.cpp
├── conda_requirements.txt
├── controller_learning/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── ckpt/
│ │ └── loop/
│ │ ├── fts_and_imu/
│ │ │ ├── ckpt-7.data-00000-of-00002
│ │ │ ├── ckpt-7.data-00001-of-00002
│ │ │ └── ckpt-7.index
│ │ └── imu_only/
│ │ ├── ckpt-37.data-00000-of-00002
│ │ ├── ckpt-37.data-00001-of-00002
│ │ └── ckpt-37.index
│ ├── common.py
│ ├── config/
│ │ ├── dagger_settings.yaml
│ │ ├── settings.py
│ │ ├── test_settings.yaml
│ │ └── train_settings.yaml
│ ├── data/
│ │ └── .gitignore
│ ├── iterative_learning_trajectories.py
│ ├── launch/
│ │ └── simulation/
│ │ ├── base_quad_simulation_no_controller.launch
│ │ ├── controller.launch
│ │ ├── custom_rotors_interface.launch
│ │ ├── minisim.launch
│ │ ├── quadrotor_empty_world_no_controller.launch
│ │ ├── quadrotor_rgb_world_no_controller.launch
│ │ ├── rpg_mpc.launch
│ │ ├── rpg_pid.launch
│ │ ├── rpg_rotors_interface.launch
│ │ ├── rviz_only.launch
│ │ └── simulation.launch
│ ├── package.xml
│ ├── results/
│ │ └── .gitignore
│ ├── setup.py
│ ├── src/
│ │ └── ControllerLearning/
│ │ ├── TrajectoryBase.py
│ │ ├── TrajectoryLearning.py
│ │ ├── __init__.py
│ │ └── models/
│ │ ├── __init__.py
│ │ ├── body_dataset.py
│ │ ├── bodyrate_learner.py
│ │ ├── nets.py
│ │ └── tf_addons_normalizations.py
│ ├── test_trajectories.py
│ ├── train.py
│ └── train_logs/
│ ├── 20191015-164750/
│ │ └── train/
│ │ ├── checkpoint
│ │ ├── ckpt-78.data-00000-of-00002
│ │ ├── ckpt-78.data-00001-of-00002
│ │ ├── ckpt-78.index
│ │ ├── ckpt-79.data-00000-of-00002
│ │ ├── ckpt-79.data-00001-of-00002
│ │ ├── ckpt-79.index
│ │ ├── ckpt-80.data-00000-of-00002
│ │ ├── ckpt-80.data-00001-of-00002
│ │ ├── ckpt-80.index
│ │ └── events.out.tfevents.1571150870.elia-ThinkPad-P51.9892.82.v2
│ ├── 20191022-082718/
│ │ └── train/
│ │ ├── checkpoint
│ │ ├── ckpt-1078.data-00000-of-00002
│ │ ├── ckpt-1078.data-00001-of-00002
│ │ ├── ckpt-1078.index
│ │ ├── ckpt-1079.data-00000-of-00002
│ │ ├── ckpt-1079.data-00001-of-00002
│ │ ├── ckpt-1079.index
│ │ ├── ckpt-1080.data-00000-of-00002
│ │ ├── ckpt-1080.data-00001-of-00002
│ │ ├── ckpt-1080.index
│ │ └── events.out.tfevents.1571725638.elia-ThinkPad-P51.29578.82.v2
│ ├── 20191022-095948/
│ │ └── train/
│ │ ├── checkpoint
│ │ ├── ckpt-1000.data-00000-of-00002
│ │ ├── ckpt-1000.data-00001-of-00002
│ │ ├── ckpt-1000.index
│ │ ├── ckpt-998.data-00000-of-00002
│ │ ├── ckpt-998.data-00001-of-00002
│ │ ├── ckpt-998.index
│ │ ├── ckpt-999.data-00000-of-00002
│ │ ├── ckpt-999.data-00001-of-00002
│ │ ├── ckpt-999.index
│ │ └── events.out.tfevents.1571731188.elia-ThinkPad-P51.5931.82.v2
│ ├── 20191022-122145/
│ │ └── train/
│ │ └── events.out.tfevents.1571739705.elia-ThinkPad-P51.15206.54.v2
│ ├── 20191022-122316/
│ │ └── train/
│ │ └── events.out.tfevents.1571739796.elia-ThinkPad-P51.16327.54.v2
│ ├── 20191022-122551/
│ │ └── train/
│ │ └── events.out.tfevents.1571739951.elia-ThinkPad-P51.16978.54.v2
│ ├── 20191022-122626/
│ │ └── train/
│ │ └── events.out.tfevents.1571739986.elia-ThinkPad-P51.17446.54.v2
│ ├── 20191022-122842/
│ │ └── train/
│ │ └── events.out.tfevents.1571740122.elia-ThinkPad-P51.17988.54.v2
│ ├── 20191022-123011/
│ │ └── train/
│ │ └── events.out.tfevents.1571740211.elia-ThinkPad-P51.18467.54.v2
│ ├── 20191022-134158/
│ │ └── train/
│ │ └── events.out.tfevents.1571744518.elia-ThinkPad-P51.27889.54.v2
│ ├── 20191022-135227/
│ │ └── train/
│ │ └── events.out.tfevents.1571745147.elia-ThinkPad-P51.31014.54.v2
│ ├── 20191022-135514/
│ │ └── train/
│ │ └── events.out.tfevents.1571745314.elia-ThinkPad-P51.32244.54.v2
│ ├── 20191023-115641/
│ │ └── train/
│ │ └── events.out.tfevents.1571824601.elia-ThinkPad-P51.12466.54.v2
│ ├── 20191023-115844/
│ │ └── train/
│ │ └── events.out.tfevents.1571824724.elia-ThinkPad-P51.13447.54.v2
│ ├── 20191023-120327/
│ │ └── train/
│ │ └── events.out.tfevents.1571825007.elia-ThinkPad-P51.15260.54.v2
│ ├── 20191023-120523/
│ │ └── train/
│ │ └── events.out.tfevents.1571825123.elia-ThinkPad-P51.16206.54.v2
│ ├── 20191023-120641/
│ │ └── train/
│ │ └── events.out.tfevents.1571825201.elia-ThinkPad-P51.17007.54.v2
│ ├── 20191023-121605/
│ │ └── train/
│ │ └── events.out.tfevents.1571825765.elia-ThinkPad-P51.19388.54.v2
│ ├── 20191023-122039/
│ │ └── train/
│ │ └── events.out.tfevents.1571826039.elia-ThinkPad-P51.21241.54.v2
│ └── 20191023-122438/
│ └── train/
│ └── events.out.tfevents.1571826278.elia-ThinkPad-P51.23680.54.v2
├── custom_rotors_interface/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── custom_rotors_interface/
│ │ └── custom_rotors_interface.h
│ ├── package.xml
│ ├── parameters/
│ │ ├── autopilot.yaml
│ │ ├── custom_rotors_interface.yaml
│ │ ├── manual_flight_assistant.yaml
│ │ └── position_controller.yaml
│ └── src/
│ ├── custom_rotors_interface.cpp
│ └── custom_rotors_interface_node.cpp
├── dependencies.yaml
├── fpv_aggressive_trajectories/
│ ├── .gitignore
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── fpv_aggressive_trajectories/
│ │ ├── fpv_aggressive_trajectories.h
│ │ ├── odometry_republisher.h
│ │ └── visualize.h
│ ├── launch/
│ │ └── simulation/
│ │ ├── controller.launch
│ │ ├── custom_rotors_interface.launch
│ │ ├── rpg_mpc.launch
│ │ ├── rpg_pid.launch
│ │ └── simulation.launch
│ ├── package.xml
│ ├── parameters/
│ │ └── mpc_params.yaml
│ ├── resources/
│ │ ├── race_track/
│ │ │ ├── iros_materials/
│ │ │ │ └── materials/
│ │ │ │ └── scripts/
│ │ │ │ ├── asphalt.material
│ │ │ │ ├── carpet.material
│ │ │ │ ├── wall_1.material
│ │ │ │ ├── wall_2.material
│ │ │ │ ├── wall_3.material
│ │ │ │ └── wall_4.material
│ │ │ └── real_world/
│ │ │ └── gate/
│ │ │ └── meshes/
│ │ │ └── gate.dae
│ │ ├── rviz/
│ │ │ ├── race_track.rviz
│ │ │ └── real_world.rviz
│ │ ├── simulation/
│ │ │ ├── urdf/
│ │ │ │ ├── component_snippets.xacro
│ │ │ │ ├── hummingbird.xacro
│ │ │ │ ├── hummingbird_base.xacro
│ │ │ │ └── mav_generic_odometry_sensor.gazebo
│ │ │ └── vehicles/
│ │ │ ├── hummingbird.gazebo
│ │ │ └── hummingbird_rgbcamera300200.gazebo
│ │ ├── urdf/
│ │ │ ├── component_snippets.xacro
│ │ │ ├── hummingbird_base.xacro
│ │ │ └── mav_generic_odometry_sensor_rgb.gazebo
│ │ └── worlds/
│ │ ├── controller_learning.world
│ │ ├── random_gates.world
│ │ ├── single_gate.world
│ │ └── track.world
│ ├── rviz/
│ │ └── trajectory_comparison_looping_sim.rviz
│ └── src/
│ ├── fpv_aggressive_trajectories.cpp
│ ├── odometry_republisher.cpp
│ └── visualize.cpp
├── odometry_converter/
│ ├── CMakeLists.txt
│ ├── include/
│ │ └── odometry_converter/
│ │ └── odometry_converter.h
│ ├── package.xml
│ └── src/
│ ├── odometry_converter.cpp
│ └── odometry_converter_node.cpp
├── pip_requirements.txt
├── test_performance/
│ ├── CMakeLists.txt
│ ├── README.md
│ ├── __init__.py
│ ├── launch/
│ │ └── test_performance.launch
│ ├── nodes/
│ │ └── test_performance.py
│ ├── package.xml
│ ├── setup.py
│ └── src/
│ └── TestPerformance/
│ ├── TestPerformance.py
│ └── __init__.py
└── trajectory_visualizer/
├── .gitignore
├── CMakeLists.txt
├── include/
│ └── trajectory_visualizer/
│ └── trajectory_visualizer.h
├── package.xml
└── src/
└── trajectory_visualizer.cpp
SYMBOL INDEX (178 symbols across 35 files)
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/acrobatic_sequence.h
function namespace (line 6) | namespace fpv_aggressive_trajectories {
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/circle_trajectory_helper.h
function namespace (line 6) | namespace acrobatic_trajectory_helper {
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/heading_trajectory_helper.h
function namespace (line 5) | namespace acrobatic_trajectory_helper {
FILE: acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/polynomial_trajectory_helper.h
function namespace (line 9) | namespace acrobatic_trajectory_helper {
FILE: acrobatic_trajectory_helper/src/acrobatic_sequence.cpp
type fpv_aggressive_trajectories (line 13) | namespace fpv_aggressive_trajectories {
FILE: acrobatic_trajectory_helper/src/circle_trajectory_helper.cpp
type acrobatic_trajectory_helper (line 6) | namespace acrobatic_trajectory_helper {
type circles (line 8) | namespace circles {
function computeVerticalCircleTrajectory (line 10) | quadrotor_common::Trajectory computeVerticalCircleTrajectory(
function computeVerticalCircleTrajectoryCorkScrew (line 99) | quadrotor_common::Trajectory computeVerticalCircleTrajectoryCorkScrew(
FILE: acrobatic_trajectory_helper/src/heading_trajectory_helper.cpp
type acrobatic_trajectory_helper (line 5) | namespace acrobatic_trajectory_helper {
type heading (line 7) | namespace heading {
function addConstantHeading (line 9) | void addConstantHeading(const double heading,
function addConstantHeadingRate (line 77) | void addConstantHeadingRate(const double initial_heading,
FILE: acrobatic_trajectory_helper/src/polynomial_trajectory_helper.cpp
type acrobatic_trajectory_helper (line 7) | namespace acrobatic_trajectory_helper {
type polynomials (line 9) | namespace polynomials {
function computeTimeOptimalTrajectory (line 12) | quadrotor_common::Trajectory computeTimeOptimalTrajectory(
function computeFixedTimeTrajectory (line 26) | quadrotor_common::Trajectory computeFixedTimeTrajectory(
function generateMinimumSnapTrajectory (line 39) | quadrotor_common::Trajectory generateMinimumSnapTrajectory(
function generateMinimumSnapTrajectory (line 54) | quadrotor_common::Trajectory generateMinimumSnapTrajectory(
function generateMinimumSnapTrajectoryWithSegmentRefinement (line 72) | quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmen...
function generateMinimumSnapTrajectoryWithSegmentRefinement (line 88) | quadrotor_common::Trajectory generateMinimumSnapTrajectoryWithSegmen...
function generateMinimumSnapRingTrajectory (line 106) | quadrotor_common::Trajectory generateMinimumSnapRingTrajectory(
function generateMinimumSnapRingTrajectory (line 118) | quadrotor_common::Trajectory generateMinimumSnapRingTrajectory(
function generateMinimumSnapRingTrajectoryWithSegmentRefinement (line 133) | quadrotor_common::Trajectory
function generateMinimumSnapRingTrajectoryWithSegmentRefinement (line 147) | quadrotor_common::Trajectory
function samplePolynomial (line 164) | quadrotor_common::Trajectory samplePolynomial(
FILE: controller_learning/common.py
function update_mpc_params (line 4) | def update_mpc_params():
function setup_sim (line 40) | def setup_sim():
function random_replace (line 69) | def random_replace():
function initialize_vio (line 74) | def initialize_vio():
FILE: controller_learning/config/settings.py
function create_settings (line 11) | def create_settings(settings_yaml, mode='test'):
class Settings (line 22) | class Settings:
method __init__ (line 23) | def __init__(self, settings_yaml, generate_log=True):
method add_flags (line 50) | def add_flags(self):
method _add_flags (line 53) | def _add_flags(self):
class TrainSetting (line 57) | class TrainSetting(Settings):
method __init__ (line 58) | def __init__(self, settings_yaml):
method _add_flags (line 63) | def _add_flags(self):
class TestSetting (line 80) | class TestSetting(Settings):
method __init__ (line 81) | def __init__(self, settings_yaml):
method _add_flags (line 86) | def _add_flags(self):
class DaggerSetting (line 101) | class DaggerSetting(Settings):
method __init__ (line 102) | def __init__(self, settings_yaml):
method _add_flags (line 107) | def _add_flags(self):
FILE: controller_learning/iterative_learning_trajectories.py
class Trainer (line 16) | class Trainer():
method __init__ (line 17) | def __init__(self, settings):
method callback_traj_done (line 24) | def callback_traj_done(self, data):
method start_experiment (line 27) | def start_experiment(self, learner):
method perform_training (line 53) | def perform_training(self):
method perform_testing (line 109) | def perform_testing(self):
function main (line 148) | def main():
FILE: controller_learning/src/ControllerLearning/TrajectoryBase.py
class TrajectoryBase (line 23) | class TrajectoryBase(object):
method __init__ (line 24) | def __init__(self, config, mode):
method start_data_recording (line 82) | def start_data_recording(self):
method stop_data_recording (line 86) | def stop_data_recording(self):
method reset_queue (line 92) | def reset_queue(self):
method publish_control_command (line 110) | def publish_control_command(self, control_command):
method preprocess_fts (line 113) | def preprocess_fts(self, data):
method add_missing_fts (line 128) | def add_missing_fts(self, features_dict):
method callback_fts (line 148) | def callback_fts(self, data):
method callback_shutdown (line 160) | def callback_shutdown(self, data):
method callback_nw_switch (line 163) | def callback_nw_switch(self, msg):
method callback_start_trajectory (line 170) | def callback_start_trajectory(self, data):
method callback_start (line 177) | def callback_start(self, data):
method callback_off (line 181) | def callback_off(self, data):
method maneuver_finished (line 185) | def maneuver_finished(self):
method callback_odometry (line 188) | def callback_odometry(self, data):
method callback_gt_odometry (line 195) | def callback_gt_odometry(self, data):
method callback_ref (line 198) | def callback_ref(self, data):
method callback_control_command (line 207) | def callback_control_command(self, data):
method shutdown_requested (line 211) | def shutdown_requested(self):
method _prepare_net_inputs (line 214) | def _prepare_net_inputs(self):
method _generate_control_command (line 255) | def _generate_control_command(self):
method write_csv_header (line 258) | def write_csv_header(self):
FILE: controller_learning/src/ControllerLearning/TrajectoryLearning.py
class TrajectoryLearning (line 18) | class TrajectoryLearning(TrajectoryBase):
method __init__ (line 19) | def __init__(self, config, mode):
method train (line 48) | def train(self):
method callback_success_reset (line 54) | def callback_success_reset(self, data):
method callback_gt_odometry (line 69) | def callback_gt_odometry(self, data):
method callback_vins_mono (line 72) | def callback_vins_mono(self, data):
method vio_init_good (line 76) | def vio_init_good(self):
method save_data (line 85) | def save_data(self):
method callback_control_command (line 156) | def callback_control_command(self, data):
method compute_trajectory_error (line 162) | def compute_trajectory_error(self):
method publish_control_command (line 172) | def publish_control_command(self, control_command):
method _generate_control_command (line 176) | def _generate_control_command(self):
method write_csv_header (line 241) | def write_csv_header(self):
FILE: controller_learning/src/ControllerLearning/models/body_dataset.py
function create_dataset (line 12) | def create_dataset(directory, settings, training=True):
class BodyDataset (line 17) | class BodyDataset:
method __init__ (line 22) | def __init__(self, directory, config, training=True):
method _recursive_list (line 54) | def _recursive_list(self, subpath, fmt='jpg'):
method build_dataset (line 57) | def build_dataset(self):
method _build_dataset (line 60) | def _build_dataset(self):
method _decode_experiment_dir (line 63) | def _decode_experiment_dir(self, directory):
class SafeDataset (line 67) | class SafeDataset(BodyDataset):
method __init__ (line 68) | def __init__(self, directory, config, training=True):
method _decode_experiment_dir (line 72) | def _decode_experiment_dir(self, dir_subpath):
method preprocess_fts (line 161) | def preprocess_fts(self, fts):
method add_missing_fts (line 176) | def add_missing_fts(self, features_dict):
method load_fts_sequence (line 198) | def load_fts_sequence(self, sample_num):
method _dataset_map (line 219) | def _dataset_map(self, sample_num):
method check_equal_dict (line 241) | def check_equal_dict(self, d1, d2):
method _preprocess_fnames (line 251) | def _preprocess_fnames(self):
method _build_dataset (line 304) | def _build_dataset(self):
FILE: controller_learning/src/ControllerLearning/models/bodyrate_learner.py
class BodyrateLearner (line 19) | class BodyrateLearner(object):
method __init__ (line 20) | def __init__(self, settings):
method train_step (line 55) | def train_step(self, inputs, labels):
method val_step (line 65) | def val_step(self, inputs, labels):
method adapt_input_data (line 70) | def adapt_input_data(self, features):
method write_train_summaries (line 78) | def write_train_summaries(self, features, gradients):
method train (line 85) | def train(self):
method test (line 136) | def test(self):
method inference (line 151) | def inference(self, inputs):
FILE: controller_learning/src/ControllerLearning/models/nets.py
function create_network (line 11) | def create_network(settings):
class Network (line 16) | class Network(Model):
method __init__ (line 17) | def __init__(self):
method create (line 20) | def create(self):
method call (line 23) | def call(self, x):
method _create (line 26) | def _create(self):
method _internal_call (line 29) | def _internal_call(self):
class AggressiveNet (line 32) | class AggressiveNet(Network):
method __init__ (line 33) | def __init__(self, config):
method _create (line 38) | def _create(self, input_size, has_bias=True, learn_affine=True):
method _pointnet_branch (line 96) | def _pointnet_branch(self, single_t_features):
method _features_branch (line 102) | def _features_branch(self, input_features):
method _states_branch (line 112) | def _states_branch(self, embeddings):
method _control_branch (line 118) | def _control_branch(self, embeddings):
method _internal_call (line 124) | def _internal_call(self, inputs):
FILE: controller_learning/src/ControllerLearning/models/tf_addons_normalizations.py
class GroupNormalization (line 25) | class GroupNormalization(tf.keras.layers.Layer):
method __init__ (line 72) | def __init__(self,
method build (line 100) | def build(self, input_shape):
method call (line 112) | def call(self, inputs):
method get_config (line 127) | def get_config(self):
method compute_output_shape (line 155) | def compute_output_shape(self, input_shape):
method _reshape_into_groups (line 158) | def _reshape_into_groups(self, inputs, input_shape, tensor_input_shape):
method _apply_normalization (line 167) | def _apply_normalization(self, reshaped_inputs, input_shape):
method _get_reshaped_weights (line 186) | def _get_reshaped_weights(self, input_shape):
method _check_if_input_shape_is_none (line 197) | def _check_if_input_shape_is_none(self, input_shape):
method _set_number_of_groups_for_instance_norm (line 205) | def _set_number_of_groups_for_instance_norm(self, input_shape):
method _check_size_of_dimensions (line 211) | def _check_size_of_dimensions(self, input_shape):
method _check_axis (line 224) | def _check_axis(self):
method _create_input_spec (line 231) | def _create_input_spec(self, input_shape):
method _add_gamma_weight (line 237) | def _add_gamma_weight(self, input_shape):
method _add_beta_weight (line 252) | def _add_beta_weight(self, input_shape):
method _create_broadcast_shape (line 267) | def _create_broadcast_shape(self, input_shape):
class InstanceNormalization (line 275) | class InstanceNormalization(GroupNormalization):
method __init__ (line 311) | def __init__(self, **kwargs):
FILE: controller_learning/test_trajectories.py
function main (line 8) | def main():
FILE: controller_learning/train.py
function main (line 15) | def main():
FILE: custom_rotors_interface/include/custom_rotors_interface/custom_rotors_interface.h
function namespace (line 15) | namespace custom_rotors_interface {
FILE: custom_rotors_interface/src/custom_rotors_interface.cpp
type custom_rotors_interface (line 6) | namespace custom_rotors_interface {
function TorquesAndThrust (line 110) | TorquesAndThrust CustomRotorsInterface::bodyRateControl(
FILE: custom_rotors_interface/src/custom_rotors_interface_node.cpp
function main (line 4) | int main(int argc, char **argv) {
FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/fpv_aggressive_trajectories.h
function namespace (line 19) | namespace fpv_aggressive_trajectories {
FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/odometry_republisher.h
function namespace (line 7) | namespace odometry_republisher {
FILE: fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/visualize.h
function namespace (line 7) | namespace fpv_aggressive_trajectories {
FILE: fpv_aggressive_trajectories/src/fpv_aggressive_trajectories.cpp
type fpv_aggressive_trajectories (line 14) | namespace fpv_aggressive_trajectories {
function main (line 380) | int main(int argc, char** argv) {
FILE: fpv_aggressive_trajectories/src/odometry_republisher.cpp
type odometry_republisher (line 5) | namespace odometry_republisher {
function main (line 76) | int main(int argc, char** argv) {
FILE: fpv_aggressive_trajectories/src/visualize.cpp
type fpv_aggressive_trajectories (line 6) | namespace fpv_aggressive_trajectories {
FILE: odometry_converter/include/odometry_converter/odometry_converter.h
function namespace (line 18) | namespace odometry_converter {
FILE: odometry_converter/src/odometry_converter.cpp
type odometry_converter (line 5) | namespace odometry_converter {
FILE: odometry_converter/src/odometry_converter_node.cpp
function main (line 5) | int main(int argc, char **argv) {
FILE: test_performance/nodes/test_performance.py
class TestRoutine (line 20) | class TestRoutine(object):
method __init__ (line 21) | def __init__(self):
method init_params (line 35) | def init_params(self):
method set_params (line 95) | def set_params(self, curr_idx):
method set_handtuned_params (line 221) | def set_handtuned_params(self):
method run_testing (line 284) | def run_testing(self):
FILE: test_performance/src/TestPerformance/TestPerformance.py
class TestPerformance (line 13) | class TestPerformance(object):
method __init__ (line 14) | def __init__(self, test_mode):
method callback_motor_speed (line 32) | def callback_motor_speed(self, data):
method callback_autopilot (line 35) | def callback_autopilot(self, data):
method run (line 77) | def run(self, num_iterations, params, idx, handtuned_params):
FILE: trajectory_visualizer/include/trajectory_visualizer/trajectory_visualizer.h
function namespace (line 8) | namespace trajectory_visualizer {
FILE: trajectory_visualizer/src/trajectory_visualizer.cpp
type trajectory_visualizer (line 7) | namespace trajectory_visualizer {
function main (line 235) | int main(int argc, char** argv) {
Condensed preview — 174 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (651K chars).
[
{
"path": ".clang-format",
"chars": 3295,
"preview": "---\nLanguage: Cpp\n# BasedOnStyle: Google\nAccessModifierOffset: -1\nAlignAfterOpenBracket: Align\nAlignConsecutiveA"
},
{
"path": ".gitignore",
"chars": 45,
"preview": ".idea/\ncmake-build-debug/\n__pycache__/\n*.pyc\n"
},
{
"path": "LICENSE",
"chars": 1086,
"preview": "MIT License\n\nCopyright (c) 2020 Robotics and Perception Group\n\nPermission is hereby granted, free of charge, to any pers"
},
{
"path": "README.md",
"chars": 5266,
"preview": "# Deep Drone Acrobatics\n\nThis repo contains the code associated to our paper Deep Drone Acrobatics. \n\n<p align=\"center\">"
},
{
"path": "acrobatic_trajectory_helper/.gitignore",
"chars": 26,
"preview": ".idea/\ncmake-build-debug/\n"
},
{
"path": "acrobatic_trajectory_helper/CMakeLists.txt",
"chars": 541,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(acrobatic_trajectory_helper)\n\n## Compile as C++11, supported in ROS Kineti"
},
{
"path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/acrobatic_sequence.h",
"chars": 1297,
"preview": "#pragma once\n\n#include <quadrotor_common/trajectory.h>\n#include <list>\n\nnamespace fpv_aggressive_trajectories {\nclass Ac"
},
{
"path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/circle_trajectory_helper.h",
"chars": 984,
"preview": "#pragma once\n\n#include <quadrotor_common/trajectory.h>\n#include <Eigen/Dense>\n\nnamespace acrobatic_trajectory_helper {\n\n"
},
{
"path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/heading_trajectory_helper.h",
"chars": 487,
"preview": "#pragma once\n\n#include <quadrotor_common/trajectory.h>\n\nnamespace acrobatic_trajectory_helper {\n\nnamespace heading {\n\nvo"
},
{
"path": "acrobatic_trajectory_helper/include/acrobatic_trajectory_helper/polynomial_trajectory_helper.h",
"chars": 4095,
"preview": "#pragma once\n\n#include <polynomial_trajectories/polynomial_trajectory.h>\n#include <polynomial_trajectories/polynomial_tr"
},
{
"path": "acrobatic_trajectory_helper/package.xml",
"chars": 632,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>acrobatic_trajectory_helper</name>\n <version>0.0.0</version>\n <desc"
},
{
"path": "acrobatic_trajectory_helper/src/acrobatic_sequence.cpp",
"chars": 15142,
"preview": "#include \"acrobatic_trajectory_helper/acrobatic_sequence.h\"\n\n#include <acrobatic_trajectory_helper/circle_trajectory_hel"
},
{
"path": "acrobatic_trajectory_helper/src/circle_trajectory_helper.cpp",
"chars": 8638,
"preview": "#include \"acrobatic_trajectory_helper/circle_trajectory_helper.h\"\n\n#include <quadrotor_common/math_common.h>\n#include <q"
},
{
"path": "acrobatic_trajectory_helper/src/heading_trajectory_helper.cpp",
"chars": 4069,
"preview": "#include \"acrobatic_trajectory_helper/heading_trajectory_helper.h\"\n\n#include <quadrotor_common/math_common.h>\n\nnamespace"
},
{
"path": "acrobatic_trajectory_helper/src/polynomial_trajectory_helper.cpp",
"chars": 8436,
"preview": "#include \"acrobatic_trajectory_helper/polynomial_trajectory_helper.h\"\n\n#include <polynomial_trajectories/constrained_pol"
},
{
"path": "conda_requirements.txt",
"chars": 290,
"preview": "defusedxml==0.6.0\ndocutils==0.15.2\ngoogle-auth-oauthlib==0.4.1\ngoogle-pasta==0.1.7\nKeras-Applications==1.0.8\nKeras-Prepr"
},
{
"path": "controller_learning/CMakeLists.txt",
"chars": 727,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(controller_learning)\n\n## Compile as C++11, supported in ROS Kinetic and ne"
},
{
"path": "controller_learning/README.md",
"chars": 1,
"preview": "\n"
},
{
"path": "controller_learning/common.py",
"chars": 4790,
"preview": "import os\nimport time\n\ndef update_mpc_params():\n params = {}\n params['Q_pos_xy'] = 200.0\n params['Q_pos_z'] = 5"
},
{
"path": "controller_learning/config/dagger_settings.yaml",
"chars": 1052,
"preview": "log_dir: \"results/loop\"\nquad_name: \"hummingbird\"\nverbose: False\nseq_len: 1 # History Lenght. When increasing to more tha"
},
{
"path": "controller_learning/config/settings.py",
"chars": 5720,
"preview": "import math\nimport os\nimport shutil\nimport sys\nimport time\nimport datetime\n\nimport yaml\n\n\ndef create_settings(settings_y"
},
{
"path": "controller_learning/config/test_settings.yaml",
"chars": 420,
"preview": "quad_name: 'hummingbird'\nlog_dir: '/tmp/test'\nideal_inputs: False\nseq_len: 1\nverbose: False\nthrow_params:\n vlin_range: "
},
{
"path": "controller_learning/config/train_settings.yaml",
"chars": 555,
"preview": "log_dir: 'results/train_fma_cork_seq_1_fts_tracks_newcam_new_end'\nquad_name: 'none'\nseq_len: 1 # History Lenght. When in"
},
{
"path": "controller_learning/data/.gitignore",
"chars": 16,
"preview": "*\n*/\n!.gitignore"
},
{
"path": "controller_learning/iterative_learning_trajectories.py",
"chars": 8078,
"preview": "#!/usr/bin/env python3\n\nimport argparse\nimport os\nimport sys\nimport time\nimport numpy as np\nimport rospy\nfrom Controller"
},
{
"path": "controller_learning/launch/simulation/base_quad_simulation_no_controller.launch",
"chars": 2194,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"world_name\"/>\n <arg name=\"paused\"/>\n <arg name="
},
{
"path": "controller_learning/launch/simulation/controller.launch",
"chars": 684,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"controller\"/>\n <arg name=\"use_ground_truth"
},
{
"path": "controller_learning/launch/simulation/custom_rotors_interface.launch",
"chars": 1176,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defau"
},
{
"path": "controller_learning/launch/simulation/minisim.launch",
"chars": 2464,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defau"
},
{
"path": "controller_learning/launch/simulation/quadrotor_empty_world_no_controller.launch",
"chars": 1737,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n\n <arg name=\"mav_name\" default=\"$(arg qu"
},
{
"path": "controller_learning/launch/simulation/quadrotor_rgb_world_no_controller.launch",
"chars": 1940,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n\n <arg name=\"mav_name\" default=\"$(ar"
},
{
"path": "controller_learning/launch/simulation/rpg_mpc.launch",
"chars": 2163,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"use_ground_truth\"/>\n <arg name=\"enable_com"
},
{
"path": "controller_learning/launch/simulation/rpg_pid.launch",
"chars": 2464,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"use_ground_truth\"/>\n <arg name=\"enable_com"
},
{
"path": "controller_learning/launch/simulation/rpg_rotors_interface.launch",
"chars": 761,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defau"
},
{
"path": "controller_learning/launch/simulation/rviz_only.launch",
"chars": 446,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defau"
},
{
"path": "controller_learning/launch/simulation/simulation.launch",
"chars": 2420,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defau"
},
{
"path": "controller_learning/package.xml",
"chars": 696,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>controller_learning</name>\n <version>0.0.0</version>\n <description>"
},
{
"path": "controller_learning/results/.gitignore",
"chars": 17,
"preview": "*\n*/\n!.gitignore\n"
},
{
"path": "controller_learning/setup.py",
"chars": 228,
"preview": "#!/usr/bin/env python\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\n\nd"
},
{
"path": "controller_learning/src/ControllerLearning/TrajectoryBase.py",
"chars": 11103,
"preview": "#!/usr/bin/env python3\nimport collections\nimport copy\nimport csv\nimport datetime\nimport os\nimport random\n\nimport numpy a"
},
{
"path": "controller_learning/src/ControllerLearning/TrajectoryLearning.py",
"chars": 13796,
"preview": "#!/usr/bin/env python3\nimport csv\nimport datetime\nimport os\nimport random\n\nimport numpy as np\nimport rospy\nfrom nav_msgs"
},
{
"path": "controller_learning/src/ControllerLearning/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "controller_learning/src/ControllerLearning/models/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "controller_learning/src/ControllerLearning/models/body_dataset.py",
"chars": 13314,
"preview": "import numpy as np\nimport glob\nimport os\nimport pandas as pd\nimport tensorflow as tf\nimport fnmatch\nimport cv2\nimport ra"
},
{
"path": "controller_learning/src/ControllerLearning/models/bodyrate_learner.py",
"chars": 6382,
"preview": "import datetime\nimport os\n\nimport numpy as np\nimport tensorflow as tf\nfrom tqdm import tqdm\n\n# Required for catkin impor"
},
{
"path": "controller_learning/src/ControllerLearning/models/nets.py",
"chars": 6166,
"preview": "import tensorflow as tf\nfrom tensorflow.keras import Model\nfrom tensorflow.keras.layers import Dense, Conv2D, LeakyReLU,"
},
{
"path": "controller_learning/src/ControllerLearning/models/tf_addons_normalizations.py",
"chars": 12136,
"preview": "# Copyright 2019 The TensorFlow Authors. All Rights Reserved.\n#\n# Licensed under the Apache License, Version 2.0 (the \"L"
},
{
"path": "controller_learning/test_trajectories.py",
"chars": 624,
"preview": "import argparse\n\nfrom common import setup_sim, update_mpc_params\nfrom config.settings import create_settings\nfrom iterat"
},
{
"path": "controller_learning/train.py",
"chars": 647,
"preview": "#!/usr/bin/env python3\n\nimport argparse\nimport os\nimport sys\nsys.path.append(\"./src/ControllerLearning/models/\")\nimport "
},
{
"path": "controller_learning/train_logs/20191015-164750/train/checkpoint",
"chars": 344,
"preview": "model_checkpoint_path: \"ckpt-80\"\nall_model_checkpoint_paths: \"ckpt-78\"\nall_model_checkpoint_paths: \"ckpt-79\"\nall_model_c"
},
{
"path": "controller_learning/train_logs/20191022-082718/train/checkpoint",
"chars": 355,
"preview": "model_checkpoint_path: \"ckpt-1080\"\nall_model_checkpoint_paths: \"ckpt-1078\"\nall_model_checkpoint_paths: \"ckpt-1079\"\nall_m"
},
{
"path": "controller_learning/train_logs/20191022-095948/train/checkpoint",
"chars": 353,
"preview": "model_checkpoint_path: \"ckpt-1000\"\nall_model_checkpoint_paths: \"ckpt-998\"\nall_model_checkpoint_paths: \"ckpt-999\"\nall_mod"
},
{
"path": "custom_rotors_interface/.gitignore",
"chars": 19,
"preview": "cmake-build-debug/\n"
},
{
"path": "custom_rotors_interface/CMakeLists.txt",
"chars": 623,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(custom_rotors_interface)\n\n# Add plain cmake packages\nfind_package(catkin_s"
},
{
"path": "custom_rotors_interface/include/custom_rotors_interface/custom_rotors_interface.h",
"chars": 2778,
"preview": "#pragma once\n\n#include <ros/ros.h>\n#include <Eigen/Eigen>\n\n#include <geometry_msgs/PoseStamped.h>\n#include <quadrotor_co"
},
{
"path": "custom_rotors_interface/package.xml",
"chars": 750,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>custom_rotors_interface</name>\n <version>0.0.0</version>\n <de"
},
{
"path": "custom_rotors_interface/parameters/autopilot.yaml",
"chars": 923,
"preview": "state_estimate_timeout: 5.0 # [s]\nvelocity_estimate_in_world_frame: false\ncontrol_command_delay: 0.05 # [s]\n\nstart_land_"
},
{
"path": "custom_rotors_interface/parameters/custom_rotors_interface.yaml",
"chars": 356,
"preview": "# Vehicle parameters\ninertia_x: 0.007\ninertia_y: 0.007\ninertia_z: 0.012\narm_length: 0.17\nmass: 0.73\n# Motors parameters\n"
},
{
"path": "custom_rotors_interface/parameters/manual_flight_assistant.yaml",
"chars": 139,
"preview": "joypad_timeout: 0.5\nsbus_timeout: 0.5\njoypad_axes_zero_tolerance: 0.05\nsbus_axes_zero_tolerance: 10\nvmax_xy: 1.5\nvmax_z:"
},
{
"path": "custom_rotors_interface/parameters/position_controller.yaml",
"chars": 346,
"preview": "position_controller:\n use_rate_mode: true\n \n kpxy: 10.0\n kdxy: 4.0\n \n kpz: 15.0\n kdz: 6.0\n krp: 12.0\n kyaw: 5.0"
},
{
"path": "custom_rotors_interface/src/custom_rotors_interface.cpp",
"chars": 9280,
"preview": "#include \"custom_rotors_interface/custom_rotors_interface.h\"\n\n#include <quadrotor_common/math_common.h>\n#include \"quadro"
},
{
"path": "custom_rotors_interface/src/custom_rotors_interface_node.cpp",
"chars": 271,
"preview": "#include <custom_rotors_interface/custom_rotors_interface.h>\n#include <memory>\n\nint main(int argc, char **argv) {\n ros:"
},
{
"path": "dependencies.yaml",
"chars": 2083,
"preview": "repositories:\n catkin_boost_python_buildtool:\n type: git\n url: git@github.com:ethz-asl/catkin_boost_python_buildt"
},
{
"path": "fpv_aggressive_trajectories/.gitignore",
"chars": 26,
"preview": "cmake-build-debug/\n.idea/\n"
},
{
"path": "fpv_aggressive_trajectories/CMakeLists.txt",
"chars": 791,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(fpv_aggressive_trajectories)\n\n## Compile as C++11, supported in ROS Kineti"
},
{
"path": "fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/fpv_aggressive_trajectories.h",
"chars": 3561,
"preview": "#pragma once\n\n#include <ros/ros.h>\n#include <Eigen/Core>\n\n#include <autopilot/autopilot_helper.h>\n#include <nav_msgs/Odo"
},
{
"path": "fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/odometry_republisher.h",
"chars": 589,
"preview": "#pragma once\n\n#include <nav_msgs/Odometry.h>\n#include <ros/ros.h>\n#include <pose_utils/pose.h>\n\nnamespace odometry_repub"
},
{
"path": "fpv_aggressive_trajectories/include/fpv_aggressive_trajectories/visualize.h",
"chars": 1020,
"preview": "#pragma once\n#include <ros/ros.h>\n#include <visualization_msgs/MarkerArray.h>\n\n#include \"quadrotor_common/trajectory.h\"\n"
},
{
"path": "fpv_aggressive_trajectories/launch/simulation/controller.launch",
"chars": 692,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"controller\"/>\n <arg name=\"use_ground_truth"
},
{
"path": "fpv_aggressive_trajectories/launch/simulation/custom_rotors_interface.launch",
"chars": 1176,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defau"
},
{
"path": "fpv_aggressive_trajectories/launch/simulation/rpg_mpc.launch",
"chars": 2410,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"use_ground_truth\"/>\n <arg name=\"enable_com"
},
{
"path": "fpv_aggressive_trajectories/launch/simulation/rpg_pid.launch",
"chars": 2318,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n\n <arg name=\"quad_name\"/>\n <arg name=\"use_ground_truth\"/>\n <arg name=\"enable_com"
},
{
"path": "fpv_aggressive_trajectories/launch/simulation/simulation.launch",
"chars": 3260,
"preview": "<?xml version=\"1.0\"?>\n<launch>\n <arg name=\"quad_name\" default=\"hummingbird\"/>\n <arg name=\"use_ground_truth\" defaul"
},
{
"path": "fpv_aggressive_trajectories/package.xml",
"chars": 787,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>fpv_aggressive_trajectories</name>\n <version>0.0.0</version>\n <desc"
},
{
"path": "fpv_aggressive_trajectories/parameters/mpc_params.yaml",
"chars": 2156,
"preview": "# rpg_quadrotor_mpc\n# A model predictive control implementation for quadrotors.\n# Copyright (C) 2017-2018 Ph"
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/asphalt.material",
"chars": 261,
"preview": "material iros/asphalt\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\tspecular 0.2 0"
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/carpet.material",
"chars": 252,
"preview": "material iros/carpet\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.2 0.2 0.2 1.0\n\t\t\tdiffuse 0.8 0.8 0.8 1.0\n\t\t\temissive 0.5 0."
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_1.material",
"chars": 253,
"preview": "material iros/wall_1\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0."
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_2.material",
"chars": 253,
"preview": "material iros/wall_2\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0."
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_3.material",
"chars": 253,
"preview": "material iros/wall_3\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0."
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/iros_materials/materials/scripts/wall_4.material",
"chars": 253,
"preview": "material iros/wall_4\n{\n\ttechnique\n\t{\n\t\tpass\n\t\t{\n\t\t\tambient 0.5 0.5 0.5 1.0\n\t\t\tdiffuse 0.5 0.5 0.5 1.0\n\t\t\temissive 0.5 0."
},
{
"path": "fpv_aggressive_trajectories/resources/race_track/real_world/gate/meshes/gate.dae",
"chars": 169605,
"preview": "<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\" ?>\r\n<COLLADA version=\"1.4.1\" xmlns=\"http://www.collada.org/2005/11/"
},
{
"path": "fpv_aggressive_trajectories/resources/rviz/race_track.rviz",
"chars": 8938,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "fpv_aggressive_trajectories/resources/rviz/real_world.rviz",
"chars": 7349,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 0\n Name: Displays\n Property Tree Widget:\n Expanded: ~\n "
},
{
"path": "fpv_aggressive_trajectories/resources/simulation/urdf/component_snippets.xacro",
"chars": 41356,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/simulation/urdf/hummingbird.xacro",
"chars": 6331,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/simulation/urdf/hummingbird_base.xacro",
"chars": 2154,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/simulation/urdf/mav_generic_odometry_sensor.gazebo",
"chars": 2380,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/simulation/vehicles/hummingbird.gazebo",
"chars": 1626,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/simulation/vehicles/hummingbird_rgbcamera300200.gazebo",
"chars": 1626,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/urdf/component_snippets.xacro",
"chars": 48902,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/urdf/hummingbird_base.xacro",
"chars": 2224,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/urdf/mav_generic_odometry_sensor_rgb.gazebo",
"chars": 3546,
"preview": "<?xml version=\"1.0\"?>\n<!--\n Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland\n Copyright 2015 Michael Burri, A"
},
{
"path": "fpv_aggressive_trajectories/resources/worlds/controller_learning.world",
"chars": 6474,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.6\">\n <world name=\"real_track\">\n\n <plugin name=\"ros_interface_plugin\" filename=\"l"
},
{
"path": "fpv_aggressive_trajectories/resources/worlds/random_gates.world",
"chars": 14307,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.6\">\n <world name=\"real_track\">\n\n <plugin name=\"ros_interface_plugin\" filename=\"l"
},
{
"path": "fpv_aggressive_trajectories/resources/worlds/single_gate.world",
"chars": 6458,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.6\">\n <world name=\"real_track\">\n\n <plugin name=\"ros_interface_plugin\" filename="
},
{
"path": "fpv_aggressive_trajectories/resources/worlds/track.world",
"chars": 11568,
"preview": "<?xml version=\"1.0\"?>\n<sdf version=\"1.6\">\n <world name=\"real_track\">\n\n <plugin name=\"ros_interface_plugin\" filename=\"l"
},
{
"path": "fpv_aggressive_trajectories/rviz/trajectory_comparison_looping_sim.rviz",
"chars": 12082,
"preview": "Panels:\n - Class: rviz/Displays\n Help Height: 78\n Name: Displays\n Property Tree Widget:\n Expanded:\n "
},
{
"path": "fpv_aggressive_trajectories/src/fpv_aggressive_trajectories.cpp",
"chars": 15843,
"preview": "#include \"fpv_aggressive_trajectories/fpv_aggressive_trajectories.h\"\n\n#include <string>\n\n#include <minimum_jerk_trajecto"
},
{
"path": "fpv_aggressive_trajectories/src/odometry_republisher.cpp",
"chars": 3417,
"preview": "#include \"fpv_aggressive_trajectories/odometry_republisher.h\"\n\n#include <quadrotor_common/geometry_eigen_conversions.h>\n"
},
{
"path": "fpv_aggressive_trajectories/src/visualize.cpp",
"chars": 9248,
"preview": "#include \"fpv_aggressive_trajectories/visualize.h\"\n\n#include <tf/transform_broadcaster.h>\n#include <tf/transform_listene"
},
{
"path": "odometry_converter/CMakeLists.txt",
"chars": 561,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(odometry_converter)\n\nfind_package(catkin_simple REQUIRED)\ncatkin_simple(AL"
},
{
"path": "odometry_converter/include/odometry_converter/odometry_converter.h",
"chars": 1583,
"preview": "#pragma once\n\n#include <ros/ros.h>\n#include <Eigen/Eigen>\n#include <fstream>\n\n#include <geometry_msgs/PoseStamped.h>\n#in"
},
{
"path": "odometry_converter/package.xml",
"chars": 594,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>odometry_converter</name>\n <version>0.0.0</version>\n <descrip"
},
{
"path": "odometry_converter/src/odometry_converter.cpp",
"chars": 8488,
"preview": "#include \"odometry_converter/odometry_converter.h\"\n\n#include \"minkindr_conversions/kindr_msg.h\"\n\nnamespace odometry_conv"
},
{
"path": "odometry_converter/src/odometry_converter_node.cpp",
"chars": 243,
"preview": "#include <memory>\n\n#include \"odometry_converter/odometry_converter.h\"\n\nint main(int argc, char **argv) {\n ros::init(arg"
},
{
"path": "pip_requirements.txt",
"chars": 305,
"preview": "cachetools==4.0.0\ncatkin-tools==0.4.5\nempy==3.3.4\npyside2==5.13.2\nosrf-pycommon==0.1.8\npyqt5-sip==12.7.0\nshiboken2==5.13"
},
{
"path": "test_performance/CMakeLists.txt",
"chars": 465,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(test_performance)\n\nfind_package(catkin REQUIRED COMPONENTS\n roscpp\n rosp"
},
{
"path": "test_performance/README.md",
"chars": 95,
"preview": "# Test Performance\n\nPerform multiple experiments to do evaluation/random search of parameters.\n"
},
{
"path": "test_performance/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "test_performance/launch/test_performance.launch",
"chars": 212,
"preview": "<?xml version=\"1.0\"?>\n<launch>\t\n <node name=\"test_performance\" pkg=\"test_performance\" type=\"test_performance.py\" output"
},
{
"path": "test_performance/nodes/test_performance.py",
"chars": 17622,
"preview": "#!/usr/bin/env python\n\nimport rospy\nfrom TestPerformance import TestPerformance\nimport os, datetime\nimport numpy as np\ni"
},
{
"path": "test_performance/package.xml",
"chars": 533,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>test_performance</name>\n <version>0.0.1</version>\n <description>Lea"
},
{
"path": "test_performance/setup.py",
"chars": 225,
"preview": "#!/usr/bin/env python\n\nfrom distutils.core import setup\nfrom catkin_pkg.python_setup import generate_distutils_setup\n\n\nd"
},
{
"path": "test_performance/src/TestPerformance/TestPerformance.py",
"chars": 8242,
"preview": "# /usr/bin/env python\nimport rospy\nimport os\nimport time\nimport json\nimport numpy as np\n\nfrom std_msgs.msg import Empty\n"
},
{
"path": "test_performance/src/TestPerformance/__init__.py",
"chars": 0,
"preview": ""
},
{
"path": "trajectory_visualizer/.gitignore",
"chars": 26,
"preview": "cmake-build-debug/\n.idea/\n"
},
{
"path": "trajectory_visualizer/CMakeLists.txt",
"chars": 357,
"preview": "cmake_minimum_required(VERSION 2.8.3)\nproject(trajectory_visualizer)\n\n## Compile as C++11, supported in ROS Kinetic and "
},
{
"path": "trajectory_visualizer/include/trajectory_visualizer/trajectory_visualizer.h",
"chars": 911,
"preview": "#pragma once\n\n#include <list>\n\n#include <quadrotor_msgs/AutopilotFeedback.h>\n#include <ros/ros.h>\n\nnamespace trajectory_"
},
{
"path": "trajectory_visualizer/package.xml",
"chars": 583,
"preview": "<?xml version=\"1.0\"?>\n<package format=\"2\">\n <name>trajectory_visualizer</name>\n <version>0.0.0</version>\n <descriptio"
},
{
"path": "trajectory_visualizer/src/trajectory_visualizer.cpp",
"chars": 8127,
"preview": "#include \"trajectory_visualizer/trajectory_visualizer.h\"\n\n#include <nav_msgs/Odometry.h>\n#include <quadrotor_common/para"
}
]
// ... and 53 more files (download for full content)
About this extraction
This page contains the full source code of the uzh-rpg/deep_drone_acrobatics GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 174 files (608.2 KB), approximately 223.1k tokens, and a symbol index with 178 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.