Merge pull request #2 from L-ED/led

Led

README.md

@@ -1,17 +1,64 @@
-# gym-pybullet-drones
+# RL_DRONE
+Diploma work based on PyBullet physics simulator. Target of this work is to develop position control system using only following input data: 
+- linear and angular velocity in drone local coordinates
+- projected gravity [orientation analog]
+- position displacement
 
-This is a minimalist refactoring of the original `gym-pybullet-drones` repository, designed for compatibility with [`gymnasium`](https://github.com/Farama-Foundation/Gymnasium), [`stable-baselines3` 2.0](https://github.com/DLR-RM/stable-baselines3/pull/1327), and SITL [`betaflight`](https://github.com/betaflight/betaflight)/[`crazyflie-firmware`](https://github.com/bitcraze/crazyflie-firmware/).
+This means drone can be operated from integrated imu data without additional velocity postprocessing from local to global coordinates
 
-> **NOTE**: if you prefer to access the original codebase, presented at IROS in 2021, please `git checkout [paper|master]` after cloning the repo, and refer to the corresponding `README.md`'s.
+Also this work contains absolute position controllers from 
+https://github.com/utiasDSL/gym-pybullet-drones.git 
 
-<img src="files/readme_images/helix.gif" alt="formation flight" width="350"> <img src="files/readme_images/helix.png" alt="control info" width="450">
 
-## Installation
+<img src="files/imu_flight.gif" alt="imu flight" width="800">
+
+## Main contributions
+- **IMU Sensor model** gym_pybullet_drones/devices/sensors/imu.py
+- **Physical drone model** gym_pybullet_drones/assets/custom.urdf
+- **Environment for system learning** gym_pybullet_drones/envs/single_agent_rl/hover
+- Sensor asynchronous clocking
+Added model of physical drone 
+
+### Drone Model
+- Calculated inertia from physical analog using Autodesk Inventor
+- Estimated Lift and Torque coefficients from open [database](https://database.tytorobotics.com/propellers/96z/6045-plastic), duplicated in gym_pybullet_drones/thrust_torque_estimation
 
-Tested on Intel x64/Ubuntu 22.04 and Apple Silicon/macOS 13.4.
+<img src="files/model.png" alt="imu flight" width="400">
+<img src="files/physical.jpg" alt="imu flight" width="400">
+
+### Environment
+Target displacement clipped by absolute max value equal 1, so system has direction information when displacement greater than 1 and has exact measure when displacement less than 1
+Sigmoid function with out scaling used to convert prediction values to range of motor speed [0, max_speed] 
+Reward function motivates agent fly to center and angular rate reward penalizes keeps policy from extra rotation
+```math
+reward = closeness*angles \\
+```
+```math
+closeness = \frac{||displacement||}{radius_{max}} \\
+```
+```math
+angles = exp(||\omega||*0.1) 
+```
+During training drone initialized with random position from uniform distribution in sphere with radius 1.5 meters
+
+
+### IMU sensor 
+measurement simulation based on two-component noise model
+```math
+measurement(t) = estimation(t) + bias(t) + noise \\
+```
+```math
+bias(t) =bias(t-1)+N(1,\sigma_{r}*\sqrt{dt})\\
+```
+```math
+noise = N(1,\frac{\sigma_{d}}{\sqrt{dt}})
+```
+
+
+## Installation
 
 ```sh
-git clone https://github.com/utiasDSL/gym-pybullet-drones.git
+git clone https://github.com/L-ED/RL_Drone.git
 cd gym-pybullet-drones/
 
 conda create -n drones python=3.10
@@ -24,86 +71,23 @@ pip3 install -e . # if needed, `sudo apt install build-essentials` to install `g
 
 ## Use
 
-### PID position control example
+### RL Position Control from local coordinates
 
 ```sh
 cd gym_pybullet_drones/examples/
-python3 pid.py
+python hover/hover_learn_multienv.py
 ```
 
-### Stable-baselines3 PPO RL example
+### PID position control example 
 
 ```sh
 cd gym_pybullet_drones/examples/
-python3 learn.py
-```
-
-### Betaflight SITL example (Ubuntu only)
-
-First, check the steps in the docstrings of [`beta.py`](https://github.com/utiasDSL/gym-pybullet-drones/blob/main/gym_pybullet_drones/examples/beta.py), then, in one terminal, run the Betaflight SITL binary
-
-```sh
-git clone https://github.com/betaflight/betaflight
-cd betaflight/ 
-make arm_sdk_install # if needed, `apt install curl``
-make TARGET=SITL # comment out this line: https://github.com/betaflight/betaflight/blob/master/src/main/main.c#L52
-betaflight/obj/main/betaflight_SITL.elf
+python3 pid.py
 ```
 
-In another terminal, run the example
+### Stable-baselines3 PPO RL example
 
 ```sh
-conda activate drones
 cd gym_pybullet_drones/examples/
-python3 beta.py # also check the steps in the file's docstrings
-```
-
-## Troubleshooting
-
-- On Ubuntu, with an NVIDIA card, if you receive a "Failed to create and OpenGL context" message, launch `nvidia-settings` and under "PRIME Profiles" select "NVIDIA (Performance Mode)", reboot and try again.
-
-Run all tests from the top folder with
-
-```sh
-pytest tests/
-```
-
-## Citation
-
-If you wish, please cite our [IROS 2021 paper](https://arxiv.org/abs/2103.02142) ([and original codebase](https://github.com/utiasDSL/gym-pybullet-drones/tree/paper)) as
-
-```bibtex
-@INPROCEEDINGS{panerati2021learning,
-      title={Learning to Fly---a Gym Environment with PyBullet Physics for Reinforcement Learning of Multi-agent Quadcopter Control}, 
-      author={Jacopo Panerati and Hehui Zheng and SiQi Zhou and James Xu and Amanda Prorok and Angela P. Schoellig},
-      booktitle={2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)},
-      year={2021},
-      volume={},
-      number={},
-      pages={7512-7519},
-      doi={10.1109/IROS51168.2021.9635857}
-}
-```
-
-## References
-
-- Carlos Luis and Jeroome Le Ny (2016) [*Design of a Trajectory Tracking Controller for a Nanoquadcopter*](https://arxiv.org/pdf/1608.05786.pdf)
-- Nathan Michael, Daniel Mellinger, Quentin Lindsey, Vijay Kumar (2010) [*The GRASP Multiple Micro UAV Testbed*](http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.169.1687&rep=rep1&type=pdf)
-- Benoit Landry (2014) [*Planning and Control for Quadrotor Flight through Cluttered Environments*](http://groups.csail.mit.edu/robotics-center/public_papers/Landry15)
-- Julian Forster (2015) [*System Identification of the Crazyflie 2.0 Nano Quadrocopter*](https://www.research-collection.ethz.ch/handle/20.500.11850/214143)
-- Antonin Raffin, Ashley Hill, Maximilian Ernestus, Adam Gleave, Anssi Kanervisto, and Noah Dormann (2019) [*Stable Baselines3*](https://github.com/DLR-RM/stable-baselines3)
-- Guanya Shi, Xichen Shi, Michael O’Connell, Rose Yu, Kamyar Azizzadenesheli, Animashree Anandkumar, Yisong Yue, and Soon-Jo Chung (2019)
-[*Neural Lander: Stable Drone Landing Control Using Learned Dynamics*](https://arxiv.org/pdf/1811.08027.pdf)
-- C. Karen Liu and Dan Negrut (2020) [*The Role of Physics-Based Simulators in Robotics*](https://www.annualreviews.org/doi/pdf/10.1146/annurev-control-072220-093055)
-- Yunlong Song, Selim Naji, Elia Kaufmann, Antonio Loquercio, and Davide Scaramuzza (2020) [*Flightmare: A Flexible Quadrotor Simulator*](https://arxiv.org/pdf/2009.00563.pdf)
-
-## TODO
-
-- [ ] Add `crazyflie-firmware` SITL support @spencerteetaert
-- [ ] Add motor delay @JacopoPan / @spencerteetaert
-- [ ] Replace `rpy` with quaternions (and `ang_vel` with body rates) in `obs` @JacopoPan
-- [ ] Replace `BaseSingleAgentAviary` and `BaseMultiAgentAviary` with a single `RLAviary`, incl. PR #161 @JacopoPan
-- [ ] Add a multi-agent MDP with 2-drone chase through a gate @JacopoPan
-
------
-> University of Toronto's [Dynamic Systems Lab](https://github.com/utiasDSL) / [Vector Institute](https://github.com/VectorInstitute) / University of Cambridge's [Prorok Lab](https://github.com/proroklab)
+python3 learn.py
+```