swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: swangnice:93394853114c358005878f2306bbd9c433b4abf9
Branches Tags
swangnice:main
...
compare: swangnice:1db08002855e8168cd3a97fbbc2533a66d19f764
Branches Tags
swangnice:main

10 Commits
9339485311 ... 1db0800285

Author SHA1 Message Date
shibo
1db0800285 add path finding 2025-01-25 16:55:59 -05:00
spencerfolk
317ffbb5d6 Fixed comments for clarity (Issue #15) 2025-01-20 12:36:00 -05:00
spencerfolk
68ad764727 Made quadrotor gymnasium env compatible with Gymnasium 1.0.0 (Issue #11) 2025-01-20 12:32:43 -05:00
spencerfolk
8500cad9fa More customization when using world.draw() 2025-01-20 12:09:44 -05:00
spencerfolk
6a9b685203 Clip motor speeds AFTER noise is added. 2025-01-20 12:05:16 -05:00
spencerfolk
0608381234 Speed up 2d lidar by ignoring faraway objects. 2025-01-20 12:04:51 -05:00
spencerfolk
d355a11e08 Remove timeout decorator dependency. 2025-01-20 12:03:51 -05:00
spencerfolk
ec2e1739c5 Merge pull request #13 from spencerfolk/pypi-package 
Migrated build to toml, published as PyPI package!
 2024-11-26 16:02:54 -05:00
spencerfolk
75b203c265 Migrated build to toml, published as PyPI package! 2024-11-26 16:00:54 -05:00
spencerfolk
12c5ff4837 Added multi mav demo script. Enhanced existing trajectories. 2024-11-26 13:44:28 -05:00
25 changed files with 1128 additions and 160 deletions
Expand all files Collapse all files

8
.idea/.gitignore generated vendored Normal file
View File

@@ -0,0 +1,8 @@
# Default ignored files
/shelf/
/workspace.xml
# Editor-based HTTP Client requests
/httpRequests/
# Datasource local storage ignored files
/dataSources/
/dataSources.local.xml

6
.idea/inspectionProfiles/profiles_settings.xml generated Normal file
View File

@@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>

4
.idea/misc.xml generated Normal file
View File

@@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="/opt/miniconda3" project-jdk-type="Python SDK" />
</project>

8
.idea/modules.xml generated Normal file
View File

@@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/quadcopter_learning.iml" filepath="$PROJECT_DIR$/.idea/quadcopter_learning.iml" />
</modules>
</component>
</project>

12
.idea/quadcopter_learning.iml generated Normal file
View File

@@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="/opt/miniconda3" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">
<option name="format" value="PLAIN" />
<option name="myDocStringFormat" value="Plain" />
</component>
</module>

6
.idea/vcs.xml generated Normal file
View File

@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="" vcs="Git" />
</component>
</project>

101
README.md
View File

@@ -1,101 +0,0 @@
# RotorPy
A Python-based multirotor simulation environment with aerodynamic wrenches, useful for education and research in estimation, planning, and control for UAVs.
<p align="center"><img src="/media/double_pillar.gif" width="32%"/><img src="/media/gusty.gif" width="32%"/><img src="/media/minsnap.gif" width="32%"/></p>
**NEW in `v1.1.0`**: RotorPy now includes a customizable [Gymnasium](https://github.com/Farama-Foundation/Gymnasium) environment found in the new `rotorpy/learning/` module.
<p align="center"><img src="/media/ppo_hover_20k.gif" width="32%"/><img src="/media/ppo_hover_600k.gif" width="32%"/><img src="/media/ppo_hover_1000k.gif" width="32%"/></p>
## Purpose and Scope
The original focus of this simulator was on accurately simulating rotary-wing UAV dynamics with added lumped parameter representations of the aerodynamics for course design and exploratory research. These aerodynamic effects, listed below, are negligible at hover in still air; however, as relative airspeed increases (e.g. for aggressive maneuvers or in the presence of high winds), they quickly become noticeable and force the student/researcher to reconcile with them.
As RotorPy continues to grow, the focus is now on building a realistic dynamics simulator that can scale to quickly generate thousands (or even millions) of simulated rotary-wing UAVs for applications in deep learning, reinforcement learning, and Monte Carlo studies on existing (or new!) algorithms in estimation, planning, and control.
The engine is designed from the bottom up to be lightweight, easy to install with very limited dependencies or requirements, and interpretable to anyone with basic working knowledge of Python. The simulator is intended to gain intuition about UAV dynamics/aerodynamics and learn how to develop control and/or estimation algorithms for rotary wing vehicles subject to aerodynamic wrenches.
The following aerodynamic effects of interest are within the scope of this model:
1. **Parasitic Drag** - Drag associated with non-lifting surfaces like the frame. This drag is quadratic in airspeed.
2. **Rotor Drag** - This is an apparent drag force that is a result of the increased drag produced by the advancing blade of a rotor. Rotor drag is linear in airspeed.
3. **Blade Flapping** - An effect of dissymmetry of lift, blade flapping is the motion of the blade up or down that results in a pitching moment. The pitching moment is linear in the airspeed.
4. **Induced Drag** - Another effect of dissymmetry of lift, more apparent in semi-rigid or rigid blades, where an increase of lift on the advancing blade causes an increased induced downwash, which in turn tilts the lift vector aft resulting in more drag. Induced drag is linear in the airspeed.
5. **Translational Lift** - In forward motion, the induced velocity at the rotor plane decreases, causing an increase in lift generation. *Note: currently this effect is NOT modeled in the thrust produced by the rotor.*
6. **Translational Drag** - A consequence of translational lift, and similar to **Induced Drag**, the increased lift produced in forward flight will produce an increase in induced drag on the rotor.
Ultimately the effects boil down to forces acting anti-parallel to the relative airspeed and a combination of pitching moments acting parallel and perpendicular to the relative airspeed. The rotor aerodynamic effects (rotor drag, blade flapping, induced drag, and translational drag) can be lumped into a single drag force acting at each rotor hub, whereas parasitic drag can be lumped into a single force and moment vector acting at the center of mass.
What's currently ignored: any lift produced by the frame or any torques produced by an imbalance of drag forces on the frame. We also currently neglect variations in the wind along the length of the UAV, implicitly assuming that the characteristic length scales of the wind fields are larger than UAV's maximum dimensions.
RotorPy also includes first-order motor dynamics to simulate lag, as well as support for spatio-temporal wind flow fields for the UAV to interact with.
# Installation
First, clone this repository into a folder of your choosing.
It is recommended that you use a virtual environment to install this simulator. I recommend using [Python venv](https://docs.python.org/3/library/venv.html) because it's lightweight.
All the necessary dependencies can be installed using the following:
**NOTE:** Read this command carefully. The period `.` is intentional.
```
pip install -e .
```
This will install all the packages as specified in `setup.py`. You may need to use `pip3` instead of `pip` to avoid conflict with Python 2 if installed.
To confirm installation, you should see the package `rotorpy` listed among the other packages available in your environment.
# Usage
#### Regular usage
A good place to start would be to reference the `rotorpy/examples/basic_usage.py` script. It goes through the necessary imports and how to create and execute an instance of the simulator.
At minimum the simulator requires vehicle, controller, and trajectory objects. The vehicle (and potentially the controller) is parameterized by a unique parameter file, such as in `rotorpy/vehicles/hummingbird_params.py`. There is also the option to specify your own IMU, world bounds, and how long you would like to run the simulator for.
The output of the simulator is a dictionary containing a time vector and the time histories of all the vehicle's states, inputs, and measurements.
#### Reinforcement Learning
New in `v1.1.0`, RotorPy includes a custom Gymnasium environment, `QuadrotorEnv`, which is a stripped down version of the regular simulation environment intended for applications in reinforcement learning. `QuadrotorEnv` features all the aerodynamics and motor dynamics, but also supports different control abstractions ranging from high level velocity vector commands all the way down to direct individual motor speed commands. This environment also allows the user to specify their own reward function.
For an example of how to interface with this environment, see `rotorpy/examples/gymnasium_basic_usage.py`. You can also see an example of training a quadrotor to hover using this environment in `rotorpy/examples/ppo_hover_train.py` and `rotorpy/examples/ppo_hover_eval.py`.
You can find this new environment in the `rotorpy/learning/` module.
# Development
It is rather straightforward if you would like to add more tracking methods into the simulator. For instance, if you'd like to add a new trajectory generator or a new controller, we've added respective templates that you can use under `rotorpy/trajectories/` and `rotorpy/controllers/` to help structure your code appropriately. If you'd like to add your own wind field, you can add a new class in `rotorpy/wind/` following the template there.
As for adding more core functionality (e.g., sensors, new vehicle dynamics, animations, etc.), those require a bit more effort to make sure that all the inputs and outputs are set up accordingly. One piece of advice is that the main loop occurs in `rotorpy/simulate.py`. Under the `while` loop, you can see the process by which the vehicle dynamics, trajectory generator, IMU sensor, and controller interface with each other.
If you are adding new functionality, as opposed to simply adding new controllers, wind fields, or trajectories, please make a new branch before starting to make those changes.
# Citation
If you use RotorPy for your work please cite our companion workshop paper contributed to the [RS4UAVs Workshop at ICRA 2023](https://imrclab.github.io/workshop-uav-sims-icra2023/):
```
@article{folk2023rotorpy,
title={RotorPy: A Python-based Multirotor Simulator with Aerodynamics for Education and Research},
author={Folk, Spencer and Paulos, James and Kumar, Vijay},
journal={arXiv preprint arXiv:2306.04485},
year={2023}
}
```
This paper addresses the theory, simulation framework, and benchmarking studies.
# Elsewhere In Literature
The following is a selection of papers that have used RotorPy (or previous versions of RotorPy) for generating their simulated results:
1. K. Y. Chee and M. A. Hsieh. "Learning-enhanced Nonlinear Model Predictive Control using Knowledge-based Neural Ordinary Differential Equations and Deep Ensembles," submitted to *5th Annual Learning for Dynamics \& Control Conference*, Philadelphia, PA USA, Jun 2023.
2. K. Y. Chee, T. Z. Jiahao and M. A. Hsieh. "KNODE-MPC: A Knowledge-Based Data-Driven Predictive Control Framework for Aerial Robots,'' in *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 2819-2826, Apr 2022.
3. Jiahao, Tom Z. and Chee, Kong Yao and Hsieh, M. Ani. "Online Dynamics Learning for Predictive Control with an Application to Aerial Robots," in *the Proc. of the 2022 Conference on Robot Learning (CoRL)*, Auckland, NZ, Dec 2022.
4. K. Mao, J. Welde, M. A. Hsieh, and V. Kumar, “Trajectory planning for the bidirectional quadrotor as a differentially flat hybrid system,” in *2023 International Conference on Robotics and Automation (ICRA) (accepted)*, 2023.
5. He, S., Hsu, C. D., Ong, D., Shao, Y. S., & Chaudhari, P. (2023). Active Perception using Neural Radiance Fields. *arXiv preprint arXiv:2310.09892*.
RotorPy was also listed among other UAV simulators in a recent survey:
Dimmig, C. A., Silano, G., McGuire, K., Gabellieri, C., Hönig, W., Moore, J., & Kobilarov, M. (2023). *Survey of Simulators for Aerial Robots*. arXiv preprint arXiv:2311.02296.
**If you use this simulator for published work, let me know as I am happy to add your reference to this list.**
# Acknowledgements
We would like to acknowledge [Jimmy Paulos](https://github.com/jpaulos) who wrote the majority of the underlying engine for RotorPy, and the teaching assistants who contributed code to the initial version of this simulator, especially [Dan Mox](https://github.com/danmox), [Laura Jarin-Lipschitz](https://github.com/ljarin), [Rebecca Li](https://github.com/rebeccali), [Shane Rozen-Levy](https://github.com/ShaneRozenLevy), [Xu Liu](https://github.com/XuRobotics), [Yuezhan Tao](https://github.com/tyuezhan), [Yu-Ming Chen](https://github.com/yminchen), and [Fernando Cladera](https://github.com/fcladera).

148
README_rotor_py.md Normal file
View File

@@ -0,0 +1,148 @@
# RotorPy
A Python-based multirotor simulation environment with aerodynamic wrenches, useful for education and research in estimation, planning, and control for UAVs.
<p align="center"><img src="/media/double_pillar.gif" width="32%"/><img src="/media/gusty.gif" width="32%"/><img src="/media/minsnap.gif" width="32%"/></p>
**NEW in `v1.1.0`**: RotorPy now includes a customizable [Gymnasium](https://github.com/Farama-Foundation/Gymnasium) environment found in the new `rotorpy/learning/` module.
<p align="center"><img src="/media/ppo_hover_20k.gif" width="32%"/><img src="/media/ppo_hover_600k.gif" width="32%"/><img src="/media/ppo_hover_1000k.gif" width="32%"/></p>
## Purpose and Scope
The original focus of this simulator was on accurately simulating rotary-wing UAV dynamics with added lumped parameter representations of the aerodynamics for course design and exploratory research. These aerodynamic effects, listed below, are negligible at hover in still air; however, as relative airspeed increases (e.g. for aggressive maneuvers or in the presence of high winds), they quickly become noticeable and force the student/researcher to reconcile with them.
As RotorPy continues to grow, the focus is now on building a realistic dynamics simulator that can scale to quickly generate thousands (or even millions) of simulated rotary-wing UAVs for applications in deep learning, reinforcement learning, and Monte Carlo studies on existing (or new!) algorithms in estimation, planning, and control.
The engine is designed from the bottom up to be lightweight, easy to install with very limited dependencies or requirements, and interpretable to anyone with basic working knowledge of Python. The simulator is intended to gain intuition about UAV dynamics/aerodynamics and learn how to develop control and/or estimation algorithms for rotary wing vehicles subject to aerodynamic wrenches.
The following aerodynamic effects of interest are within the scope of this model:
1. **Parasitic Drag** - Drag associated with non-lifting surfaces like the frame. This drag is quadratic in airspeed.
2. **Rotor Drag** - This is an apparent drag force that is a result of the increased drag produced by the advancing blade of a rotor. Rotor drag is linear in airspeed.
3. **Blade Flapping** - An effect of dissymmetry of lift, blade flapping is the motion of the blade up or down that results in a pitching moment. The pitching moment is linear in the airspeed.
4. **Induced Drag** - Another effect of dissymmetry of lift, more apparent in semi-rigid or rigid blades, where an increase of lift on the advancing blade causes an increased induced downwash, which in turn tilts the lift vector aft resulting in more drag. Induced drag is linear in the airspeed.
5. **Translational Lift** - In forward motion, the induced velocity at the rotor plane decreases, causing an increase in lift generation. *Note: currently this effect is NOT modeled in the thrust produced by the rotor.*
6. **Translational Drag** - A consequence of translational lift, and similar to **Induced Drag**, the increased lift produced in forward flight will produce an increase in induced drag on the rotor.
Ultimately the effects boil down to forces acting anti-parallel to the relative airspeed and a combination of pitching moments acting parallel and perpendicular to the relative airspeed. The rotor aerodynamic effects (rotor drag, blade flapping, induced drag, and translational drag) can be lumped into a single drag force acting at each rotor hub, whereas parasitic drag can be lumped into a single force and moment vector acting at the center of mass.
What's currently ignored: any lift produced by the frame or any torques produced by an imbalance of drag forces on the frame. We also currently neglect variations in the wind along the length of the UAV, implicitly assuming that the characteristic length scales of the wind fields are larger than UAV's maximum dimensions.
RotorPy also includes first-order motor dynamics to simulate lag, as well as support for spatio-temporal wind flow fields for the UAV to interact with.
# Installation
RotorPy can be installed using `pip`:
```
pip install rotorpy
```
# Usage
There are a few example scripts found in `rotorpy/examples/` that demonstrate how to use RotorPy in a variety of ways including for Monte Carlo evaluations, reinforcement learning, and swarms.
#### Regular usage
A good place to start would be to reference the `rotorpy/examples/basic_usage.py` script. It goes through the necessary imports and how to create and execute an instance of the simulator.
At minimum the simulator requires vehicle, controller, and trajectory objects. The vehicle (and potentially the controller) is parameterized by a unique parameter file, such as in `rotorpy/vehicles/hummingbird_params.py`. There is also the option to specify your own IMU, world bounds, and how long you would like to run the simulator for.
The output of the simulator is a dictionary containing a time vector and the time histories of all the vehicle's states, inputs, and measurements.
Below is a minimum working example:
```
from rotorpy.vehicles.multirotor import Multirotor
from rotorpy.vehicles.crazyflie_params import quad_params
from rotorpy.controllers.quadrotor_control import SE3Control
from rotorpy.trajectories.circular_traj import CircularTraj
from rotorpy.wind.default_winds import SinusoidWind
sim_instance = Environment(vehicle=Multirotor(quad_params), # vehicle object, must be specified.
controller=SE3Control(quad_params), # controller object, must be specified.
trajectory=CircularTraj(radius=2), # trajectory object, must be specified.
wind_profile=SinusoidWind(), # OPTIONAL: wind profile object, if none is supplied it will choose no wind.
sim_rate = 100, # OPTIONAL: The update frequency of the simulator in Hz. Default is 100 Hz.
imu = None, # OPTIONAL: imu sensor object, if none is supplied it will choose a default IMU sensor.
mocap = None, # OPTIONAL: mocap sensor object, if none is supplied it will choose a default mocap.
estimator = None, # OPTIONAL: estimator object
world = None, # OPTIONAL: the world, same name as the file in rotorpy/worlds/, default (None) is empty world
safety_margin= 0.25 # OPTIONAL: defines the radius (in meters) of the sphere used for collision checking
)
x0 = {'x': np.array([0,0,0]),
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
sim_instance.vehicle.initial_state = x0
# The results are a dictionary containing the relevant state, input, and measurements vs time.
results = sim_instance.run(t_final = 20, # The maximum duration of the environment in seconds
use_mocap = False, # Boolean: determines if the controller should use the motion capture estimates.
terminate = False, # Boolean: if this is true, the simulator will terminate when it reaches the last waypoint.
plot = True, # Boolean: plots the vehicle states and commands
plot_mocap = True, # Boolean: plots the motion capture pose and twist measurements
plot_estimator = True, # Boolean: plots the estimator filter states and covariance diagonal elements
plot_imu = True, # Boolean: plots the IMU measurements
animate_bool = True, # Boolean: determines if the animation of vehicle state will play.
animate_wind = True, # Boolean: determines if the animation will include a scaled wind vector to indicate the local wind acting on the UAV.
verbose = True, # Boolean: will print statistics regarding the simulation.
fname = None # Filename is specified if you want to save the animation. The save location is rotorpy/data_out/.
)
```
#### Reinforcement Learning
New in `v1.1.0`, RotorPy includes a custom Gymnasium environment, `QuadrotorEnv`, which is a stripped down version of the regular simulation environment intended for applications in reinforcement learning. `QuadrotorEnv` features all the aerodynamics and motor dynamics, but also supports different control abstractions ranging from high level velocity vector commands all the way down to direct individual motor speed commands. This environment also allows the user to specify their own reward function.
For an example of how to interface with this environment, see `rotorpy/examples/gymnasium_basic_usage.py`. You can also see an example of training a quadrotor to hover using this environment in `rotorpy/examples/ppo_hover_train.py` and `rotorpy/examples/ppo_hover_eval.py`.
You can find this new environment in the `rotorpy/learning/` module.
# Development
It is rather straightforward if you would like to add more tracking methods into the simulator. For instance, if you'd like to add a new trajectory generator or a new controller, we've added respective templates that you can use under `rotorpy/trajectories/` and `rotorpy/controllers/` to help structure your code appropriately. If you'd like to add your own wind field, you can add a new class in `rotorpy/wind/` following the template there.
As for adding more core functionality (e.g., sensors, new vehicle dynamics, animations, etc.), those require a bit more effort to make sure that all the inputs and outputs are set up accordingly. One piece of advice is that the main loop occurs in `rotorpy/simulate.py`. Under the `while` loop, you can see the process by which the vehicle dynamics, trajectory generator, IMU sensor, and controller interface with each other.
If you are adding new functionality, as opposed to simply adding new controllers, wind fields, or trajectories, please make a new branch before starting to make those changes.
# Citation
If you use RotorPy for your work please cite our companion workshop paper contributed to the [RS4UAVs Workshop at ICRA 2023](https://imrclab.github.io/workshop-uav-sims-icra2023/):
```
@article{folk2023rotorpy,
title={{RotorPy}: A Python-based Multirotor Simulator with Aerodynamics for Education and Research},
author={Folk, Spencer and Paulos, James and Kumar, Vijay},
journal={arXiv preprint arXiv:2306.04485},
year={2023}
}
```
This paper addresses the theory, simulation framework, and benchmarking studies.
# Elsewhere In Literature
The following is a selection of papers that have used RotorPy (or previous versions of RotorPy) for generating their simulated results:
1. K. Y. Chee and M. A. Hsieh. "Learning-enhanced Nonlinear Model Predictive Control using Knowledge-based Neural Ordinary Differential Equations and Deep Ensembles," submitted to *5th Annual Learning for Dynamics \& Control Conference*, Philadelphia, PA USA, Jun 2023.
2. K. Y. Chee, T. Z. Jiahao and M. A. Hsieh. "KNODE-MPC: A Knowledge-Based Data-Driven Predictive Control Framework for Aerial Robots,'' in *IEEE Robotics and Automation Letters*, vol. 7, no. 2, pp. 2819-2826, Apr 2022.
3. Jiahao, Tom Z. and Chee, Kong Yao and Hsieh, M. Ani. "Online Dynamics Learning for Predictive Control with an Application to Aerial Robots," in *the Proc. of the 2022 Conference on Robot Learning (CoRL)*, Auckland, NZ, Dec 2022.
4. K. Mao, J. Welde, M. A. Hsieh, and V. Kumar, “Trajectory planning for the bidirectional quadrotor as a differentially flat hybrid system,” in *2023 International Conference on Robotics and Automation (ICRA) (accepted)*, 2023.
5. He, S., Hsu, C. D., Ong, D., Shao, Y. S., & Chaudhari, P. (2023). "Active Perception using Neural Radiance Fields," submitted to *arXiv preprint arXiv:2310.09892*.
6. Tao, R., Cheng, S., Wang, X., Wang, S., & Hovakimyan, N. (2024). "DiffTune-MPC: Closed-loop learning for model predictive control," in *IEEE Robotics and Automation Letters*.
7. Hsu, C. D., & Chaudhari, P. (2024). "Active Scout: Multi-Target Tracking Using Neural Radiance Fields in Dense Urban Environments." submitted to *arXiv preprint arXiv:2406.07431*.
8. Sanghvi, H., Folk, S., & Taylor, C. J. "OCCAM: Online Continuous Controller Adaptation with Meta-Learned Models," in *8th Annual Conference on Robot Learning (CoRL)*.
RotorPy was also listed among other UAV simulators in two recent surveys:
Dimmig, C. A., Silano, G., McGuire, K., Gabellieri, C., Hšnig, W., Moore, J., & Kobilarov, M. (2024). "Survey of Simulators for Aerial Robots: An Overview and In-Depth Systematic Comparisons," in *IEEE Robotics & Automation Magazine*.
Nikolaiev, M., & Novotarskyi, M. (2024). "Comparative Review of Drone Simulators," in *Information, Computing and Intelligent systems*, (4), 79-98.
**If you use this simulator for published work, let me know as I am happy to add your reference to this list.**
# Acknowledgements
We would like to acknowledge [Jimmy Paulos](https://github.com/jpaulos) who wrote the majority of the underlying engine for RotorPy, and the teaching assistants who contributed code to the initial version of this simulator, especially [Dan Mox](https://github.com/danmox), [Laura Jarin-Lipschitz](https://github.com/ljarin), [Rebecca Li](https://github.com/rebeccali), [Shane Rozen-Levy](https://github.com/ShaneRozenLevy), [Xu Liu](https://github.com/XuRobotics), [Yuezhan Tao](https://github.com/tyuezhan), [Yu-Ming Chen](https://github.com/yminchen), and [Fernando Cladera](https://github.com/fcladera).

2
examples/gymnasium_basic_usage.py
View File

@@ -48,7 +48,7 @@ env = gym.make("Quadrotor-v0",
# Now reset the quadrotor.
# Setting initial_state to 'random' will randomly place the vehicle in the map near the origin.
# But you can also set the environment resetting to be deterministic.
observation, info = env.reset(initial_state='random')
observation, info = env.reset(options={'initial_state': 'random'})
# Number of timesteps
T = 300

177
examples/multimav_multiprocessing_demo.py Normal file
View File

@@ -0,0 +1,177 @@
"""
Imports
"""
from rotorpy.vehicles.multirotor import Multirotor
from rotorpy.vehicles.crazyflie_params import quad_params
from rotorpy.controllers.quadrotor_control import SE3Control
from rotorpy.trajectories.hover_traj import HoverTraj
from rotorpy.trajectories.circular_traj import CircularTraj, ThreeDCircularTraj
from rotorpy.trajectories.lissajous_traj import TwoDLissajous
from rotorpy.trajectories.speed_traj import ConstantSpeed
from rotorpy.trajectories.minsnap import MinSnap
from rotorpy.world import World
from rotorpy.utils.animate import animate
from rotorpy.simulate import merge_dicts
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from scipy.spatial.transform import Rotation
import os
import yaml
import multiprocessing
####################### Helper functions
def run_sim(trajectory, t_offset, t_final=10, t_step=1/100):
"""
Runs an instance of the simulation environment which creates a vehicle object and tracking controller on
an individual cpu process using Python's multiprocessing.
Inputs:
trajectory: the trajectory object for this mav to track.
t_offset: time offset (useful for offsetting multiple mavs on the same trajectory).
t_final: duration of the sim for this object.
t_step: timestep for the simulation.
Outputs:
time: time array.
states: array of quadrotor states.
controls: array of quadrotor control variables.
flats: array of flat outputs describing the trajectory to track.
"""
mav = Multirotor(quad_params)
controller = SE3Control(quad_params)
# Init mav at the first waypoint for the trajectory.
x0 = {'x': trajectory.update(t_offset)['x'],
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
time = [0]
states = [x0]
flats = [trajectory.update(time[-1] + t_offset)]
controls = [controller.update(time[-1], states[-1], flats[-1])]
while True:
if time[-1] >= t_final:
break
time.append(time[-1] + t_step)
states.append(mav.step(states[-1], controls[-1], t_step))
flats.append(trajectory.update(time[-1] + t_offset))
controls.append(controller.update(time[-1], states[-1], flats[-1]))
time = np.array(time, dtype=float)
states = merge_dicts(states)
controls = merge_dicts(controls)
flats = merge_dicts(flats)
return time, states, controls, flats
def worker_fn(cfg):
"""
Enumerates over the configurations for each process in multiprocessing.
"""
return run_sim(*cfg)
def find_collisions(all_positions, epsilon=1e-1):
"""
Checks if any two agents get within epsilon meters of any other agent.
Inputs:
all_positions: the position vs time for each agent concatenated into one array.
epsilon: the distance threshold constituting a collision.
Outputs:
collisions: a list of dictionaries where each dict describes the time of a collision, agents involved, and the location.
"""
N, M, _ = all_positions.shape
collisions = []
for t in range(N):
# Get positions.
pos_t = all_positions[t]
dist_sq = np.sum((pos_t[:, np.newaxis, :] - pos_t[np.newaxis, :, :])**2, axis=-1)
# Set diagonal to a large value to avoid false positives.
np.fill_diagonal(dist_sq, np.inf)
close_pairs = np.where(dist_sq < epsilon**2)
for i, j in zip(*close_pairs):
if i < j: # avoid duplicate pairs.
collision_info = {
"timestep": t,
"agents": (i, j),
"location": pos_t[i]
}
collisions.append(collision_info)
return collisions
####################### Start of user code
# Construct the world.
world = World.empty([-3, 3, -3, 3, -3, 3])
# Generate a list of configurations to run in parallel. Each config has a trajectory, time offset, sim duration, and sim time discretization.
dt = 1/100
tf = 10
# Hard coded list of Lissajous maneuvers.
config_list = [(TwoDLissajous(A=1, B=1, a=2, b=1, x_offset=-0.5, y_offset=0, height=2.0), 0, tf, dt),
(TwoDLissajous(A=1, B=1, a=2, b=1, x_offset=-0.25, y_offset=0, height=2.0), 0.5, tf, dt),
(TwoDLissajous(A=1, B=1, a=2, b=1, x_offset=0.0, y_offset=0, height=2.0), 1.0, tf, dt),
(TwoDLissajous(A=1, B=1, a=2, b=1, x_offset=0.25, y_offset=0, height=2.0), 1.5, tf, dt),
(TwoDLissajous(A=1, B=1, a=2, b=1, x_offset=0.50, y_offset=0, height=2.0), 2.0, tf, dt)]
# Programmatic construction of a swarm of MAVs following a MinSnap trajectory.
Nc = 7
R = 0.5
for i in range(Nc):
x0 = np.array([-2 + R*np.cos(i*2*np.pi/Nc), R*np.sin(i*2*np.pi/Nc), 0])
xf = np.array([ 2 + R*np.cos(i*2*np.pi/Nc), R*np.sin(i*2*np.pi/Nc), 0])
config_list.append((MinSnap(points=np.row_stack((x0, xf)), v_avg=1.0, verbose=False), 0, tf, dt))
# Run RotorPy in parallel.
with multiprocessing.Pool() as pool:
results = pool.map(worker_fn, config_list)
# Concatentate all the relevant states/inputs for animation.
all_pos = []
all_rot = []
all_wind = []
all_time = results[0][0]
for r in results:
all_pos.append(r[1]['x'])
all_wind.append(r[1]['wind'])
all_rot.append(Rotation.from_quat(r[1]['q']).as_matrix())
all_pos = np.stack(all_pos, axis=1)
all_wind = np.stack(all_wind, axis=1)
all_rot = np.stack(all_rot, axis=1)
# Check for collisions.
collisions = find_collisions(all_pos, epsilon=2e-1)
# Animate.
ani = animate(all_time, all_pos, all_rot, all_wind, animate_wind=False, world=world, filename=None)
# Plot the positions of each agent in 3D, alongside collision events (when applicable)
fig = plt.figure()
ax = fig.add_subplot(projection='3d')
colors = plt.cm.tab10(range(all_pos.shape[1]))
for mav in range(all_pos.shape[1]):
ax.plot(all_pos[:, mav, 0], all_pos[:, mav, 1], all_pos[:, mav, 2], color=colors[mav])
ax.plot([all_pos[-1, mav, 0]], [all_pos[-1, mav, 1]], [all_pos[-1, mav, 2]], '*', markersize=10, markerfacecolor=colors[mav], markeredgecolor='k')
world.draw(ax)
for event in collisions:
ax.plot([all_pos[event['timestep'], event['agents'][0], 0]], [all_pos[event['timestep'], event['agents'][0], 1]], [all_pos[event['timestep'], event['agents'][0], 2]], 'rx', markersize=10)
ax.set_xlabel("x, m")
ax.set_ylabel("y, m")
ax.set_zlabel("z, m")
plt.show()

2
examples/ppo_hover_train.py
View File

@@ -59,7 +59,7 @@ env = gym.make("Quadrotor-v0",
# check_env(env, warn=True) # you can check the environment using built-in tools
# Reset the environment
observation, info = env.reset(initial_state='random', options={'pos_bound': 2, 'vel_bound': 0})
observation, info = env.reset(options={'initial_state': 'random', 'pos_bound': 2, 'vel_bound': 0})
# Create a new model
model = PPO(MlpPolicy, env, verbose=1, ent_coef=0.01, tensorboard_log=log_dir)

156
path_finding/graph_search.py Normal file
View File

@@ -0,0 +1,156 @@
from heapq import heappush, heappop # Recommended.
import numpy as np
from rotorpy.world import World
from path_finding.occupancy_map import OccupancyMap # Recommended.
def get_neighbors(index, occ_map):
x, y, z = index
neighbors = []
costs = []
for dx in [-1, 0, 1]:
for dy in [-1, 0, 1]:
for dz in [-1, 0, 1]:
# Skip the center point (0, 0, 0)
if dx == 0 and dy == 0 and dz == 0:
continue
nx, ny, nz = x + dx, y + dy, z + dz
# Check if the node is not occupied
if not occ_map.is_occupied_index((nx, ny, nz)):
neighbors.append((nx, ny, nz))
changes = abs(dx)+abs(dy)+abs(dz)
if changes == 1:
cost = 1
elif changes ==2:
cost = 1.41
else:
cost = 1.73
costs.append(cost)
return neighbors, costs
def dijkstra(start, goal, start_index, goal_index, occ_map):
frontier = []
heappush(frontier,(0,start_index))
previous_nodes = {start_index:None}
cost = {start_index:0}
nodes_expanded = 0
while frontier:
current = heappop(frontier)[1]
nodes_expanded += 1
if current == goal_index:
break
next_nodes, next_costs = get_neighbors(current, occ_map)
for next in next_nodes:
new_cost = cost[current]+next_costs[next_nodes.index(next)]
if next not in cost or new_cost < cost[next]:
cost[next] = new_cost
heappush(frontier,(new_cost, next))
previous_nodes[next] = current
if goal_index not in previous_nodes:
return None, nodes_expanded
path = []
current = goal_index
while current != start_index:
path.append(occ_map.index_to_metric_center(current))
current = previous_nodes[current]
path.append(start)
path.reverse()
path[-1] = goal
return np.array(path), nodes_expanded
def heuristic(next, goal):
euclidean_distance = np.sqrt((next[0] - goal[0])**2 + (next[1] - goal[1])**2 + (next[2] - goal[2])**2)
manhattan_distance = (goal[0]-next[0])+(goal[1]-next[1])+(goal[2]-next[2])
return euclidean_distance
def astar_algorithm(start, goal, start_index, goal_index, occ_map):
frontier = []
heappush(frontier,(0,start_index))
previous_nodes = {start_index:None}
cost = {start_index:0}
nodes_expanded = 0
while frontier:
current = heappop(frontier)[1]
nodes_expanded += 1
if current == goal_index:
break
next_nodes, next_costs = get_neighbors(current, occ_map)
for next in next_nodes:
new_cost = cost[current]+next_costs[next_nodes.index(next)]
if next not in cost or new_cost < cost[next]:
cost[next] = new_cost
total_cost = new_cost + heuristic(next, goal_index)
heappush(frontier,(total_cost, next))
previous_nodes[next] = current
if goal_index not in previous_nodes:
return None, nodes_expanded
path = []
current = goal_index
while current != start_index:
path.append(occ_map.index_to_metric_center(current))
current = previous_nodes[current]
path.append(start)
path.reverse()
path[-1] = goal
return np.array(path), nodes_expanded
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
return a tuple (path, nodes_expanded)
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
nodes_expanded, the number of nodes that have been expanded
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
# MY CODE STARTS HERE
if astar == False:
return dijkstra(start, goal, start_index, goal_index, occ_map)
if astar == True:
return astar_algorithm(start, goal, start_index, goal_index, occ_map)

222
path_finding/occupancy_map.py Normal file
View File

@@ -0,0 +1,222 @@
import heapq
import numpy as np
from scipy.spatial import Rectangle
from scipy.spatial.transform import Rotation
from rotorpy.world import World
from rotorpy.utils import shapes
class OccupancyMap:
def __init__(self, world=World.empty((0, 2, 0, 2, 0, 2)), resolution=(.1, .1, .1), margin=.2):
"""
This class creates a 3D voxel occupancy map of the configuration space from a flightsim World object.
Parameters:
world, a flightsim World object
resolution, the discretization of the occupancy grid in x,y,z
margin, the inflation radius used to create the configuration space (assuming a spherical drone)
"""
self.world = world
self.resolution = np.array(resolution)
self.margin = margin
self._init_map_from_world()
def index_to_metric_negative_corner(self, index):
"""
Return the metric position of the most negative corner of a voxel, given its index in the occupancy grid
"""
return index*np.array(self.resolution) + self.origin
def index_to_metric_center(self, index):
"""
Return the metric position of the center of a voxel, given its index in the occupancy grid
"""
return self.index_to_metric_negative_corner(index) + self.resolution/2.0
def metric_to_index(self, metric):
"""
Returns the index of the voxel containing a metric point.
Remember that this and index_to_metric and not inverses of each other!
If the metric point lies on a voxel boundary along some coordinate,
the returned index is the lesser index.
"""
return np.floor((metric - self.origin)/self.resolution).astype('int')
def _metric_block_to_index_range(self, bounds, outer_bound=True):
"""
A fast test that returns the closed index range intervals of voxels
intercepting a rectangular bound. If outer_bound is true the returned
index range is conservatively large, if outer_bound is false the index
range is conservatively small.
"""
# Implementation note: The original intended resolution may not be
# exactly representable as a floating point number. For example, the
# floating point value for "0.1" is actually bigger than 0.1. This can
# cause surprising results on large maps. The solution used here is to
# slightly inflate or deflate the resolution by the smallest
# representative unit to achieve either an upper or lower bound result.
sign = 1 if outer_bound else -1
min_index_res = np.nextafter(self.resolution, sign * np.inf) # Use for lower corner.
max_index_res = np.nextafter(self.resolution, -sign * np.inf) # Use for upper corner.
bounds = np.asarray(bounds)
# Find minimum included index range.
min_corner = bounds[0::2]
min_frac_index = (min_corner - self.origin)/min_index_res
min_index = np.floor(min_frac_index).astype('int')
min_index[min_index == min_frac_index] -= 1
min_index = np.maximum(0, min_index)
# Find maximum included index range.
max_corner = bounds[1::2]
max_frac_index = (max_corner - self.origin)/max_index_res
max_index = np.floor(max_frac_index).astype('int')
max_index = np.minimum(max_index, np.asarray(self.map.shape)-1)
return (min_index, max_index)
def _init_map_from_world(self):
"""
Creates the occupancy grid (self.map) as a boolean numpy array. True is
occupied, False is unoccupied. This function is called during
initialization of the object.
"""
# Initialize the occupancy map, marking all free.
bounds = self.world.world['bounds']['extents']
voxel_dimensions_metric = []
voxel_dimensions_indices = []
for i in range(3):
voxel_dimensions_metric.append(abs(bounds[1+i*2]-bounds[i*2]))
voxel_dimensions_indices.append(int(np.ceil(voxel_dimensions_metric[i]/self.resolution[i])))
self.map = np.zeros(voxel_dimensions_indices, dtype=bool)
self.origin = np.array(bounds[0::2])
# Iterate through each block obstacle.
for block in self.world.world.get('blocks', []):
extent = block['extents']
block_rect = Rectangle([extent[1], extent[3], extent[5]], [extent[0], extent[2], extent[4]])
# Get index range that is definitely occupied by this block.
(inner_min_index, inner_max_index) = self._metric_block_to_index_range(extent, outer_bound=False)
a, b = inner_min_index, inner_max_index
self.map[a[0]:(b[0]+1), a[1]:(b[1]+1), a[2]:(b[2]+1)] = True
# Get index range that is definitely not occupied by this block.
outer_extent = extent + self.margin * np.array([-1, 1, -1, 1, -1, 1])
(outer_min_index, outer_max_index) = self._metric_block_to_index_range(outer_extent, outer_bound=True)
# Iterate over uncertain voxels with rect-rect distance check.
for i in range(outer_min_index[0], outer_max_index[0]+1):
for j in range(outer_min_index[1], outer_max_index[1]+1):
for k in range(outer_min_index[2], outer_max_index[2]+1):
# If map is not already occupied, check for collision.
if not self.map[i,j,k]:
metric_loc = self.index_to_metric_negative_corner((i,j,k))
voxel_rect = Rectangle(metric_loc+self.resolution, metric_loc)
rect_distance = voxel_rect.min_distance_rectangle(block_rect)
self.map[i,j,k] = rect_distance <= self.margin
def draw_filled(self, ax):
"""
Visualize the occupancy grid (mostly for debugging)
Warning: may be slow with O(10^3) occupied voxels or more
Parameters:
ax, an Axes3D object
"""
self.world.draw_empty_world(ax)
it = np.nditer(self.map, flags=['multi_index'])
while not it.finished:
if self.map[it.multi_index] == True:
metric_loc = self.index_to_metric_negative_corner(it.multi_index)
xmin, ymin, zmin = metric_loc
xmax, ymax, zmax = metric_loc + self.resolution
c = shapes.Cuboid(ax, xmax-xmin, ymax-ymin, zmax-zmin, alpha=0.1, linewidth=1, edgecolors='k', facecolors='b')
c.transform(position=(xmin, ymin, zmin))
it.iternext()
def _draw_voxel_face(self, ax, index, direction):
# Normalized coordinates of the top face.
face = np.array([(1,1,1), (-1,1,1), (-1,-1,1), (1,-1,1)])
# Rotate to find normalized coordinates of target face.
if direction[0] != 0:
axis = np.array([0, 1, 0]) * np.pi/2 * direction[0]
elif direction[1] != 0:
axis = np.array([-1, 0, 0]) * np.pi/2 * direction[1]
elif direction[2] != 0:
axis = np.array([1, 0, 0]) * np.pi/2 * (1-direction[2])
face = (Rotation.from_rotvec(axis).as_matrix() @ face.T).T
# Scale, position, and draw using Face object.
face = 0.5 * face * np.reshape(self.resolution, (1,3))
f = shapes.Face(ax, face, alpha=0.1, linewidth=1, edgecolors='k', facecolors='b')
f.transform(position=(self.index_to_metric_center(index)))
def draw_shell(self, ax):
self.world.draw_empty_world(ax)
it = np.nditer(self.map, flags=['multi_index'])
while not it.finished:
idx = it.multi_index
if self.map[idx] == True:
for d in [(0,0,-1), (0,0,1), (0,-1,0), (0,1,0), (-1,0,0), (1,0,0)]:
neigh_idx = (idx[0]+d[0], idx[1]+d[1], idx[2]+d[2])
neigh_exists = self.is_valid_index(neigh_idx)
if not neigh_exists or (neigh_exists and not self.map[neigh_idx]):
self._draw_voxel_face(ax, idx, d)
it.iternext()
def draw(self, ax):
self.draw_shell(ax)
def is_valid_index(self, voxel_index):
"""
Test if a voxel index is within the map.
Returns True if it is inside the map, False otherwise.
"""
for i in range(3):
if voxel_index[i] >= self.map.shape[i] or voxel_index[i] < 0:
return False
return True
def is_valid_metric(self, metric):
"""
Test if a metric point is within the world.
Returns True if it is inside the world, False otherwise.
"""
bounds = self.world.world['bounds']['extents']
for i in range(3):
if metric[i] <= bounds[i*2] or metric[i] >= bounds[i*2+1]:
return False
return True
def is_occupied_index(self, voxel_index):
"""
Test if a voxel index is occupied.
Returns True if occupied or outside the map, False otherwise.
"""
return (not self.is_valid_index(voxel_index)) or self.map[tuple(voxel_index)]
def is_occupied_metric(self, voxel_metric):
"""
Test if a metric point is within an occupied voxel.
Returns True if occupied or outside the map, False otherwise.
"""
ind = self.metric_to_index(voxel_metric)
return (not self.is_valid_index(ind)) or self.is_occupied_index(ind)
if __name__ == "__main__":
from flightsim.axes3ds import Axes3Ds
import matplotlib.pyplot as plt
# Create a world object first
world = World.random_forest(world_dims=(5, 5, 5), tree_width=.1, tree_height=5, num_trees=10)
# Create a figure
fig = plt.figure()
ax = Axes3Ds(fig)
# Draw the world
world.draw(ax)
# Create an Occupancy map
oc = OccupancyMap(world, (.2, .2, .5), .1)
# Draw the occupancy map (may be slow for many voxels; will look weird if plotted on top of a world.draw)
oc.draw(ax)
plt.show()

51
pyproject.toml Normal file
View File

@@ -0,0 +1,51 @@
[project]
name = "rotorpy"
version = "1.1.1"
description = "A multirotor simulator with aerodynamics for education and research."
readme = "README.md"
requires-python = ">=3.8"
license = { file = "LICENSE.md" }
keywords = ["drone", "uav", "quadrotor", "multirotor", "aerodynamics", "simulation", "controls", "robotics", "estimation"] # Optional
authors = [
{ name = "Spencer Folk", email = "sfolk@seas.upenn.edu" }
]
maintainers = [
{ name = "Spencer Folk", email = "sfolk@seas.upenn.edu" }
]
classifiers = [
"Development Status :: 4 - Beta",
"Intended Audience :: Developers",
"License :: OSI Approved :: MIT License",
"Operating System :: OS Independent",
"Programming Language :: Python :: 3",
"Programming Language :: Python :: 3 :: Only",
]
dependencies = [
'cvxopt',
'matplotlib',
'filterpy == 1.4.5',
'numpy',
'scipy',
'pandas',
'tqdm',
'gymnasium',
]
[project.optional-dependencies]
learning = [
'stable_baselines3'
]
[project.urls]
"Homepage" = "https://github.com/spencerfolk/rotorpy"
"Bug Reports" = "https://github.com/spencerfolk/rotorpy/issues"
"Source" = "https://github.com/spencerfolk/rotorpy"
[build-system]
requires = ["setuptools>=44.0.0", "wheel"]
build-backend = "setuptools.build_meta"
[tool.setuptools.packages.find]
include = ["rotorpy", "rotorpy.*"]
[tool.setuptools]

2
rotorpy/controllers/quadrotor_control.py
View File

@@ -62,7 +62,7 @@ class SE3Control(object):
k = self.k_m/self.k_eta # Ratio of torque to thrust coefficient.
# Below is an automated generation of the control allocator matrix. It assumes that all thrust vectors are aligned
# with the z axis and that the "sign" of each rotor yaw moment alternates starting with positive for r1. 'TM' = "thrust and moments"
# with the z axis.
self.f_to_TM = np.vstack((np.ones((1,self.num_rotors)),
np.hstack([np.cross(self.rotor_pos[key],np.array([0,0,1])).reshape(-1,1)[0:2] for key in self.rotor_pos]),
(k * self.rotor_dir).reshape(1,-1)))

54
rotorpy/learning/quadrotor_environments.py
View File

@@ -70,9 +70,9 @@ class QuadrotorEnv(gym.Env):
self.metadata['render_fps'] = render_fps
self.initial_state = initial_state
self.initial_state = deepcopy(initial_state)
self.vehicle_state = initial_state
self.vehicle_state = deepcopy(initial_state)
assert control_mode in self.metadata["control_modes"] # Don't accept improper control modes
self.control_mode = control_mode
@@ -184,34 +184,44 @@ class QuadrotorEnv(gym.Env):
# Close the plots
plt.close('all')
def reset(self, seed=None, initial_state='random', options={'pos_bound': 2, 'vel_bound': 0}):
def reset(self, seed=None, options={'initial_state': 'random', 'pos_bound': 2, 'vel_bound': 0}):
"""
Reset the environment
Inputs:
seed: the seed for any random number generation, mostly for reproducibility.
initial_state: determines how to set the quadrotor again. Options are...
'random': will randomly select the state of the quadrotor.
'deterministic': will set the state to the initial state selected by the user when creating
the quadrotor environment (usually hover).
the user can also specify the state itself as a dictionary... e.g.
reset(options={'initial_state':
{'x': np.array([0,0,0]),
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
})
seed: the seed for any random number generation, mostly for reproducibility.
options: dictionary for misc options for resetting the scene.
'initial_state': determines how to set the quadrotor again. Options are...
'random': will randomly select the state of the quadrotor.
'deterministic': will set the state to the initial state selected by the user when creating
the quadrotor environment (usually hover).
the user can also specify the state itself as a dictionary... e.g.
reset(options={'initial_state':
{'x': np.array([0,0,0]),
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
})
'pos_bound': the min/max position region for random placement.
'vel_bound': the min/max velocity region for random placement
"""
# If any options are not specified, set them to default values.
if 'pos_bound' not in options:
options['pos_bound'] = 2
if 'vel_bound' not in options:
options['vel_bound'] = 0
if 'initial_state' not in options:
options['initial_state'] = 'random'
# Assert that the bounds are greater than or equal to 0.
assert options['pos_bound'] >= 0 and options['vel_bound'] >= 0 , "Bounds must be greater than or equal to 0."
# Reset the gym environment
super().reset(seed=seed)
if initial_state == 'random':
if options['initial_state'] == 'random':
# Randomly select an initial state for the quadrotor. At least assume it is level.
pos = np.random.uniform(low=-options['pos_bound'], high=options['pos_bound'], size=(3,))
vel = np.random.uniform(low=-options['vel_bound'], high=options['vel_bound'], size=(3,))
@@ -222,13 +232,13 @@ class QuadrotorEnv(gym.Env):
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
elif initial_state == 'deterministic':
elif options['initial_state'] == 'deterministic':
state = self.initial_state
elif isinstance(initial_state, dict):
elif isinstance(options['initial_state'], dict):
# Ensure the correct keys are in dict.
if all(key in initial_state for key in ('x', 'v', 'q', 'w', 'wind', 'rotor_speeds')):
state = initial_state
if all(key in options['initial_state'] for key in ('x', 'v', 'q', 'w', 'wind', 'rotor_speeds')):
state = options['initial_state']
else:
raise KeyError("Missing state keys in your initial_state. You must specify values for ('x', 'v', 'q', 'w', 'wind', 'rotor_speeds')")

11
rotorpy/sensors/range_sensors.py
View File

@@ -124,6 +124,17 @@ class TwoDRangeSensor():
distances = np.ones((self.N_rays,))*1e5 # Begin with
for object_edges in self.world_edges:
# Object edges is a list of tuples, each tuple describing one of the four edges
# Check if the object is within the field of view of the sensor
xmin = object_edges[0][0]
xmax = object_edges[1][0]
ymin = object_edges[0][1]
ymax = object_edges[2][1]
if (xmin > x + self.Dmax) or (xmax < x - self.Dmax) or (ymin > y + self.Dmax) or (ymax < y - self.Dmax):
# Don't check if the object is outside the field of view
continue
for edge in object_edges:
# First collect the coordinates for point 1 and 2 corresponding to this edge
x1 = edge[0]

6
rotorpy/trajectories/hover_traj.py
View File

@@ -6,12 +6,14 @@ class HoverTraj(object):
By modifying the initial condition, you can create step response
experiments.
"""
def __init__(self):
def __init__(self, x0=np.array([0, 0, 0])):
"""
This is the constructor for the Trajectory object. A fresh trajectory
object will be constructed before each mission.
"""
self.x0 = x0
def update(self, t):
"""
Given the present time, return the desired flat output and derivatives.
@@ -28,7 +30,7 @@ class HoverTraj(object):
yaw, yaw angle, rad
yaw_dot, yaw rate, rad/s
"""
x = np.zeros((3,))
x = self.x0
x_dot = np.zeros((3,))
x_ddot = np.zeros((3,))
x_dddot = np.zeros((3,))

10
rotorpy/trajectories/lissajous_traj.py
View File

@@ -9,7 +9,7 @@ class TwoDLissajous(object):
The standard Lissajous on the XY curve as defined by https://en.wikipedia.org/wiki/Lissajous_curve
This is planar in the XY plane at a fixed height.
"""
def __init__(self, A=1, B=1, a=1, b=1, delta=0, height=0, yaw_bool=False):
def __init__(self, A=1, B=1, a=1, b=1, delta=0, x_offset=0, y_offset=0, height=0, yaw_bool=False):
"""
This is the constructor for the Trajectory object. A fresh trajectory
object will be constructed before each mission.
@@ -20,6 +20,8 @@ class TwoDLissajous(object):
a := frequency on the X axis
b := frequency on the Y axis
delta := phase offset between the x and y parameterization
x_offset := the offset of the trajectory in the x axis
y_offset := the offset of the trajectory in the y axis
height := the z height that the lissajous occurs at
yaw_bool := determines whether the vehicle should yaw
"""
@@ -28,6 +30,8 @@ class TwoDLissajous(object):
self.a, self.b = a, b
self.delta = delta
self.height = height
self.x_offset = x_offset
self.y_offset = y_offset
self.yaw_bool = yaw_bool
@@ -47,8 +51,8 @@ class TwoDLissajous(object):
yaw, yaw angle, rad
yaw_dot, yaw rate, rad/s
"""
x = np.array([self.A*np.sin(self.a*t + self.delta),
self.B*np.sin(self.b*t),
x = np.array([self.x_offset + self.A*np.sin(self.a*t + self.delta),
self.y_offset + self.B*np.sin(self.b*t),
self.height])
x_dot = np.array([self.a*self.A*np.cos(self.a*t + self.delta),
self.b*self.B*np.cos(self.b*t),

3
rotorpy/vehicles/multirotor.py
View File

@@ -118,7 +118,7 @@ class Multirotor(object):
k = self.k_m/self.k_eta # Ratio of torque to thrust coefficient.
# Below is an automated generation of the control allocator matrix. It assumes that all thrust vectors are aligned
# with the z axis and that the "sign" of each rotor yaw moment alternates starting with positive for r1.
# with the z axis.
self.f_to_TM = np.vstack((np.ones((1,self.num_rotors)),np.hstack([np.cross(self.rotor_pos[key],np.array([0,0,1])).reshape(-1,1)[0:2] for key in self.rotor_pos]), (k * self.rotor_dir).reshape(1,-1)))
self.TM_to_f = np.linalg.inv(self.f_to_TM)
@@ -200,6 +200,7 @@ class Multirotor(object):
# Add noise to the motor speed measurement
state['rotor_speeds'] += np.random.normal(scale=np.abs(self.motor_noise), size=(self.num_rotors,))
state['rotor_speeds'] = np.clip(state['rotor_speeds'], self.rotor_speed_min, self.rotor_speed_max)
return state

16
rotorpy/world.py
View File

@@ -158,7 +158,7 @@ class World(object):
c.transform(position=(xmin, ymin, zmin))
return list(c.artists)
def draw(self, ax, facecolor=None, edgecolor=None, alpha=0.6):
def draw(self, ax, alpha=None, edgecolor=None, facecolor=None):
"""
Draw world onto existing Axes3D axes and return artists corresponding to the
blocks.
@@ -174,14 +174,20 @@ class World(object):
"""
bounds_artists = self.draw_empty_world(ax)
if alpha is None:
alpha = 0.7
if edgecolor is None:
edgecolor = 'k'
block_artists = []
for b in self.world.get('blocks', []):
(xmin, xmax, ymin, ymax, zmin, zmax) = b['extents']
if facecolor is None:
facecolor = b.get('color', None)
if edgecolor is None:
edgecolor = 'k'
c = Cuboid(ax, xmax-xmin, ymax-ymin, zmax-zmin, alpha=alpha, linewidth=1, edgecolors=edgecolor, facecolors=facecolor)
fc = b.get('color', None)
else:
fc = facecolor
c = Cuboid(ax, xmax-xmin, ymax-ymin, zmax-zmin, alpha=alpha, linewidth=1, edgecolors=edgecolor, facecolors=fc)
c.transform(position=(xmin, ymin, zmin))
block_artists.extend(c.artists)
return bounds_artists + block_artists

15
rotorpy/worlds/window.json Normal file
View File

@@ -0,0 +1,15 @@
{
"bounds": {"extents": [0.0, 10.0, -5.0, 20.0, 0.0, 6.0]},
"blocks": [
{"extents": [0.0, 10.0, 2.0, 2.5, 0.0, 1.5]},
{"extents": [0.0, 10.0, 2.0, 2.5, 4.5, 6.0]},
{"extents": [0.0, 3.0, 2.0, 2.5, 1.5, 4.5]},
{"extents": [7.0, 10.0, 2.0, 2.5, 1.5, 4.5]},
{"extents": [3.0, 7.0, 0.0, 0.5, 2.4, 4.5]},
{"extents": [0.0, 10.0, 15.0, 20.0, 0.0, 1.0]},
{"extents": [0.0, 10.0, 15.0, 16.0, 1.0, 3.5]},
{"extents": [0.0, 10.0, 18.0, 19.0, 4.5, 6.0]}
],
"start": [0.7, -4.3, 0.7],
"goal": [8.0, 18.0, 3.0]
}

130
sandbox/sandbox_graph_search.py Normal file
View File

@@ -0,0 +1,130 @@
"""
Imports
"""
# The simulator is instantiated using the Environment class
from rotorpy.environments import Environment
# Vehicles. Currently there is only one.
# There must also be a corresponding parameter file.
from rotorpy.vehicles.multirotor import Multirotor
from rotorpy.vehicles.crazyflie_params import quad_params
# from rotorpy.vehicles.hummingbird_params import quad_params # There's also the Hummingbird
# You will also need a controller (currently there is only one) that works for your vehicle.
from rotorpy.controllers.quadrotor_control import SE3Control
# And a trajectory generator
from rotorpy.trajectories.hover_traj import HoverTraj
from rotorpy.trajectories.circular_traj import CircularTraj, ThreeDCircularTraj
from rotorpy.trajectories.lissajous_traj import TwoDLissajous
from rotorpy.trajectories.speed_traj import ConstantSpeed
from rotorpy.trajectories.minsnap import MinSnap
# You can optionally specify a wind generator, although if no wind is specified it will default to NoWind().
from rotorpy.wind.default_winds import NoWind, ConstantWind, SinusoidWind, LadderWind
from rotorpy.wind.dryden_winds import DrydenGust, DrydenGustLP
from rotorpy.wind.spatial_winds import WindTunnel
# You can also optionally customize the IMU and motion capture sensor models. If not specified, the default parameters will be used.
from rotorpy.sensors.imu import Imu
from rotorpy.sensors.external_mocap import MotionCapture
# You can also specify a state estimator. This is optional. If no state estimator is supplied it will default to null.
from rotorpy.estimators.wind_ukf import WindUKF
# Also, worlds are how we construct obstacles. The following class contains methods related to constructing these maps.
from rotorpy.world import World
# Reference the files above for more documentation.
# Other useful imports
import numpy as np # For array creation/manipulation
import matplotlib.pyplot as plt # For plotting, although the simulator has a built in plotter
from scipy.spatial.transform import Rotation # For doing conversions between different rotation descriptions, applying rotations, etc.
import os # For path generation
# Other Imports
import sys
sys.path.append('/Users/oushiha/Documents/quadcopter_learning')
from path_finding.graph_search import graph_search
from path_finding.occupancy_map import OccupancyMap
"""
Instantiation
"""
# Obstacle maps can be loaded in from a JSON file using the World.from_file(path) method. Here we are loading in from
# an existing file under the rotorpy/worlds/ directory. However, you can create your own world by following the template
# provided (see rotorpy/worlds/README.md), and load that file anywhere using the appropriate path.
world = World.from_file(os.path.abspath(os.path.join(os.path.dirname(__file__),'..','rotorpy','worlds','window.json')))
# "world" is an optional argument. If you don't load a world it'll just provide an empty playground!
start = world.world['start']
goal = world.world['goal']
graph_search_results = np.array([0.25,0.25,0.25])
graph_search_margin = 0.5
points, nodes_expanded = graph_search(world, graph_search_results, graph_search_margin, start, goal, True)
# An instance of the simulator can be generated as follows:
sim_instance = Environment(vehicle=Multirotor(quad_params), # vehicle object, must be specified.
controller=SE3Control(quad_params), # controller object, must be specified.
trajectory=MinSnap(points, v_max=3, v_avg=2.5), # trajectory object, must be specified.
wind_profile=SinusoidWind(), # OPTIONAL: wind profile object, if none is supplied it will choose no wind.
sim_rate = 100, # OPTIONAL: The update frequency of the simulator in Hz. Default is 100 Hz.
imu = None, # OPTIONAL: imu sensor object, if none is supplied it will choose a default IMU sensor.
mocap = None, # OPTIONAL: mocap sensor object, if none is supplied it will choose a default mocap.
estimator = None, # OPTIONAL: estimator object
world = world, # OPTIONAL: the world, same name as the file in rotorpy/worlds/, default (None) is empty world
safety_margin= 0.25 # OPTIONAL: defines the radius (in meters) of the sphere used for collision checking
)
# This generates an Environment object that has a unique vehicle, controller, and trajectory.
# You can also optionally specify a wind profile, IMU object, motion capture sensor, estimator,
# and the simulation rate for the simulator.
"""
Execution
"""
# Setting an initial state. This is optional, and the state representation depends on the vehicle used.
# Generally, vehicle objects should have an "initial_state" attribute.
x0 = {'x': start,
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
sim_instance.vehicle.initial_state = x0
# Executing the simulator as specified above is easy using the "run" method:
# All the arguments are listed below with their descriptions.
# You can save the animation (if animating) using the fname argument. Default is None which won't save it.
results = sim_instance.run(t_final = 30, # The maximum duration of the environment in seconds
use_mocap = False, # Boolean: determines if the controller should use the motion capture estimates.
terminate = False, # Boolean: if this is true, the simulator will terminate when it reaches the last waypoint.
plot = True, # Boolean: plots the vehicle states and commands
plot_mocap = True, # Boolean: plots the motion capture pose and twist measurements
plot_estimator = True, # Boolean: plots the estimator filter states and covariance diagonal elements
plot_imu = True, # Boolean: plots the IMU measurements
animate_bool = True, # Boolean: determines if the animation of vehicle state will play.
animate_wind = True, # Boolean: determines if the animation will include a scaled wind vector to indicate the local wind acting on the UAV.
verbose = True, # Boolean: will print statistics regarding the simulation.
fname = None # Filename is specified if you want to save the animation. The save location is rotorpy/data_out/.
)
# There are booleans for if you want to plot all/some of the results, animate the multirotor, and
# if you want the simulator to output the EXIT status (end time reached, out of control, etc.)
# The results are a dictionary containing the relevant state, input, and measurements vs time.
# To save this data as a .csv file, you can use the environment's built in save method. You must provide a filename.
# The save location is rotorpy/data_out/
sim_instance.save_to_csv("basic_usage.csv")
# Instead of producing a CSV, you can manually unpack the dictionary into a Pandas DataFrame using the following:
from rotorpy.utils.postprocessing import unpack_sim_data
dataframe = unpack_sim_data(results)

115
sandbox/sandbox_se3_hover.py Normal file
View File

@@ -0,0 +1,115 @@
"""
Imports
"""
# The simulator is instantiated using the Environment class
from rotorpy.environments import Environment
# Vehicles. Currently there is only one.
# There must also be a corresponding parameter file.
from rotorpy.vehicles.multirotor import Multirotor
from rotorpy.vehicles.crazyflie_params import quad_params
# from rotorpy.vehicles.hummingbird_params import quad_params # There's also the Hummingbird
# You will also need a controller (currently there is only one) that works for your vehicle.
from rotorpy.controllers.quadrotor_control import SE3Control
# And a trajectory generator
from rotorpy.trajectories.hover_traj import HoverTraj
from rotorpy.trajectories.circular_traj import CircularTraj, ThreeDCircularTraj
from rotorpy.trajectories.lissajous_traj import TwoDLissajous
from rotorpy.trajectories.speed_traj import ConstantSpeed
from rotorpy.trajectories.minsnap import MinSnap
# You can optionally specify a wind generator, although if no wind is specified it will default to NoWind().
from rotorpy.wind.default_winds import NoWind, ConstantWind, SinusoidWind, LadderWind
from rotorpy.wind.dryden_winds import DrydenGust, DrydenGustLP
from rotorpy.wind.spatial_winds import WindTunnel
# You can also optionally customize the IMU and motion capture sensor models. If not specified, the default parameters will be used.
from rotorpy.sensors.imu import Imu
from rotorpy.sensors.external_mocap import MotionCapture
# You can also specify a state estimator. This is optional. If no state estimator is supplied it will default to null.
from rotorpy.estimators.wind_ukf import WindUKF
# Also, worlds are how we construct obstacles. The following class contains methods related to constructing these maps.
from rotorpy.world import World
# Reference the files above for more documentation.
# Other useful imports
import numpy as np # For array creation/manipulation
import matplotlib.pyplot as plt # For plotting, although the simulator has a built in plotter
from scipy.spatial.transform import Rotation # For doing conversions between different rotation descriptions, applying rotations, etc.
import os # For path generation
"""
Instantiation
"""
# Obstacle maps can be loaded in from a JSON file using the World.from_file(path) method. Here we are loading in from
# an existing file under the rotorpy/worlds/ directory. However, you can create your own world by following the template
# provided (see rotorpy/worlds/README.md), and load that file anywhere using the appropriate path.
world = World.from_file(os.path.abspath(os.path.join(os.path.dirname(__file__),'..','rotorpy','worlds','double_pillar.json')))
# "world" is an optional argument. If you don't load a world it'll just provide an empty playground!
# An instance of the simulator can be generated as follows:
sim_instance = Environment(vehicle=Multirotor(quad_params), # vehicle object, must be specified.
controller=SE3Control(quad_params), # controller object, must be specified.
trajectory=HoverTraj(np.array([0,0,2])), # trajectory object, must be specified.
wind_profile=SinusoidWind(), # OPTIONAL: wind profile object, if none is supplied it will choose no wind.
sim_rate = 100, # OPTIONAL: The update frequency of the simulator in Hz. Default is 100 Hz.
imu = None, # OPTIONAL: imu sensor object, if none is supplied it will choose a default IMU sensor.
mocap = None, # OPTIONAL: mocap sensor object, if none is supplied it will choose a default mocap.
estimator = None, # OPTIONAL: estimator object
world = world, # OPTIONAL: the world, same name as the file in rotorpy/worlds/, default (None) is empty world
safety_margin= 0.25 # OPTIONAL: defines the radius (in meters) of the sphere used for collision checking
)
# This generates an Environment object that has a unique vehicle, controller, and trajectory.
# You can also optionally specify a wind profile, IMU object, motion capture sensor, estimator,
# and the simulation rate for the simulator.
"""
Execution
"""
# Setting an initial state. This is optional, and the state representation depends on the vehicle used.
# Generally, vehicle objects should have an "initial_state" attribute.
x0 = {'x': np.array([0,0,0]),
'v': np.zeros(3,),
'q': np.array([0, 0, 0, 1]), # [i,j,k,w]
'w': np.zeros(3,),
'wind': np.array([0,0,0]), # Since wind is handled elsewhere, this value is overwritten
'rotor_speeds': np.array([1788.53, 1788.53, 1788.53, 1788.53])}
sim_instance.vehicle.initial_state = x0
# Executing the simulator as specified above is easy using the "run" method:
# All the arguments are listed below with their descriptions.
# You can save the animation (if animating) using the fname argument. Default is None which won't save it.
results = sim_instance.run(t_final = 20, # The maximum duration of the environment in seconds
use_mocap = False, # Boolean: determines if the controller should use the motion capture estimates.
terminate = False, # Boolean: if this is true, the simulator will terminate when it reaches the last waypoint.
plot = True, # Boolean: plots the vehicle states and commands
plot_mocap = True, # Boolean: plots the motion capture pose and twist measurements
plot_estimator = True, # Boolean: plots the estimator filter states and covariance diagonal elements
plot_imu = True, # Boolean: plots the IMU measurements
animate_bool = True, # Boolean: determines if the animation of vehicle state will play.
animate_wind = True, # Boolean: determines if the animation will include a scaled wind vector to indicate the local wind acting on the UAV.
verbose = True, # Boolean: will print statistics regarding the simulation.
fname = None # Filename is specified if you want to save the animation. The save location is rotorpy/data_out/.
)
# There are booleans for if you want to plot all/some of the results, animate the multirotor, and
# if you want the simulator to output the EXIT status (end time reached, out of control, etc.)
# The results are a dictionary containing the relevant state, input, and measurements vs time.
# To save this data as a .csv file, you can use the environment's built in save method. You must provide a filename.
# The save location is rotorpy/data_out/
sim_instance.save_to_csv("basic_usage.csv")
# Instead of producing a CSV, you can manually unpack the dictionary into a Pandas DataFrame using the following:
from rotorpy.utils.postprocessing import unpack_sim_data
dataframe = unpack_sim_data(results)

23
setup.py
View File

@@ -1,23 +0,0 @@
from distutils.core import setup
from os.path import isdir
from itertools import product
# Gather our flightsim and any projXX packages that happen to exist.
all_packages = ['rotorpy']
packages = list(filter(isdir, all_packages))
setup(
name='rotorpy',
packages=packages,
version='1.1.0',
install_requires=[
'cvxopt',
'matplotlib',
'filterpy == 1.4.5',
'numpy',
'scipy',
'pandas',
'ndsplines',
'timeout_decorator',
'tqdm',
'gymnasium == 0.29.1'])